Robotics System Toolbox学习笔记（一）：简单建立一个机器人rigidBody、rigidBodyJoint用法

本文软件版本

Matlab:R2019b Robotics System Toolbox

软件介绍

Robotics System Toolbox提供了用于设计，模拟和测试操纵器，移动机器人和类人机器人的工具和算法。对于机械手和类人机器人，该工具箱包括用于碰撞检查，轨迹生成，正向和反向运动学以及使用刚体树表示的动力学的算法。对于移动机器人，它包括用于映射，本地化，路径规划，路径跟踪和运动控制的算法。该工具箱提供了常见的工业机器人应用程序的参考示例。它还包括可购买的工业机器人模型库，您可以导入，可视化和模拟。

代码功能介绍

rigidBody

ridigBody用来创建一个刚体，以便于将其添加到树（树状的机器人操作臂）上，每一个刚体都有一个关节附带。 body = rigidBody(bodyname)表示创建一个名字为”bodyname”的刚体。

rigidBody的性质

例如：  

body1 = rigidBody('B1');
% 默认情况下刚体附带的关节是固定关节，可将其替换成名为'J1'的旋转关节
body1.Joint = rigidBodyJoint('J1', 'revolute');

rigidBody的函数

• copy创建刚体对象的深层副本。

NEWBODY = copy(body)表示会创建一个刚体对象的可选择性深层副本。一些内在的性质会被重置为默认，而不是被复制，例如父刚体、子刚体。  

% 创建一个名字为B1的刚体
b1 = rigidBody('B1');
% 深层复制
b2 = copy(b1);

• addVisual将可视化数据添加到刚体上。

addVisual(BODY, 'Mesh', FILENAME)将FILENAME中指定的Mesh附加到刚体BODY上。 假定多边形网格的框架与BODY的框架一致。网格文件必须为STL格式。 addVisual(BODY, 'Mesh', FILENAME, TFORM)中，TFORM为相对于主体坐标系的4*4齐次变换矩阵。  

b1 = rigidBody('B1');
% 添加一个可视化数据到刚体B1上
% trvec2tform将位置向量[0.1 0.1 0.2]转变为齐次变换矩阵
% eul2tform(eul,sequence)将欧拉角转换成其次变换矩阵，sequence默认为ZYX，另外还有ZYZ、XYZ
% 上述两个相乘得到T姿态
T = trvec2tform([0.1 0.1 0.2]) * eul2tform([0 0 pi], 'ZYX');
addVisual(b1, 'Mesh', 'mesh/B1.stl', T);

• clearVisual删除刚体上的所有可视化数据

% 从urdf导入机器人模型
lbr = importrobot('iiwa14.urdf');
% 删除iiwa14模型中刚体1上的可视化数据
clearVisual(lbr.Bodies{1});

rigidBodyJoint

创建一个关节。在一个树形机器人结构中，一个关节属于一个特定的刚体，并且每个刚体至少有一个关节。 jointObj = rigidBodyJoint(jname);创建一个具有特定名称jname的固定关节 jointObj = rigidBodyJoint(jname, jtype);创建一个名称为jname的指定关节类型jtype的关节。其中jtype为关节类型，默认为fixed，另外还有revolute、prismatic。

rigidBodyJoint的性质

rigidBodyJoint的函数

• copy创建rigidBodyJoint对象的副本

NEWJNT = COPY(JNT)表示会返回一个与关节JNT性质一样的新关节NEWJWT。  

% 创建一个旋转关节joint_1
jnt1 = rigidBodyJoint('joint_1', 'revolute');
% 创建一个关节对象的副本
jnt1cp = copy(jnt1);
% 删除原关节对象
delete(jnt1);

• setFixedTransform创建关节的固定变换属性

setFixedTransform(jointObj,tform)直接使用提供的tform齐次变换矩阵设置rigidBodyJoint对象jointObj的JointToParentTransform属性。 setFixedTransform(jointObj,dhparams,"dh")使用标准DH参数设置ChildToJointTransform属性。 JointToParentTransform属性设置为一个单位矩阵。 DH参数以$[a\alpha d\theta ]$顺序给出。因为角度取决于关节的位置，因此$\theta$参数被忽略。 setFixedTransform(jointObj,mdhparams,'mdh')具体操作与上一个函数相同，不同点在于该函数使用改进DH参数设置。

例子

clear
%           a       alpha   d      theta    
dhparams = [0   	pi/2	0   	0;
            0.4318	0       0       0
            0.0203	-pi/2	0.15005	0;
            0   	pi/2	0.4318	0;
            0       -pi/2	0   	0;
            0       0       0       0];

robot = rigidBodyTree;
% 创建一个rigidBody对象并给它一个唯一的名称。
body1 = rigidBody('body1'); %第一个刚体
jnt1 = rigidBodyJoint('jnt1','revolute');%刚体上面的关节是转动关节 名字叫jnt1

setFixedTransform(jnt1,dhparams(1,:),'dh');%设置DH变换
body1.Joint = jnt1;
% 调用addBody将第一个身体关节连接到机器人树形结构上。
addBody(robot,body1,'base')
% 创建并添加其他刚体到机器人。在调用addBody来附加它时，指定以前的主体名称。每个固定变换相对于前一个关节坐标系。
body2 = rigidBody('body2');
jnt2 = rigidBodyJoint('jnt2','revolute');
body3 = rigidBody('body3');
jnt3 = rigidBodyJoint('jnt3','revolute');
body4 = rigidBody('body4');
jnt4 = rigidBodyJoint('jnt4','revolute');
body5 = rigidBody('body5');
jnt5 = rigidBodyJoint('jnt5','revolute');
body6 = rigidBody('body6');
jnt6 = rigidBodyJoint('jnt6','revolute');

setFixedTransform(jnt2,dhparams(2,:),'dh');
setFixedTransform(jnt3,dhparams(3,:),'dh');
setFixedTransform(jnt4,dhparams(4,:),'dh');
setFixedTransform(jnt5,dhparams(5,:),'dh');
setFixedTransform(jnt6,dhparams(6,:),'dh');

body2.Joint = jnt2;
body3.Joint = jnt3;
body4.Joint = jnt4;
body5.Joint = jnt5;
body6.Joint = jnt6;

addBody(robot,body2,'body1')
addBody(robot,body3,'body2')
addBody(robot,body4,'body3')
addBody(robot,body5,'body4')
addBody(robot,body6,'body5')

% 查看细节 部件名,关节名 .索引序号
showdetails(robot)

% 获取机器人的home构型
% 修改配置并将第二个关节位置设置为pi / 2。在机械手配置中显示结果更改。
conf= homeConfiguration(robot);
conf(2).JointPosition=pi/2;
% show可视化结果在figure中显示,此结果是默认配置的机器人状态	
show(robot,conf);
axis([-0.5,0.5,-0.5,0.5,-0.5,0.5])

  输出：  

--------------------
Robot: (6 bodies)

 Idx    Body Name   Joint Name   Joint Type    Parent Name(Idx)   Children Name(s)
 ---    ---------   ----------   ----------    ----------------   ----------------
   1        body1         jnt1     revolute             base(0)   body2(2)  
   2        body2         jnt2     revolute            body1(1)   body3(3)  
   3        body3         jnt3     revolute            body2(2)   body4(4)  
   4        body4         jnt4     revolute            body3(3)   body5(5)  
   5        body5         jnt5     revolute            body4(4)   body6(6)  
   6        body6         jnt6     revolute            body5(5)   
--------------------

参考

Robot Models — Functions