T2=transl(0.6,0.5,0.2);T3=transl(0.6,0.5,-0.2);T4=transl(0.6,-0.5,-0.2);T={T1,T2,T3,T4,T1};eval('puma560');for i=1:1,    for j=1:4,        start_point=T{j};        end_point=T{j+1};        T0=ctraj(start_point,end_point,36);        qianhe=findobj('Tag','qianhe');    if isempty(qianhe),        qianhe=figure('Name','PUMA560機器人仿真演示窗口（三維坐標）','Tag','qianhe');        figure(qianhe);        plot3(squeeze(T0(1,4,:)),squeeze(T0(2,4,:)),squeeze(T0(3,4,:)));        hold on;    else        hold on;        figure(qianhe);        plot3(squeeze(T0(1,4,:)),squeeze(T0(2,4,:)),squeeze(T0(3,4,:)));    end        q=ikine(p560,T0);        puma=findobj('Tag','puma560_gui');    if ~isempty(puma),        puma=guihandles(puma);        axes(puma.axes1);        cla;        plot(p560,q);    else        puma=guihandles(puma560_gui);        axes(puma.axes1);        cla;        plot(p560,q);    end    endend% --- Executes on slider movement.function slider_q1_Callback(hObject, eventdata, handles)% hObject    handle to slider_q1 (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    structure with handles and user data (see GUIDATA)% Hints: get(hObject,'Value') returns position of slider%        get(hObject,'Min') and get(hObject,'Max') to determine range of sliderval=get(hObject,'Value');set(handles.edit_q1,'String',num2str(val));drive_robot(handles);% --- Executes during object creation, after setting all properties.function slider_q1_CreateFcn(hObject, eventdata, handles)% hObject    handle to slider_q1 (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    empty - handles not created until after all CreateFcns called% Hint: slider controls usually have a light gray background.if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))    set(hObject,'BackgroundColor',[.9 .9 .9]);end% --- Executes on slider movement.function slider_q2_Callback(hObject, eventdata, handles)% hObject    handle to slider_q2 (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    structure with handles and user data (see GUIDATA)% Hints: get(hObject,'Value') returns position of slider%        get(hObject,'Min') and get(hObject,'Max') to determine range of sliderval=get(hObject,'Value');set(handles.edit_q2,'String',num2str(val));drive_robot(handles);% --- Executes during object creation, after setting all properties.function slider_q2_CreateFcn(hObject, eventdata, handles)% hObject    handle to slider_q2 (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    empty - handles not created until after all CreateFcns called% Hint: slider controls usually have a light gray background.if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))    set(hObject,'BackgroundColor',[.9 .9 .9]);end% --- Executes on slider movement.function slider_q3_Callback(hObject, eventdata, handles)% hObject    handle to slider_q3 (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    structure with handles and user data (see GUIDATA)% Hints: get(hObject,'Value') returns position of slider%        get(hObject,'Min') and get(hObject,'Max') to determine range of sliderval=get(hObject,'Value');set(handles.edit_q3,'String',num2str(val));drive_robot(handles);% --- Executes during object creation, after setting all properties.function slider_q3_CreateFcn(hObject, eventdata, handles)% hObject    handle to slider_q3 (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    empty - handles not created until after all CreateFcns called% Hint: slider controls usually have a light gray background.if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))    set(hObject,'BackgroundColor',[.9 .9 .9]);end% --- Executes on slider movement.function slider_q4_Callback(hObject, eventdata, handles)% hObject    handle to slider_q4 (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    structure with handles and user data (see GUIDATA)% Hints: get(hObject,'Value') returns position of slider%        get(hObject,'Min') and get(hObject,'Max') to determine range of sliderval=get(hObject,'Value');set(handles.edit_q4,'String',num2str(val));drive_robot(handles);% --- Executes during object creation, after setting all properties.function slider_q4_CreateFcn(hObject, eventdata, handles)% hObject    handle to slider_q4 (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    empty - handles not created until after all CreateFcns called% Hint: slider controls usually have a light gray background.if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))    set(hObject,'BackgroundColor',[.9 .9 .9]);end% --- Executes on slider movement.function slider_q5_Callback(hObject, eventdata, handles)% hObject    handle to slider_q5 (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    structure with handles and user data (see GUIDATA)% Hints: get(hObject,'Value') returns position of slider%        get(hObject,'Min') and get(hObject,'Max') to determine range of sliderval=get(hObject,'Value');set(handles.edit_q5,'String',num2str(val));drive_robot(handles);% --- Executes during object creation, after setting all properties.function slider_q5_CreateFcn(hObject, eventdata, handles)% hObject    handle to slider_q5 (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    empty - handles not created until after all CreateFcns called% Hint: slider controls usually have a light gray background.if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))    set(hObject,'BackgroundColor',[.9 .9 .9]);end% --- Executes on slider movement.function slider_q6_Callback(hObject, eventdata, handles)% hObject    handle to slider_q6 (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    structure with handles and user data (see GUIDATA)% Hints: get(hObject,'Value') returns position of slider%        get(hObject,'Min') and get(hObject,'Max') to determine range of sliderval=get(hObject,'Value');set(handles.edit_q6,'String',num2str(val));drive_robot(handles);% --- Executes during object creation, after setting all properties.function slider_q6_CreateFcn(hObject, eventdata, handles)% hObject    handle to slider_q6 (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    empty - handles not created until after all CreateFcns called% Hint: slider controls usually have a light gray background.if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))    set(hObject,'BackgroundColor',[.9 .9 .9]);endfunction edit_q1_Callback(hObject, eventdata, handles)% hObject    handle to edit_q1 (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    structure with handles and user data (see GUIDATA)% Hints: get(hObject,'String') returns contents of edit_q1 as text%        str2double(get(hObject,'String')) returns contents of edit_q1 as a doubleval=str2double(get(hObject,'String'));slider_min=get(handles.slider_q1,'Min');slider_max=get(handles.slider_q1,'Max');if val>=slider_min && val<=slider_max,    set(handles.slider_q1,'Value',val);    drive_robot(handles);else    uiwait(errordlg(['Your Value must be in ' num2str(slider_min) ' ~ ' num2str(slider_max) '!'],'Error'));    set(hObject,'String','');end% --- Executes during object creation, after setting all properties.function edit_q1_CreateFcn(hObject, eventdata, handles)% hObject    handle to edit_q1 (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    empty - handles not created until after all CreateFcns called% Hint: edit controls usually have a white background on Windows.%       See ISPC and COMPUTER.if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))    set(hObject,'BackgroundColor','white');endfunction edit_q2_Callback(hObject, eventdata, handles)% hObject    handle to edit_q2 (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    structure with handles and user data (see GUIDATA)% Hints: get(hObject,'String') returns contents of edit_q2 as text%        str2double(get(hObject,'String')) returns contents of edit_q2 as a doubleval=str2double(get(hObject,'String'));slider_min=get(handles.slider_q2,'Min');slider_max=get(handles.slider_q2,'Max');if val>=slider_min && val<=slider_max,    set(handles.slider_q2,'Value',val);    drive_robot(handles);else    uiwait(errordlg(['Your Value must be in ' num2str(slider_min) ' ~ ' num2str(slider_max) '!'],'Error'));    set(hObject,'String','');end% --- Executes during object creation, after setting all properties.function edit_q2_CreateFcn(hObject, eventdata, handles)% hObject    handle to edit_q2 (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    empty - handles not created until after all CreateFcns called% Hint: edit controls usually have a white background on Windows.%       See ISPC and COMPUTER.if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))    set(hObject,'BackgroundColor','white');endfunction edit_q3_Callback(hObject, eventdata, handles)% hObject    handle to edit_q3 (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    structure with handles and user data (see GUIDATA)% Hints: get(hObject,'String') returns contents of edit_q3 as text%        str2double(get(hObject,'String')) returns contents of edit_q3 as a doubleval=str2double(get(hObject,'String'));slider_min=get(handles.slider_q3,'Min');slider_max=get(handles.slider_q3,'Max');if val>=slider_min && val<=slider_max,    set(handles.slider_q3,'Value',val);    drive_robot(handles);else    uiwait(errordlg(['Your Value must be in ' num2str(slider_min) ' ~ ' num2str(slider_max) '!'],'Error'));    set(hObject,'String','');end% --- Executes during object creation, after setting all properties.function edit_q3_CreateFcn(hObject, eventdata, handles)% hObject    handle to edit_q3 (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    empty - handles not created until after all CreateFcns called% Hint: edit controls usually have a white background on Windows.%       See ISPC and COMPUTER.if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))    set(hObject,'BackgroundColor','white');endfunction edit_q4_Callback(hObject, eventdata, handles)% hObject    handle to edit_q4 (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    structure with handles and user data (see GUIDATA)% Hints: get(hObject,'String') returns contents of edit_q4 as text%        str2double(get(hObject,'String')) returns contents of edit_q4 as a doubleval=str2double(get(hObject,'String'));slider_min=get(handles.slider_q4,'Min');slider_max=get(handles.slider_q4,'Max');if val>=slider_min && val<=slider_max,    set(handles.slider_q4,'Value',val);    drive_robot(handles);else    uiwait(errordlg(['Your Value must be in ' num2str(slider_min) ' ~ ' num2str(slider_max) '!'],'Error'));    set(hObject,'String','');end% --- Executes during object creation, after setting all properties.function edit_q4_CreateFcn(hObject, eventdata, handles)% hObject    handle to edit_q4 (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    empty - handles not created until after all CreateFcns called% Hint: edit controls usually have a white background on Windows.%       See ISPC and COMPUTER.if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))    set(hObject,'BackgroundColor','white');endfunction edit_q5_Callback(hObject, eventdata, handles)% hObject    handle to edit_q5 (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    structure with handles and user data (see GUIDATA)% Hints: get(hObject,'String') returns contents of edit_q5 as text%        str2double(get(hObject,'String')) returns contents of edit_q5 as a doubleval=str2double(get(hObject,'String'));slider_min=get(handles.slider_q5,'Min');slider_max=get(handles.slider_q5,'Max');if val>=slider_min && val<=slider_max,    set(handles.slider_q5,'Value',val);    drive_robot(handles);else    uiwait(errordlg(['Your Value must be in ' num2str(slider_min) ' ~ ' num2str(slider_max) '!'],'Error'));    set(hObject,'String','');end% --- Executes during object creation, after setting all properties.function edit_q5_CreateFcn(hObject, eventdata, handles)% hObject    handle to edit_q5 (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    empty - handles not created until after all CreateFcns called% Hint: edit controls usually have a white background on Windows.%       See ISPC and COMPUTER.if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))    set(hObject,'BackgroundColor','white');endfunction edit_q6_Callback(hObject, eventdata, handles)% hObject    handle to edit_q6 (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    structure with handles and user data (see GUIDATA)% Hints: get(hObject,'String') returns contents of edit_q6 as text%        str2double(get(hObject,'String')) returns contents of edit_q6 as a doubleval=str2double(get(hObject,'String'));slider_min=get(handles.slider_q6,'Min');slider_max=get(handles.slider_q6,'Max');if val>=slider_min && val<=slider_max,    set(handles.slider_q6,'Value',val);    drive_robot(handles);else    uiwait(errordlg(['Your Value must be in ' num2str(slider_min) ' ~ ' num2str(slider_max) '!'],'Error'));    set(hObject,'String','');end% --- Executes during object creation, after setting all properties.function edit_q6_CreateFcn(hObject, eventdata, handles)% hObject    handle to edit_q6 (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    empty - handles not created until after all CreateFcns called% Hint: edit controls usually have a white background on Windows.%       See ISPC and COMPUTER.if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))    set(hObject,'BackgroundColor','white');endfunction drive_robot(handles)theta1=get(handles.slider_q1,'Value');theta2=get(handles.slider_q2,'Value');theta3=get(handles.slider_q3,'Value');theta4=get(handles.slider_q4,'Value');theta5=get(handles.slider_q5,'Value');theta6=get(handles.slider_q6,'Value');puma=findobj('Tag','puma560_gui');if ~isempty(puma),    puma=guihandles(puma);    axes(puma.axes1);    cla;    q=[theta1 theta2 theta3 theta4 theta5 theta6];    eval('puma560');    plot(p560,q);else    puma=guihandles(puma560_gui());    axes(puma.axes1);    cla;    q=[theta1 theta2 theta3 theta4 theta5 theta6];    eval('puma560');    plot(p560,q);end