function varargout = puma560_control(varargin)% PUMA560_CONTROL M-file for puma560_control.fig%      PUMA560_CONTROL, by itself, creates a new PUMA560_CONTROL or raises the existing%      singleton*.%%      H = PUMA560_CONTROL returns the handle to a new PUMA560_CONTROL or the handle to%      the existing singleton*.%%      PUMA560_CONTROL('CALLBACK',hObject,eventData,handles,...) calls the local%      function named CALLBACK in PUMA560_CONTROL.M with the given input arguments.%%      PUMA560_CONTROL('Property','Value',...) creates a new PUMA560_CONTROL or raises the%      existing singleton*.  Starting from the left, property value pairs are%      applied to the GUI before puma560_control_OpeningFcn gets called.  An%      unrecognized property name or invalid value makes property application%      stop.  All inputs are passed to puma560_control_OpeningFcn via varargin.%%      *See GUI Options on GUIDE's Tools menu.  Choose "GUI allows only one%      instance to run (singleton)".%% See also: GUIDE, GUIDATA, GUIHANDLES% Edit the above text to modify the response to help puma560_control% Last Modified by GUIDE v2.5 04-May-2008 17:11:08% Begin initialization code - DO NOT EDITgui_Singleton = 1;gui_State = struct('gui_Name',       mfilename, ...                   'gui_Singleton',  gui_Singleton, ...                   'gui_OpeningFcn', @puma560_control_OpeningFcn, ...                   'gui_OutputFcn',  @puma560_control_OutputFcn, ...                   'gui_LayoutFcn',  [] , ...                   'gui_Callback',   []);if nargin && ischar(varargin{1})    gui_State.gui_Callback = str2func(varargin{1});endif nargout    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});else    gui_mainfcn(gui_State, varargin{:});end% End initialization code - DO NOT EDIT% --- Executes just before puma560_control is made visible.function puma560_control_OpeningFcn(hObject, eventdata, handles, varargin)% This function has no output args, see OutputFcn.% hObject    handle to figure% eventdata  reserved - to be defined in a future version of MATLAB% handles    structure with handles and user data (see GUIDATA)% varargin   command line arguments to puma560_control (see VARARGIN)% Choose default command line output for puma560_controlhandles.output = hObject;% Update handles structureguidata(hObject, handles);% UIWAIT makes puma560_control wait for user response (see UIRESUME)% uiwait(handles.puma560_control);%初始化程序puma_gui=findobj('Tag','puma560_gui');if ~isempty(puma_gui),    %存在PUMA560演示窗口else    %不存在PUMA560演示窗口    uiwait(msgbox('您還沒有打開PUMA560演示窗口。程序將自動為您打開！','提示'));    puma560_gui();end% --- Outputs from this function are returned to the command line.function varargout = puma560_control_OutputFcn(hObject, eventdata, handles) % varargout  cell array for returning output args (see VARARGOUT);% hObject    handle to figure% eventdata  reserved - to be defined in a future version of MATLAB% handles    structure with handles and user data (see GUIDATA)% Get default command line output from handles structurevarargout{1} = handles.output;% --- Executes on button press in pushbtn_yunZ.function pushbtn_yunZ_Callback(hObject, eventdata, handles)% hObject    handle to pushbtn_yunZ (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    structure with handles and user data (see GUIDATA)%運動學正問題% Forward kinematics is the problem of solving the Cartesian position and % orientation of a mechanism given knowledge of the kinematic structure and % the joint coordinates.close(findobj('tag','puma560_gui'));prompt={'輸入時間向量t：','輸入起始點坐標:','輸入終止點坐標：'};name='參數：';numlines=1;defaultanswer={'0:0.056:2','[0 0 0 0 0 0]','[0 pi/2 -pi/2 0 0 0]'};answer=inputdlg(prompt,name,numlines,defaultanswer);if ~isempty(answer),    t=str2num(answer{1});    qz=str2num(answer{2});    qr=str2num(answer{3});endeval('puma560');figure('Name','PUMA560機器人仿真演示窗口---運動學正問題');q=jtraj(qz,qr,t);T=fkine(p560,q);subplot(3,1,1);plot(t, squeeze(T(1,4,:)));xlabel('Time (s)');ylabel('X (m)');subplot(3,1,2);plot(t, squeeze(T(2,4,:)));xlabel('Time (s)');ylabel('Y (m)');subplot(3,1,3);plot(t, squeeze(T(3,4,:)));xlabel('Time (s)');ylabel('Z (m)');puma_gui=puma560_gui();puma_gui=guihandles(puma_gui);axes(puma_gui.axes1);plot(p560,q);x=squeeze(T(1,4,:));y=squeeze(T(2,4,:));z=squeeze(T(3,4,:));figure('Name','PUMA560機器人仿真演示窗口---運動學正問題');plot3(x,y,z);% --- Executes on button press in pushbtn_yunN.function pushbtn_yunN_Callback(hObject, eventdata, handles)% hObject    handle to pushbtn_yunN (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    structure with handles and user data (see GUIDATA)%運動學逆問題% Inverse kinematics is the problem of finding the robot joint coordinates,% given a homogeneous transform representing the last link of the manipulator.% It is very useful when the path is planned in Cartesian space, for instance % a straight line path as shown in the trajectory demonstration.eval('puma560');close(findobj('tag','puma560_gui'));prompt={'輸入時間向量：','輸入起始點坐標:','輸入終止點坐標：'};name='參數：';numlines=1;defaultanswer={'0:0.056:2','transl(0.6,-0.5,0)','transl(0.4,0.5,0.2)'};answer=inputdlg(prompt,name,numlines,defaultanswer);if ~isempty(answer),    t=str2num(answer{1});    T1=str2num(answer{2});    T2=str2num(answer{3});endT=ctraj(T1,T2,length(t));q=ikine(p560,T);figure('Name','PUMA560機器人仿真演示窗口---運動學逆問題');subplot(3,2,1)plot(t,q(:,1))xlabel('Time (s)');ylabel('Joint 1 (rad)')subplot(3,2,2)plot(t,q(:,2))xlabel('Time (s)');ylabel('Joint 2 (rad)')subplot(3,2,3)plot(t,q(:,3))xlabel('Time (s)');ylabel('Joint 3 (rad)')subplot(3,2,4)plot(t,q(:,4))xlabel('Time (s)');ylabel('Joint 4 (rad)')subplot(3,2,5)plot(t,q(:,5))xlabel('Time (s)');ylabel('Joint 5 (rad)')subplot(3,2,6)plot(t,q(:,6))xlabel('Time (s)');ylabel('Joint 6 (rad)')eval('puma560');puma_gui=guihandles(puma560_gui());axes(puma_gui.axes1);plot(p560,q);% --- Executes on button press in pushbtn_dongZ.function pushbtn_dongZ_Callback(hObject, eventdata, handles)% hObject    handle to pushbtn_dongZ (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    structure with handles and user data (see GUIDATA)%動力學正問題% Forward dynamics is the computation of joint accelerations given position and% velocity state, and actuator torques.  It is useful in simulation of a robot% control system.prompt={'輸入時間段：'};name='參數';numlines=1;defaultanswer={'[0 2]'};answer=inputdlg(prompt,name,numlines,defaultanswer);if ~isempty(answer),    t0=str2num(answer{1});endeval('puma560');[t,q,qd]=fdyn(nofriction(p560),t0(1),t0(2));nq=find(all(q==0)==0);figure('Name','PUMA560機器人仿真演示窗口---動力學正問題(位置)','NumberTitle','off');for i=1:length(nq),    subplot(round(length(nq)/2),2,i);    plot(t,q(:,i));    xlabel('Time(s)');    ylabel(['Joint ' num2str(nq(i)) ' (rad)']);endnq=find(all(qd==0)==0);figure('Name','PUMA560機器人仿真演示窗口---動力學正問題(速度)','NumberTitle','off');for i=1:length(nq),    subplot(round(length(nq)/2),2,i);    plot(t,qd(:,i));    xlabel('Time(s)');    ylabel(['Joint ' num2str(nq(i)) ' (rad/s)']);endpuma_gui=guihandles(puma560_gui());axes(puma_gui.axes1);plot(p560,q);% --- Executes on button press in pushbtn_dongN.function pushbtn_dongN_Callback(hObject, eventdata, handles)% hObject    handle to pushbtn_dongN (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    structure with handles and user data (see GUIDATA)%動力學逆問題% Inverse dynamics computes the joint torques required to achieve the specified% state of joint position, velocity and acceleration.  % The recursive Newton-Euler formulation is an efficient matrix oriented% algorithm for computing the inverse dynamics, and is implemented in the % function rne().% Inverse dynamics requires inertial and mass parameters of each link, as well% as the kinematic parameters.  This is achieved by augmenting the kinematic % description matrix with additional columns for the inertial and mass % parameters for each link.prompt={'輸入時間向量：','輸入起始點：','輸入終止點：'};name='參數';numlines=1;defaultanswer={'0:0.056:2','[0 0 0 0 0 0]','[0 pi/2 -pi/2 0 0 0]'};answer=inputdlg(prompt,name,numlines,defaultanswer);if ~isempty(answer),    t=str2num(answer{1});    qz=str2num(answer{2});    qr=str2num(answer{3});endeval('puma560');[q,qd,qdd]=jtraj(qz,qr,t);tau=rne(p560,q,qd,qdd);taug=gravload(p560,q);nt=find(all(tau~=0));ng=find(all(taug~=0));figure('Name','PUMA560機器人仿真演示窗口---動力學逆問題(無重力)','NumberTitle','off');for i=1:length(nt),    subplot(length(nt),1,i);    plot(t,tau(:,nt(i)));    xlabel('Time(s)');    ylabel(['Joint ' num2str(nt(i)) ' torque(Nm)']);endfigure('Name','PUMA560機器人仿真演示窗口---動力學逆問題(有重力)','NumberTitle','off');for i=1:length(ng),    subplot(length(ng),1,i);    plot(t,taug(:,ng(i)));    xlabel('Time(s)');    ylabel(['Joint ' num2str(ng(i)) ' Gravity torque(Nm)']);endpuma_gui=guihandles(puma560_gui());axes(puma_gui.axes1);plot(p560,q);% --- Executes on button press in pushbtn_trace.function pushbtn_trace_Callback(hObject, eventdata, handles)% hObject    handle to pushbtn_trace (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    structure with handles and user data (see GUIDATA)%軌跡規劃：關節空間(jtraj)，直角坐標(ctraj)和基坐標變換(trinterp)三種方案str={'關節空間','直角坐標'};[s,v]=listdlg('PromptString','選擇方案:','SelectionMode','single','ListString',str); switch s,    case 0,    case 1,        prompt={'輸入時間向量:','輸入起始點坐標:','輸入終止點坐標：'};        name='輸入所需參數';        numlines=1;        defaultanswer={'0:0.056:2','[0 0 0 0 0 0]','[0 pi/2 -pi/2 0 0 0]'};        answer=inputdlg(prompt,name,numlines,defaultanswer);        t=str2num(answer{1});        qz=str2num(answer{2});        qr=str2num(answer{3});        [q,qd,qdd]=jtraj(qz,qr,t);        nq=find(all(q==0)==0);        for i=1:length(nq),            figure('Name','PUMA560機器人仿真演示窗口---軌跡規劃(關節空間）');            subplot(3,1,1);            plot(t,q(:,nq(i)));            title('Theta');            xlabel('Time(s)');            ylabel(['Joint ' num2str(nq(i)) '(rad)']);            subplot(3,1,2);            plot(t,qd(:,nq(i)));            title('Velocity');            xlabel('Time(s)');            ylabel(['Joint ' num2str(nq(i)) 'vel(rad/s)']);            subplot(3,1,3);            plot(t,q(:,nq(i)));            title('Acceleration');            xlabel('Time(s)');            ylabel(['Joint ' num2str(nq(i)) 'accel(rad/s^2)']);        end        puma_gui=guihandles(puma560_gui());        axes(puma_gui.axes1);        eval('puma560');        plot(p560,q);    case 2,        prompt={'輸入時間向量：','輸入起始點坐標:','輸入終止點坐標：'};        name='輸入所需參數：';        numlines=1;        defaultanswer={'0:0.056:2','transl(0.6,-0.5,0)','transl(0.4,0.5,0.2)'};        answer=inputdlg(prompt,name,numlines,defaultanswer);        if ~isempty(answer),            t=str2num(answer{1});            T1=str2num(answer{2});            T2=str2num(answer{3});        end        T=ctraj(T1,T2,length(t));        h=findobj('tag','puma560_gui');        if ~isempty(h),            close(findobj('Tag','puma560_gui'));        end        figure('Name','PUMA560機器人仿真演示窗口---軌跡規劃（直角坐標）');        subplot(3,1,1);        plot(t, squeeze(T(1,4,:)));        xlabel('Time (s)');        ylabel('X (m)');        subplot(3,1,2);        plot(t, squeeze(T(2,4,:)));        xlabel('Time (s)');        ylabel('Y (m)');        subplot(3,1,3);        plot(t, squeeze(T(3,4,:)))        xlabel('Time (s)');        ylabel('Z (m)');        figure('Name','PUMA560機器人仿真演示窗口---軌跡規劃（直角坐標）');        plot3(squeeze(T(1,4,:)),squeeze(T(2,4,:)),squeeze(T(3,4,:)));        eval('puma560');        q=ikine(p560,T);        puma_gui=guihandles(puma560_gui());        axes(puma_gui.axes1);        plot(p560,q);end% --- Executes on button press in pushbtn_exit.function pushbtn_exit_Callback(hObject, eventdata, handles)% hObject    handle to pushbtn_exit (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    structure with handles and user data (see GUIDATA)%退出程序close(findobj('Tag','puma560_control'));puma_gui=findobj('Tag','puma560_gui');if ~isempty(puma_gui),close(puma_gui);end% --- Executes on button press in pushbtn_import.function pushbtn_import_Callback(hObject, eventdata, handles)% hObject    handle to pushbtn_import (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    structure with handles and user data (see GUIDATA)%載入數據puma=findobj('Tag','puma560_gui');if ~isempty(puma),    %如果存在PUMA560演示窗口    puma=guihandles(puma);    eval('puma560');    axes(puma.axes1);    qz=zeros(1,6);    plot(p560,qz);else    %如果不存在PUMA560演示窗口    puma=guihandles(puma560_gui());    axes(puma.axes1);    qz=zeros(1,6);    eval('puma560');    plot(p560,qz);end% --- Executes on button press in pushbtn_animation.function pushbtn_animation_Callback(hObject, eventdata, handles)% hObject    handle to pushbtn_animation (see GCBO)% eventdata  reserved - to be defined in a future version of MATLAB% handles    structure with handles and user data (see GUIDATA)T1=transl(0.6,-0.5,0.2);