PUMA560机器人模型，修改第五部分笛卡尔空间直线插补，其它部分可小改

Question

PUMA560机器人模型

clear;d1=685;d2=152;d3=50;d4=457;d6=127;a2=381;a3=25;
L(1)=Link([0,0,0,0],'modified'); % Link([theta,d,a, alpha])
L(2)=Link([0,d2,0,-pi/2],'modified');
L(3)=Link([0,0,a2,0],'modified');% 实际图-d3,这里采用0
L(4)=Link([0,d4,a3,-pi/2],'modified');
L(5)=Link([0,0,0,pi/2],'modified');
L(6)=Link([0,0,0,-pi/2],'modified');
L(1).qlim=[-160,160]/180*pi;
L(2).qlim=[-225,45]/180*pi;
L(3).qlim=[-45,225]/180*pi;
L(4).qlim=[-110,170]/180*pi;
L(5).qlim=[-100,100]/180*pi;
L(6).qlim=[-266,266]/180*pi;
Six_Link=SerialLink([L(1),L(2),L(3),L(4),L(5),L(6)],'name','DOF6');
figure(1)
Six_Link.plot([pi/2,0,-pi/2,0,0,0])
Six_Link.disp
figure(2)Six_Link.teach
% 二、正运动学
ks = pi/180;t=0:0.1:8; %8秒完成轨迹，步长0.1T1 = Six_Link.fkine([0 0 0 0 0 0]*ks); %将关节角度转换为末端位姿的齐次变换阵
T2 = Six_Link.fkine([70 10 30 -50 30 30]*ks);% 三、逆运动学 
q1 = Six_Link.ikine(T1); q2 = Six_Link.ikine(T2);% 四、关节空间轨迹规划 
[q,qt,qtt]=jtraj(q1,q2,t); %计算从q1到q2的关节空间轨迹函数jtraj,生成三个参数：%q为关节角度值，qt为关节角速度值，qtt为关节角加速度值
Six_Link.plot(q,'movie','Motion.gif')%动态绘制轨迹运动% 
五、操作空间（笛卡尔空间）轨迹规划 
% 利用ctraj函数和ikine函数实现
TD = ctraj(T1, T2, length(t)); % 计算两个位姿之间的轨迹
qk = zeros(length(t),6); % 初始化关节角度矩阵
q0=[0 0 0 0 0 0];
for i=1:50qk(i,:) = Six_Link.ikine(TD(i),'q0', q0, 'mask', [1 1 1 1 1 1]); % 计算逆解   
end% 计算关节角速度（qt）和关节角加速度（qtt）%qt = diff(qk)/0.1;
%qtt = diff(qt)/0.1;% 在计算中，将时间间隔设置为0.1秒（即轨迹步长），可根据实际情况调整% 由于diff函数会减少一个数据点，因此qt比qk少1行，qtt比qt又少1行% 为了方便绘制，可以补充一行0元素，使得三个矩阵行数相同
%qt = [qt; zeros(1,6)];%qtt = [qtt; zeros(2,6)];% 六、绘制结果图
% 1、绘出6个关节的角度，角速度，角加速度的信息图
figure('name','关节随时间变化图')
subplot(3, 1, 1);
plot(t, q) %绘制关节角随时间的变化
grid on;xlabel('时间(s)');ylabel('关节角度(rad)')
legend('关节1','关节2','关节3','关节4','关节5','关节6','location','northeastoutside')% 补充绘制角速度、角加速度信息图的代码
subplot(3, 1, 2);plot(t, qt) %绘制关节角速度随时间的变化
grid on;xlabel('时间(s)');ylabel('关节角速度(rad/s)')
legend('关节1','关节2','关节3','关节4','关节5','关节6','location','northeastoutside')
subplot(3, 1, 3);plot(t, qtt) %绘制关节角加速度随时间的变化
grid on;xlabel('时间(s)');ylabel('关节角加速度(rad/s^2)')
legend('关节1','关节2','关节3','关节4','关节5','关节6','location','northeastoutside')% 补充绘制角速度、角加速度信息图的代码
% 2、末端点轨迹(x-y-z视图)
figure('name','末端点轨迹线')
T_n = Six_Link.fkine(q); %将关节角度转换为末端位姿的齐次变换阵
p = transl(T_n); %取齐次变换矩阵的位置矢量部分
plot3(p(:,1),p(:,2),p(:,3));
grid on;
xlabel('X轴(mm)');ylabel('Y轴(mm)');zlabel('Z轴(mm)');

