Robotics System Toolbox学习笔记（二）：Robot Models 函数与示例

本文软件版本

Matlab：R2019b Robotics System Toolbox 看官方文档源码比什么都强= =

loadrobot

加载机器人模型。  

% 根据指定的机器人名称将机器人模型加载为rigidBodyTree对象，即该函数返回一个rigidBodyTree对象
robotRBT = loadrobot(robotname)

% metaData是机器人模型源信息，是一个结构体，包含机器人名称、urdf文件路径、模型来源URL
[robotRBT, metaData] = loadrobot(robotname)

% Name-Value对参数，例如'Gravity', [0 0 -9.81]
[robotRBT, metaData] = loadrobot(robotname, Name, Value)

  该工具箱中可选用的机器人名称robotname如下：  
 

robot1 = loadrobot('kinovaJacoJ2N6S200')
show(robot1)

    输出：  

robot1 = 
  rigidBodyTree - 属性:

     NumBodies: 13
        Bodies: {[1×1 rigidBody]  [1×1 rigidBody]  [1×1 rigidBody]  [1×1 rigidBody]  [1×1 rigidBody]  [1×1 rigidBody]  [1×1 rigidBody]  [1×1 rigidBody]  [1×1 rigidBody]  [1×1 rigidBody]  [1×1 rigidBody]  [1×1 rigidBody]  [1×1 rigidBody]}
          Base: [1×1 rigidBody]
     BodyNames: {1×13 cell}
      BaseName: 'world'
       Gravity: [0 0 0]
    DataFormat: 'struct'

ans = 
  Axes (Primary) - 属性:

             XLim: [-1.5000 1.5000]
             YLim: [-1.5000 1.5000]
           XScale: 'linear'
           YScale: 'linear'
    GridLineStyle: '-'
         Position: [0.1300 0.1100 0.7750 0.8150]
            Units: 'normalized'

importrobot

从urdf文件、文本或者Simscape Multibody model导入刚体树模型。  

% 通过解析filename所指定的urdf文件来返回一个rigidBodyTree对象
robot = importrobot(filename)

% 解析URDF文本。将URDFtext指定为字符串标量或字符向量
robot = importrobot(URDFtext)

% 导入Simscape Multibody model并返回一个rigidBodyTree对象，importInfo为用于存储导入信息的对象，它是作为initialBodyTreeImportInfo对象返回的。该对象包含输入模型与生成的机器人输出之间的关系。
[robot, importInfo] = importrobot(model)

  例子：  

robot2 = importrobot('iiwa7.urdf') % iiwa7.urdf是系统自带的
show(robot2)

    除了系统自带的，也可以自己写urdf文件或者是自己在SolidWorks建模然后转成urdf格式文件，例如可以利用我之前博客中建立的五自由度机械臂urdf文件，  

robot2 = importrobot('robot2.urdf')
robot2.Gravity = [0 0 -9.81];
show(robot2)
showdetails(robot2)

rigidBodyTree

创建一个树形结构机器人。 rigidBodyTree表示刚体与关节的连接性。使用此类在MATLAB中构建机器人操纵器模型。也可以直接使用importrobot函数将urdf文件导入成机器人模型。 刚体树模型由刚体作为RigidBody对象组成。每个刚体都有一个与之关联的rigidBodyJoint对象，该对象定义了它如何相对于其父实体进行移动。使用setFixedTransform定义关节坐标系和相邻实体之一的坐标系之间的固定齐次变换。 也可以使用rigidBodyTree类的方法在模型中添加、替换或删除刚体。 机器人动力学计算也是可能的。为机器人模型中的每个刚性主体指定质量Mass，质心CenterOfMass和惯性属性Inertia。  

% 返回一个树形结构的机器人对象，可通过使用addBody函数在robot树上添加刚体
robot = rigidBodyTree

% 指定生成代码时机器人对象中允许的主体数量的上限。另外，还必须将DataFormat属性指定为“名称/值”对。
robot = rigidBodyTree('MaxNumBodies', N, 'DataFormat', data	Format)

 
  对象的函数：

addBody

向机器人中添加刚体  

% 向机器人对象添加一个刚体，并附加到由parentname指定的父刚体。body属性定义该实体相对于父实体的移动方式。
addBody(robot, body, parentname)

addSubtree

向机器人中添加子树  

% subtree是新子树模型，robot是已经建立的机器人树模型，将前者添加到后者的名为parentname的父刚体上
addSubtree(robot, parentname, subtree)

centerOfMass

返回质心坐标及质心雅可比  

% 在机器人home构型时计算相对于base坐标系的机器人质心位置坐标，robot为rigidBodyTree对象
com = centerOfMass(robot)

% 在机器人特定关节构型时计算相对于base坐标系的机器人质心位置坐标
com = centerOfMass(robot, configuration)

% 除了质心坐标外，还返回质心雅可比矩阵，该矩阵是刚体质心速度与关节速度之间的映射
[com, comJac] = centerOfMass(robot, configuration)

    configuration表示机器人的构型，是一个1*n的结构体，其中n为关节数。

copy

返回一个机器人模型副本  

