机器人工具箱函数

五次多项式轨迹tpoly:

t=linspace(0,2,51);在0-2秒内进行51次插值

[P,dP,ddP]=Tploy(0,3,t);位置，速度，加速度

[P,dP,ddP]=Tploy(0,3,t，0.02,0.01)；0-3的位置之内进行51次插值,后两个是最大最小速度

混合曲线轨迹lspb:

t=linspace(0,2,51);

[P,dP,ddP]=lspb(0,3,t);速度有一段匀速

[P,dP,ddP]=Tploy(0,3,t，0.1)；最大速度指定

多维轨迹mtraj：二维

t=linspace(0,2,51);

[P,dP,ddP]=mtraj(@tpoly，[0,0],[3,4],t);五次多项式插值方法进行多维轨迹

多维多段轨迹mstraj:

TRAJ = mstraj(WP,QDMAX,TSEG,Q0,DT,TACC,OPTIONS);

(经过的路径，最大速度，每段时间间隔，初始位置，插值间隔，加速时间，)

                       （这两个只能设置一个）(初始位置为空即0,0)

wp=[0,0;3,4;1,2];

P1=mstraj(wp,[   ],[2,1],[   ],0.04,0);

P2= mstraj(wp,[   ],[2,1],[   ],0.04,0.5);

 tranl:位置给到矩阵

tr2rpy：将三位的旋转矩阵转换成横滚、俯仰、航偏角。

即 [r,p,y] = tr2rpy(T)；%T是3*3的矩阵

输入是旋转变换矩阵T，输出参数是欧拉角的roll，pitch和yaw，即1*3的矩阵。三个角度是弧度制