puma560_control.m

PUMA560仿真系统,基于MATLAB的robotics toolbox工具箱。

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 EDIT
gui_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});
end

if 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_control
handles.output = hObject;

% Update handles structure
guidata(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 structure
varargout{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});
end
eval('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});
end
T=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});
end
eval('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)']);
end
nq=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)']);
end
puma_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});
end
eval('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)']);
end
figure('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)']);
end
puma_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);