% 创建具有相同属性的机器人的深层副本。newrobot中的任何更改都不会反映在robot中。
newrobot = copy(robot)

externalForce

返回相对于base的外力矩阵  

% 返回一个外力矩阵，可以将该返回值用于inverseDynamics和forwardDynamic函数的输入，以将外力wrench施加到由bodyname指定的刚体上。假定wrench输入相对于base坐标系，robot为rigidBodyTree对象
% wrench是一个[Tx Ty Tz Fx Fy Fz]向量，表示施加到body的扭矩和力。wrench的前三个元素对应于绕xyz轴的力矩。最后三个元素是沿相同轴的线性力。除非已经指定机器人构型，否则将假定wrench相对于基系。
fext = externalForce(robot, bodyname, wrench)

% 假设wrench作用在指定构型的bodyname刚体上，返回计算得到的外力矩阵。力矩阵
% fext相对于base坐标系
fext = externalForce(robot, bodyname, wrench, configuration)

forwardDynamtic

正向动力学函数，根据关节力矩矢量tau，返回得到各关节加速度向量  

% 在关节原始速度为零且无外力的情况下，计算由于机器人home构型下由重力引起的关节加速度，robot为rigidBodyTree对象
jointAccel = forwardDynamics(robot)

% 此时指定机器人的关节构型向量
jointAccel = forwardDynamics(robot,configuration)

% 此时额外指定关节速度向量
jointAccel = forwardDynamics(robot,configuration,jointVel)

% 指定构型向量、关节速度向量以及关节力矩向量
jointAccel = forwardDynamics(robot,configuration,jointVel,jointTorq)

% 还指定了一个外部力矩阵
jointAccel = forwardDynamics(robot,configuration,jointVel,jointTorq,fext)

geometricJacobian

返回几何雅可比矩阵  

% 针对指定的末端执行器endeffectorname和机器人模型robot的构型configuration，计算相对于base基座的几何雅可比矩阵
% jacobian是6*n矩阵，n是末端执行器的自由度
jacobian = geometricJacobian(robot,configuration,endeffectorname)

gravityTorqu

返回计算得到的补偿重力的关节力矩

<code class="prism language-matlab has-numbering">% 在重力作用下，计算将机器人robot保持在其原始构型configuration所需的关节扭矩。robot为rigidBodyTree对象
% gravTorq为对应每个关节的重力补偿力矩向量
gravTorq = gravityTorque(robot)

% 指定机器人的关节构型configuration，同样是在重力作用下
gravTorq = gravityTorque(robot,configuration)
</code>

getBody

按名称获取刚体  

% robot为rigidBodyTree对象，从robot中得到名为bodyname的刚体
% body为rigidBody对象
body = getBody(robot,bodyname)

getTransform

获取body坐标系之间的齐次变换  

% 计算使用指定的机器人robot构型将bodyname对应的body坐标系相对于机器人robot基系的转换
% robot为rigidBodyTree对象，bodyname是字符串
% transform为4*4齐次变换矩阵
transform = getTransform(robot,configuration,bodyname)

% 在指定的机器人robot构型下，计算targetbody对应的body坐标系相对于sourcebody对应的body坐标系的转换
transform = getTransform(robot,configuration,sourcebody,targetbody)

inverseDynamics

逆向动力学函数，由给定运动计算所需要的关节力矩  

% 在home构型下，计算机器人robot在不施加外力的情况下保持静止时所需的关节扭矩
% robot为rigidBodyTree对象
jointTorq = inverseDynamics(robot)

% 指定构型
jointTorq = inverseDynamics(robot,configuration)

% 指定构型向量、关节速度向量
jointTorq = inverseDynamics(robot,configuration,jointVel)

% 指定构型向量、关节速度向量、关节加速度向量
jointTorq = inverseDynamics(robot,configuration,jointVel,jointAccel)

% 指定构型、关节速度、关节加速度，同时施加外力fext
jointTorq = inverseDynamics(robot,configuration,jointVel,jointAccel,fext)

massMatrix

返回关节空间质量矩阵  

% 机器人robot在home构型下，返回该构型下的关节空间质量矩阵
% robot为rigidBodyTree对象
H = massMatrix(robot)

% 此时指定机器人robot构型
% H为机器人的质量矩阵，以大小为n×n的正定对称矩阵形式返回，其中n是机器人的速度自由度
H = massMatrix(robot,configuration)

removeBody

从robot树上去除body刚体  

% 从机器人robot模型中移出bodyname指定的body以及所有随后附着的body
removeBody(robot,bodyname)

% 指定去除bodyname对应的body以及所有随后附着的body，返回剩下的子树
% newSubtree为rigidBodyTree对象
newSubtree = removeBody(robot,bodyname)

replaceBody

更换robot上的刚体body  

% 用新的newbody对应的body替换掉原来的bodyname对应的body
% 除“父级”和“子级”属性外，body的所有属性都会相应更新。机器人模型的其余部分不受影响
replaceBody(robot,bodyname,newbody)

replaceJoint

更换body上的joint  

