使用Matlab的Robotics Toolbox进行运动学三维坐标位姿描述

实现移动和旋转代码：

%绘制坐标系的移动位姿
clc
clear
close('all')
T1=troty(0);
T2=transl(4,0,0)
T=T2*T1;
p1=[2;4;3;1];
Ap1=T*p1;
%%坐标系逆变换，坐标点正变换
Tr=inv(T);
Bp1=Tr*p1;
%%绘图
T0=transl(0,0,0);
trplot(T0,'frame','A','color','b');
axis([-5 5 -5 5 -5 5]);
hold on,
tranimate(T0,T,'frame','B','color','r')

其他相关函数说明：

平移和旋转矩阵：

        transl(x,y,z)：(x,y,z)的平移

T1 = transl(0.5,0.0,0.0);   

trotx(theta) / troty(theta) / trotz(theta)：绕x / y / z轴theta的旋转

R2 = trotx(pi/2)*troty(pi/2)*trotz(-pi/2);

rotx(theta) / roty(theta) / rotz(theta)：绕x / y / z轴theta得到的旋转矩阵

R3 = rotx(30,"deg")*roty(30,"deg")*rotz(30,"deg");

注意：trotx和rotx默认都为弧度，trotx可直接与平移函数相乘，而rotx不可

绘制坐标变换：

trplot ( R )：绘制坐标系变换R

trplot(T0,'frame','A','color','b');

tranimate ( R )：绘制旋转动画R

tranimate(T0,T0*R2,'frame','B','color','r')