```

Answer 1

clear;d1=685;d2=152;d3=50;d4=457;d6=127;a2=381;a3=25;
L(1)=Link([0,0,0,0],‘modified’); % Link([theta,d,a, alpha])
L(2)=Link([0,d2,0,-pi/2],‘modified’);
L(3)=Link([0,0,a2,0],‘modified’);% 实际图-d3,这里采用0
L(4)=Link([0,d4,a3,-pi/2],‘modified’);
L(5)=Link([0,0,0,pi/2],‘modified’);
L(6)=Link([0,0,0,-pi/2],‘modified’);
L(1).qlim=[-160,160]/180pi;
L(2).qlim=[-225,45]/180pi;
L(3).qlim=[-45,225]/180pi;
L(4).qlim=[-110,170]/180pi;
L(5).qlim=[-100,100]/180pi;
L(6).qlim=[-266,266]/180pi;
Six_Link=SerialLink([L(1),L(2),L(3),L(4),L(5),L(6)],‘name’,‘DOF6’);
figure(1)
Six_Link.plot([pi/2,0,-pi/2,0,0,0])
Six_Link.disp
figure(2)
Six_Link.teach

% 二、正运动学
ks = pi/180;t=0:0.1:8; %8秒完成轨迹，步长0.1
T1 = Six_Link.fkine([0 0 0 0 0 0]*ks); %将关节角度转换为末端位姿的齐次变换阵
T2 = Six_Link.fkine([70 10 30 -50 30 30]*ks);

% 三、逆运动学
q1 = Six_Link.ikine(T1);
q2 = Six_Link.ikine(T2);

% 四、关节空间轨迹规划
[q,qt,qtt]=jtraj(q1,q2,t); %计算从q1到q2的关节空间轨迹函数jtraj,生成三个参数：%q为关节角度值，qt为关节角速度值，qtt为关节角加速度值

% 五、笛卡尔空间直线插补轨迹规划
TD = ctraj(T1, T2, length(t)); % 计算两个位姿之间的轨迹
qk = zeros(length(t),6); % 初始化关节角度矩阵
q0=[0 0 0 0 0 0];
for i=1:length(t)
qk(i,:) = Six_Link.ikine(TD(i),‘q0’, q0, ‘mask’, [1 1 1 1 1 1]); % 计算逆解
end

% 计算关节角速度（qt）和关节角加速度（qtt）
qt = diff(qk)/0.1; % 在计算中，将时间间隔设置为0.1秒（即轨迹步长），可根据实际情况调整
qtt = diff(qt)/0.1;

% 由于diff函数会减少一个数据点，因此qt比qk少1行，qtt比qt又少1行
% 为了方便绘制，可以补充一行0元素，使得三个矩阵行数相同
qt = [qt; zeros(1,6)];
qtt = [qtt; zeros(1,6)];

% 六、绘制结果图

% 1、绘出6个关节的角度、角速度、角加速度的信息图
figure(‘name’,‘关节随时间变化图’)
subplot(3, 1, 1);
plot(t, q) %绘制关节角随时间的变化
grid on;xlabel(‘时间(s)’);ylabel(‘关节角度(rad)’)
legend(‘关节1’,‘关节2’,‘关节3’,‘关节4’,‘关节5’,‘关节6’,‘location’,‘northeastoutside’)

subplot(3, 1, 2);
plot(t, qt) %绘制关节角速度随时间的变化
grid on;xlabel(‘时间(s)’);ylabel(‘关节角速度(rad/s)’)
legend(‘关节1’,‘关节2’,‘关节3’,‘关节4’,‘关节5’,‘关节6’,‘location’,‘northeastoutside’)

subplot(3, 1, 3);
plot(t, qtt) %绘制关节角加速度随时间的变化
grid on;xlabel(‘时间(s)’);ylabel(‘关节角加速度(rad/s^2)’)
legend(‘关节1’,‘关节2’,‘关节3’,‘关节4’,‘关节5’,‘关节6’,‘location’,‘northeastoutside’)

% 2、末端点轨迹(x-y-z视图)
figure(‘name’,‘末端点轨迹线’)
T_n = Six_Link.fkine(q); %将关节角度转换为末端位姿的齐次变换阵
p = transl(T_n); %取齐次变换矩阵的位置矢量部分
plot3(p(:,1),p(:,2),p(:,3));
grid on;
xlabel(‘X轴(mm)’);ylabel(‘Y轴(mm)’);zlabel(‘Z轴(mm)’);

% 显示结果
Six_Link.plot(q,‘movie’,‘Motion.gif’) % 动态绘制轨迹运动
disp(‘Done’)