puma+560+matlab+3d,求助，关于puma560的问题

在puma560仿真系统的基础上，想建立一个三自由度得机械手，如何正确设定相关参数。自己设的参数，运行以后，仿真的动画轨迹有问题，请大虾解惑。

clear L

L{1} = link([ pi/2   0            0        0        0], 'standard');

L{2} = link([ 0      0.1        0        0        0], 'standard');

L{3} = link([ 0      0           0        0        0], 'standard');

L{1}.m = 0;

L{2}.m = 17.4;

L{3}.m = 4.8;

L{1}.r = [ 0    0           0 ];

L{2}.r = [ -.3638  .006    .2275];

L{3}.r = [ -.0203  -.0141  .070];

L{1}.I = [  0         0.35         0         0         0         0];

L{2}.I = [  .13         .524         .539         0         0         0];

L{3}.I = [   .066  .086         .0125   0         0         0];

L{1}.Jm =  200e-6;

L{2}.Jm =  200e-6;

L{3}.Jm =  200e-6;

L{1}.G =  -62.6111;

L{2}.G =  107.815;

L{3}.G =  -53.7063;

% viscous friction (motor referenced)

L{1}.B =   1.48e-3;

L{2}.B =   .817e-3;

L{3}.B =   1.38e-3;

% Coulomb friction (motor referenced)

L{1}.Tc = [ .395        -.435];

L{2}.Tc = [ .126        -.071];

L{3}.Tc = [ .132        -.105];

%

% some useful poses

%

qz = [0 0 0 ]; % zero angles, L shaped pose

qr = [0 pi/2 -pi/2 ]; % ready pose, arm up

qstretch = [0 0 -pi/2 ];

M = [ 1 1 1 0 0 0];

prompt={'输入时间向量：','输入起始点坐标:','输入终止点坐标：'};

name='参数：';

numlines=1;

defaultanswer={'0:0.056:2','transl(0.1,0,0)','transl(0,0,0.1)'};

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));

q0 = [0 0 0 ];

p5603 = robot(L, 'Puma 560', 'Unimation', 'params of 8/95');

q=ikine(p5603,T,q0,M);                                 % ikine 工具箱函数调用

plot(p5603,q);

figure('Name','PUMA560机器人仿真演示窗口---运动学逆问题');                           % 6个角度的变化曲线

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)')

p560.name = 'Puma 560';

p560.manuf = 'Unimation';