matlab的ctraj,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;