puma560 机器人构建MATLAB

%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,qdd(:,3))%关节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(3,4,:);
figure;
plot3(x,y,z,'ko')%轨迹图像
axis([-1 1 -1 1 -1 1])
grid on;