matlab机械臂运动仿真

matlab机械臂运动仿真

%% 创建机械臂
L1=Link([0 12.4 0 pi/2 0 -pi/2]);
L2=Link([0 0 0 -pi/2 ]);
L3=Link([0 15.43 0 pi/2 ]);
L4=Link([0 0 0 -pi/2 0 0]);
L5=Link([0 15.925 0 pi/2]);
L6=Link([0 0 0 -pi/2 ]);
L7=Link([0 15.0 0 0 0 pi/2]);
Rbt=SerialLink([L1 L2 L3 L4 L5 L6 L7]);
%% 规划路径
q0=[0 0 0 0 0 0 0];
qsq1=[0.46088 0.37699 0 1.31 0 1.4451 0];
qsq2=[.81681 0.56549 0 1.0681 0 1.2566 0 ];
qsq3=[2.36 0.69115 0 0.848 0 1.4451 0 ];
qsq4=[2.66 0.37699 0 1.31 0 1.4451 0];
qsq5=[pi/2 0.62831 0 1.5708 0 0.94249 0];
qsq6=[0 0.62831 0 1.5708 0 0.94249 0];
t=0:.04:2;
sqtraj1=jtraj(q0,qsq1,t); 
sqtraj2=jtraj(qsq1,qsq2,t); 
sqtraj3=jtraj(qsq2,qsq3,t); 
sqtraj4=jtraj(qsq3,qsq4,t);
sqtraj5=jtraj(qsq4,qsq1,t);
sqtraj6=jtraj(qsq1,q0,t);
sqtraj7=jtraj(qsq6,q0,t);
hold on
atj=zeros(4,4);
%% 绘制动画
view(-35,40)
xlim([-40,40])
ylim([-40,40])
zlim([0,60])
for i=1:1:51
    atj=Rbt.fkine(sqtraj1(i,:));
    JTA(i,:)=transl(atj);
    jta=JTA;
    plot3(jta(i,1),jta(i,2),jta(i,3),'r.')
    Rbt.plot(sqtraj1(i,:))
    plot3(JTA(i,1),JTA(i,2),JTA(i,3),'b')
end
for i=1:1:51
    atj2=Rbt.fkine(sqtraj2(i,:));
    JTA2(i,:)=transl(atj2);
    jta2=JTA2;
    plot3(jta2(i,1),jta2(i,2),jta2(i,3),'r.')
    Rbt.plot(sqtraj2(i,:))
    plot3(JTA2(i,1),JTA2(i,2),JTA2(i,3),'b')
end
for i=1:1:51
    atj3=Rbt.fkine(sqtraj3(i,:));
    JTA3(i,:)=transl(atj3);
    jta3=JTA3;
    plot3(jta3(i,1),jta3(i,2),jta3(i,3),'r.')
    Rbt.plot(sqtraj3(i,:))
    plot3(JTA3(i,1),JTA3(i,2),JTA3(i,3),'b')
end
for i=1:1:51
    atj4=Rbt.fkine(sqtraj4(i,:));
    JTA4(i,:)=transl(atj4);
    jta4=JTA4;
    plot3(jta4(i,1),jta4(i,2),jta4(i,3),'r.')
    Rbt.plot(sqtraj4(i,:))
    plot3(JTA4(i,1),JTA4(i,2),JTA4(i,3),'b')
end
for i=1:1:51
    atj5=Rbt.fkine(sqtraj5(i,:));
    JTA5(i,:)=transl(atj5);
    jta5=JTA5;
    plot3(jta5(i,1),jta5(i,2),jta5(i,3),'r.')
    Rbt.plot(sqtraj5(i,:))
    plot3(JTA5(i,1),JTA5(i,2),JTA5(i,3),'b')
end
for i=1:1:51
    atj6=Rbt.fkine(sqtraj6(i,:));
    JTA6(i,:)=transl(atj6);
    jta6=JTA6;
    plot3(jta6(i,1),jta6(i,2),jta6(i,3),'r.')
    Rbt.plot(sqtraj6(i,:))
    plot3(JTA6(i,1),JTA6(i,2),JTA6(i,3),'b')
end
 for i=1:1:51
    atj7=Rbt.fkine(sqtraj7(i,:));
    JTA7(i,:)=transl(atj7);
    jta7=JTA7;
    plot3(jta7(i,1),jta7(i,2),jta7(i,3),'r.')
    Rbt.plot(sqtraj7(i,:))
    plot3(JTA7(i,1),JTA7(i,2),JTA7(i,3),'b')
end