clear
L1=Link([0 0 0 0 0],'modified');
L2=Link([-pi/2 0 70 -pi/2 0],'modified');
L3=Link([0 20 100 0],'modified');
L4=Link([pi 70 0 -pi/2],'modified');
L5=Link([0 0 0 -pi/2 0],'modified');
L6=Link([0 50 0 pi/2 0],'modified');
L1.qlim=[-pi pi]; %对关节限制角度
L2.qlim=[0 0.73*pi];
L3.qlim=[0 0.78*pi];
L4.qlim=[-0.92*pi 0.92*pi];
L5.qlim=[0 1.02*pi];
L6.qlim=[-pi pi];
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','robot'); %生成模型
q1=[pi/4 pi/2 pi/6 pi/3 pi/2 pi/5]; %初始关节值
q2=[-pi/2 0 0.23*pi pi/9 pi/8 pi/4]; %运动后的关节值
step=80; %设置步数,越大越精确
[q,qd,qdd]=jtraj(q1,q2,step); %求取轨迹,速度,加速度
subplot(2,2,1); %将plot窗口分区(m,n,q)表示分成m行n列;q表示第几个窗口,[1,2]=q=2;[2,2] q=4;
robot.plot(q,'trail','r'); %trail是提醒计算机跟踪轨迹,并记录下来,具体plot的操作请看作者发的另外一篇文章,这里不赘述。
subplot(2,2,2);
i=1:1:6; %i取值在1-6,每次加1;
plot(q(:,i)); %q为六列矩阵,(:,i)表示第i列的全部元素;(i,:)则表示i行的全部元素;(m:n,i)表示i列的m-n对应的元素。
title('位置');
%grid on(调出网格),off则对应的关闭网格;
subplot(2,2,3);
i=1:1:6;
plot(qd(:,i));
title('速度'); %名字,也可以在窗口里面再自行设置
%grid on;
subplot(2,2,4);
i=1:1:6;
plot(qdd(:,i));
title('加速度');
%grid on;
%t1=robot.fkine([pi/4 pi/2 pi/6 pi/3 pi/2 pi/5]);
%t2=robot.fkine([-pi/2 0 0.23*pi pi/9 pi/8 pi/4]);
%t=ctraj(t1,t2,step);
%m=transl(t);
%plot(m,'r');
%title('t1-t2直线轨迹');
%tt=tr2rpy(t);
%plot(tt,'r');
%title('横滚-俯仰-偏航角');