参考代码https://blog.csdn.net/jldemanman/article/details/80911704,作者:JojenZz
这篇代码里面的modmyt06是一个SE3矩阵,我用size查看发现它不是4*4的而是1*1的,所以没法直接用modmyt06(1,4)提取出x方向上的平移。查看SE3文档说明后,使用命令transl将SE3的平移向量提取出来就可以用了,查看提取后的矩阵看到是一个1*3的,刚好是xyz。只需要改动for循环里的代码即可。
for n=1:1:30000
q=[theta1(n) theta2(n) theta3(n) theta4(n) theta5(n) theta6(n)];
T6=fkine(robot,q);
T=T6.transl;
plot3(T(1,1),T(1,2),T(1,3),'b.','MarkerSize',0.5);
end