程序1：(M→N 点)
clear;
clc;
L1=link([0 0 pi 0 0]);
L2=link([-pi/2 0 0 0.1491 0]);
L3=link([0 0.4318 -pi/2 0 0]);
L5=link([pi/2 0 0 0 0]);
r=robot({L1 L2 L3 L5});
r.name='PUMA560';
drivebot(r) ;
qM=[0,0,0.0,0];
qN=[1.5,0,-0.6,0];
t=[0:0.1:10];
[q,qd,qdd]=jtraj(qA,qB,t);
plot(r,q)
figure
subplot(1,3,1)
plot(t,q(:,3))
xlabel('时间 t/s');
ylabel('关节的角位移/rad');
grid on;
subplot(1,3,2)
plot(t,qd(:,3))
xlabel('时间 t/s');
ylabel('关节的角位移 rad/s)') ;
grid on;
subplot(1,3,3)
plot(t,qdd(:,3))
xlabel('时间t/s');
ylabel('关节的角位移 rad/s^2)');
grid on;
T=fkine(r,q);
x(1,1:101)=T(1,4,:); y(1,1:101)=T(2,4,:); z(1,1:101)=T(2,4,:);
figure; plot3(x,y,z,'ko')
axis([-1 1 -1 1 -1 1])
grid on；
程序2（N→Q 点）
clear;
clc;
L1=link([0 0 pi 0 0]);
L2=link([-pi/2 0 0 0.1491 0]);
L3=link([0 0.4318 -pi/2 0 0]);
L5=link([pi/2 0 0 0 0]);
r=robot({L1 L2 L3 L5});
r.name='PUMA560';
drivebot(r) ;
qN=[1.5,0,0.-0.6,0];
qQ=[2,0,0.6,0];
t=[0:0.1:10];
[q,qd,qdd]=jtraj(qA,qB,t);
plot(r,q)
figure
subplot(1,3,1)
plot(t,q(:,3))
xlabel('时间 t/s');
ylabel('关节的角位移/rad');
grid on;
subplot(1,3,2)
plot(t,qd(:,3))
xlabel('时间 t/s');
ylabel('关节的角位移 rad/s)') ;
grid on;
subplot(1,3,3)
plot(t,qdd(:,3))
xlabel('时间t/s');
ylabel('关节的角位移 rad/s^2)');
grid on;
T=fkine(r,q);
x(1,1:101)=T(1,4,:); y(1,1:101)=T(2,4,:); z(1,1:101)=T(2,4,:);
figure; plot3(x,y,z,'ko')
axis([-1 1 -1 1 -1 1])
grid on；
程序3（Q→M 点）
clear;
clc;
L1=link([0 0 pi 0 0]);
L2=link([-pi/2 0 0 0.1491 0]);
L3=link([0 0.4318 -pi/2 0 0]);
L5=link([pi/2 0 0 0 0]);
r=robot({L1 L2 L3 L5});
r.name='PUMA560';
drivebot(r) ;
qQ=[2,0,0.6,0];
qM=[0,0,0,0];
t=[0:0.1:10];
[q,qd,qdd]=jtraj(qA,qB,t);
plot(r,q)
figure
subplot(1,3,1)
plot(t,q(:,3))
xlabel('时间 t/s');
ylabel('关节的角位移/rad');
grid on;
subplot(1,3,2)
plot(t,qd(:,3))
xlabel('时间 t/s');
ylabel('关节的角位移 rad/s)') ;
grid on;
subplot(1,3,3)
plot(t,qdd(:,3))
xlabel('时间t/s');
ylabel('关节的角位移 rad/s^2)');
grid on;
T=fkine(r,q);
x(1,1:101)=T(1,4,:); y(1,1:101)=T(2,4,:); z(1,1:101)=T(2,4,:);
figure; plot3(x,y,z,'ko')
axis([-1 1 -1 1 -1 1])
grid on；
