%puma560 机器人构建
clc; clear;
%modified 改进的D-H法
L1=link([0 0 pi 0 0],'modified');
L2=link([-pi/2 0 0 0.1491 0],'modified');
L3=link([0 0.4318 -pi/2 0 0],'modified');
L4=link([-pi/2 0.0203 0 0.4318 0],'modified');
L5=link([pi/2 0 0 0 0],'modified');
L6=link([-pi/2 0 0 0 0],'modified');
r=robot({L1 L2 L3 L4 L5 L6});
r.name='PUMA560';%模型的名称
drivebot(r)
%前3个关节对机械手位置的影响
qA=[0,0,0,0,0,0]; %起始点关节空间矢量
qB=[2,-1,-0.25,0,0,0]; %终止点关节空间矢量
t=[0:0.1:10]; %仿真时间
[q,qd,qdd]=jtraj(qA,qB,t); %关节空间规划
plot(r,q)
%关节3的角速度、角速度和角加速度曲线
figure
subplot(1,3,1)
plot(t,q(:,3))%关节3的位移曲线
xlabel('时间t/s');ylabel('关节的角位移/rad');grid on
subplot(1,3,2)
plot(t,qd(:,3))%关节3的角速度曲线
xlabel('时间t/s');ylabel('关节的角速度/(rad/s)')
grid on;
subplot(1,3,3)
plot(t,