% 如果body是机器人模型的一部分，则在该robot模型中替换关节。这种方法是更改机器人模型中关节的唯一方法。不能直接分配刚体的“关节”属性
replaceJoint(robot,bodyname,joint)

subtree

从机器人模型上创建子树  

% 使用bodyname指定的body作为父刚体来创建新的机器人模型。随后所有附加的body（包括了bodyname对应的body）将添加到子树中。原始机器人模型不受影响
newSubtree = subtree(robot,bodyname)

velocityProduct

返回抵消关节速度引起力的各关节力矩  

% jointVel，速度，指定为矢量。关节速度的数量等于机器人的速度自由度。要使用jointVel的矢量形式，请将机器人的DataFormat属性设置为'row'或'column'
% 计算在特定关节构型configuration下抵消指定关节速度引起的力所需的关节扭矩。重力扭矩不包括在此计算中
jointTorq = velocityProduct(robot,configuration,jointVel)

% 计算在某一关节构型下，为抵消指定关节速度所引起的力所需的关节力矩。重力扭矩不包括在这个计算中。
load exampleRobots.mat lbr
lbr.DataFormat = 'row';
qdot = [0 0 0.2 0.3 0 0.1 0];
% 注意是抵消所以是负
tau = -velocityProduct(lbr, [], qdot);

例子

clear
load exampleRobots.mat
showdetails(puma1)

body3 = getBody(puma1,'L3'); 
% 获取L3(body3)刚体的子刚体
childBody = body3.Children{1} 

% 得到body3的副本，该副本不会复制body3的子刚体和父刚体属性
body3Copy = copy(body3)

% 更换L3的关节属性不改变其他
newJoint = rigidBodyJoint('prismatic');% 移动关节
replaceJoint(puma1,'L3',newJoint);% 替换L3的关节为新关节

showdetails(puma1)

% 删除整个body并使用removeBody获得子树。删除的刚体L4包含在子树中。
subtree = removeBody(puma1,'L4')

% 移除修改后的L3刚体。将原始复制的L3刚体添加到父刚体L2上，然后返回子树。
removeBody(puma1,'L3');
addBody(puma1,body3Copy,'L2')% L2是父
addSubtree(puma1,'L3',subtree)% 加载之前子树，父是L3

showdetails(puma1)

% 指定刚体树的动力学特性
% 要使用动力学函数来计算关节扭矩和加速度，请指定rigidBodyTree对象和rigidBody的动力学特性。
% 创建一个刚体树模型。创造两个刚体附着在它上面。
clear
robot = rigidBodyTree('DataFormat','row');
body1 = rigidBody('body1');
body2 = rigidBody('body2');

joint1 = rigidBodyJoint('joint1','revolute');
joint2 = rigidBodyJoint('joint2');
setFixedTransform(joint2,trvec2tform([1 0 0]))
body1.Joint = joint1;
body2.Joint = joint2;

% 指定两个物体的动力学特性。将body添加到机器人树中。
body1.Mass = 2;
body1.CenterOfMass = [0.5 0 0];
body1.Inertia = [0.167 0.001 0.167 0 0 0];

body2.Mass = 1;
body2.CenterOfMass = [0 0 0];
body2.Inertia = 0.0001*[4 4 4 0 0 0];

addBody(robot, body1, 'base');
addBody(robot, body2, 'body1');

%计算整个机器人的质心位置。画出机器人的位置。将视图移动到xy平面。
comPos = centerOfMass(robot);

show(robot)

hold on
plot(comPos(1), comPos(2), 'or') % 图中红色的空心圆圈即为此时的质心位置
view(2)

%改变第二个物体的质量。注意重心的变化。图中绿色的*为此时的质心位置
body2.Mass = 20;
replaceBody(robot,'body2',body2) % 注意这里是直接替换刚体

comPos2 = centerOfMass(robot);
plot(comPos2(1), comPos2(2), '*g')
hold off

%在刚体树模型上计算由于外力引起的正向动力学（已知力矩求位置、速度、加速度）
% 加载预定义的KUKA LBR机器人模型，该模型指定为rigidBodyTree对象。
clear
load exampleRobots.mat lbr
lbr.DataFormat = 'row';
lbr.Gravity = [0 0 -9.81];
q = homeConfiguration(lbr);
q(2)=pi/4
% wrench是一个[Tx Ty Tz Fx Fy Fz]向量，表示施加到body的扭矩和力。wrench的
% 前三个元素对应于绕xyz轴的力矩。最后三个元素是沿相同轴的线性力。除非已经指定机器
% 人构型，否则将假定wrench相对于基系。
wrench = [0 0 0.5 0 0 0.3];

% 假设wrench作用在指定构型的tool0刚体上，返回计算得到的外力矩阵。力矩阵
% fext相对于base坐标系，q为此时的机器人构型
fext = externalForce(lbr,'tool0',wrench,q);
% 计算当lbr处于主构型时，末端执行器“tool0”所受的外力在重力作用下产生的关节加速度。
% 假设关节速度和关节力矩为零(输入为空向量[])。
qddot = forwardDynamics(lbr,q,[],[],fext);

参考

Robot Models — Functions