Matlab 机器人工具箱 轨迹生成----jtraj ctraj

% [q,qd,qdd] = jtraj(q0,qf,m); %利用五次多项式规划轨迹
% tc = ctraj(T0,T1,n);         %利用匀加速匀减速规划轨迹

%机器人建模
L1 = Link('d',0,'a',0,'alpha',pi/2);
L2 = Link('d',0,'a',0.5,'alpha',0,'offset',pi/2);
L3 = Link('d',0,'a',0,'alpha',pi/2,'offset',pi/4);
L4 = Link('d',1,'a',0,'alpha',-pi/2);
L5 = Link('d',0,'a',0,'alpha',pi/2);
L6 = Link('d',1,'a',0,'alpha',0);
%机器人搭建与命名
robot = SerialLink([L1,L2,L3,L4,L5,L6]);
robot.name = '球形腕关节的拟人臂';
robot.manuf = 'qiangbaa';

%******************%
%      jtraj       %
%******************%
%轨迹规划参数设置
init_ang = [0 0 0 0 0 0];
targ_ang = [pi/4, -pi/3, pi/5, pi/2, -pi/4, pi/6];
step = 50;
%轨迹规划方法
[q,qd,qdd] = jtraj(init_ang,targ_ang,step);                                %直接得到角度、角速度、角加速度的的序列
%动画显示
subplot(3,2,[1,3]); %subplot 对画面分区，【1.3】占用1 3的位置，3.2三行两列
robot.plot(q);

%显示位置、速度、加速度变化曲线
subplot(3, 2, 2);
i = 1:6;
plot(q(:,1));
title('位置');
grid on;

subplot(3, 2, 4);
i = 1:6;
plot(qd(:,1));
title('速度');
grid on;

subplot(3, 2, 6);
i = 1:6;
plot(qdd(:,1));
title('加速度');
grid on;

t = robot.fkine(q);%运动学正解
rpy=tr2rpy(t);     %t中提取位置（xyz）
subplot(3,2,5);
plot2(rpy);

 

%******************%
%      ctraj       %
%******************%
%参数设置
T0 = robot.fkine(init_ang);%运动学正解
T1 = robot.fkine(targ_ang);%运动学正解

Tc = ctraj(T0,T1,step);                                                    %得到每一步的T阵

tt = transl(Tc);
plot2(tt,'r');
title('T0-T1直线轨迹');
grid on;