Matlab机器人工具箱（2-3）：一些demo（轨迹规划）

01 轨迹规划函数（关节空间和笛卡尔）

%运动学轨迹
mdl_puma560%调出puma560 DH模型作为实验对象
t=[0:0.05:2];%两秒完成轨迹，步长0.05
%关节运动（产生两位置间路径，结果表达为一系列中间点的关节角度）
%产生位姿矩阵法1：直接给出关节角度
%T1 = p560.fkine([0 0 0 0 0 0]);%生成一个位姿
%T2 = p560.fkine([pi 0 0 0 0 0]);%生成一个位姿
%产生位姿矩阵法2：描述位置
T1 = transl(0.4,0.2,0)*trotx(pi);%位移*旋转,创建齐次变换
T2 = transl(0.4,-0.2,0)*trotx(pi/2);
q=p560.jtraj(T1,T2,t);%输入SE3 or 4*4变换矩阵，生成轨迹
p560.plot(q)%绘制轨迹
%笛卡尔运动
%笛卡尔空间中直线运动,生成从SE3空间两点间直线的一系列中间位置，结果表达为4*4齐次换矩阵
Ts=ctraj(T1,T2,length(t));

代码详解：
利用Robotics Toolbox提供的ctraj和jtraj函数可以实现笛卡尔规划和关节空间规划。

其中ctraj函数的调用格式：

• TC = CTRAJ(T0, T1, N)
• TC = CTRAJ(T0, T1, R)
  说明：
  参数TC为从T0到T1的笛卡尔规划轨迹，N为点的数量，R为给定路径距离向量，R的每个值必须在0到1之间。

其中jtraj函数的调用格式：

• [Q QD QDD] = JTRAJ(Q0, Q1, N)
• [Q QD QDD] = JTRAJ(Q0, Q1, N, QD0, QD1)
• [Q QD QDD] = JTRAJ(Q0, Q1, T)
• [Q QD QDD] = JTRAJ(Q0, Q1, T, QD0, QD1)
  说明：
  参数Q为从状态Q0到Q1的关节空间规划轨迹，N为规划的点数，T为给定的时间向量的长度，速度非零边界可以用QD0和QD1来指定。QD和QDD为返回的规划轨迹的速度和加速度。

02 关节空间和笛卡尔空间的轨迹规划（对比）

1. qq=jtraj=(q1,q2,t)
2. qT=p560.jtraj(T1,T2,t)
3. Ts=ctraj(T1,T2,length(t));

①和②是关节规划的函数，③是笛卡尔规划的函数。
①②是先根据首尾位姿，求出关节角度，再规划
③是根据首尾位姿，求轨迹，再逆解得到关节角度

①的参数是关节角度，②的参数是位姿矩阵

clc
clear

mdl_puma560%调出puma560 DH模型作为实验对象
t=[0:0.05:2];%两秒完成轨迹，步长0.05

T1 = transl(0.4,0.2,0)*trotx(pi);%位移*旋转,创建齐次变换
T2 = transl(0.4,-0.2,0)*trotx(pi/2);

q1 = p560.ikine6s(T1);%根据末端位姿，求各关节
q2 = p560.ikine6s(T2);

qq=jtraj(q1,q2,t);%根据各关节，生成轨迹
Tqq=p560.fkine(qq);

qT=p560.jtraj(T1,T2,t);%根据位姿矩阵，生成轨迹
Tqt=p560.fkine(qT);

%笛卡尔运动
%笛卡尔空间中直线运动,生成从SE3空间两点间直线的一系列中间位置，结果表达为4*4齐次换矩阵
Ts=ctraj(T1,T2,length(t));
qs=p560.ikine6s(Ts);

figure(1)%绘制各关节角度
for i=1:6
      subplot(2,3,i)
      plot(t, qq(:,i))
      hold on;
      plot(t, qT(:,i))
      plot(t, qs(:,i)) 
      legend('关节空间-根据各关节规划','关节空间-根据位姿规划','笛卡尔空间')
      hold off;
end

figure(2)
pq=transl(Tqq);%提取旋转矩阵中的位移部分
pT=transl(Tqt);
ps=transl(Ts);
%依次是 '关节空间-根据各关节规划','关节空间-根据位姿规划','笛卡尔空间'
subplot(3,1,1)
plot3(pq(:,1),pq(:,2),pq(:,3))
subplot(3,1,2)
plot3(pT(:,1),pT(:,2),pT(:,3))
subplot(3,