shuxue3d.m

机器人运行轨迹计算

clf
p1=[0,0,0];
T02=T01*T12;
p2=T02(1:3,4)';
T03=T01*T12*T23;
p3=T03(1:3,4)';
T04=T01*T12*T23*T34;
p4=T04(1:3,4)';
T05=T01*T12*T23*T34*T45;
p5=T05(1:3,4)';
T06=T01*T12*T23*T34*T45*T56;
p6=T06(1:3,4)';
p=[p1;p2;p3;p4;p5;p6]
hold on
plot3(p(1:2,1),p(1:2,2),p(1:2,3),'LineWidth',1,'Color',[0.2 0.2 0.2])
plot3(p(2:3,1),p(2:3,2),p(2:3,3),'Color',[11/256 132/256 199/256])
plot3(p(3:4,1),p(3:4,2),p(3:4,3),'Color',[191/256 0 191/256])
plot3(p(4:5,1),p(4:5,2),p(4:5,3),'Color',[1 0 0])
plot3(p(5:6,1),p(5:6,2),p(5:6,3),'Color',[1 0 0])
plot3(p(2,1),p(2,2),p(2,3),'.')
plot3(p(3,1),p(3,2),p(3,3),'.')
plot3(p(4,1),p(4,2),p(4,3),'.')
plot3(p(5,1),p(5,2),p(5,3),'.')
plot3(p(6,1),p(6,2),p(6,3),'.')
plot3(20,-200,120,'*')
xlim([-600 600])
ylim([-600 600])
zlim([0 600])
view(-60,48)
grid on