# rigidBodyTree

Create tree-structured robot

## Description

The rigidBodyTree is a representation of the connectivity of rigid bodies with joints. Use this class to build robot manipulator models in MATLAB®. If you have a robot model specified using the Unified Robot Description Format (URDF), use importrobot to import your robot model.

A rigid body tree model is made up of rigid bodies as rigidBody objects. Each rigid body has a rigidBodyJoint object associated with it that defines how it can move relative to its parent body. Use setFixedTransform to define the fixed transformation between the frame of a joint and the frame of one of the adjacent bodies. You can add, replace, or remove rigid bodies from the model using the methods of the RigidBodyTree class.

Robot dynamics calculations are also possible. Specify the Mass, CenterOfMass, and Inertia properties for each rigidBody in the robot model. You can calculate forward and inverse dynamics with or without external forces and compute dynamics quantities given robot joint motions and joint inputs. To use the dynamics-related functions, set the DataFormat property to "row" or "column".

For a given rigid body tree model, you can also use the robot model to calculate joint angles for desired end-effector positions using the robotics inverse kinematics algorithms. Specify your rigid body tree model when using inverseKinematics or generalizedInverseKinematics.

The show method supports visualization of body meshes. Meshes are specified as .stl files and can be added to individual rigid bodies using addVisual. Also, by default, the importrobot function loads all the accessible .stl files specified in your URDF robot model.

## Creation

### Description

example

robot = rigidBodyTree creates a tree-structured robot object. Add rigid bodies to it using addBody.

robot = rigidBodyTree("MaxNumBodies",N,"DataFormat",dataFormat) specifies an upper bound on the number of bodies allowed in the robot when generating code. You must also specify the DataFormat property as a name-value pair.

## Properties

expand all

Number of bodies in the robot model (not including the base), returned as an integer.

List of rigid bodies in the robot model, returned as a cell array of handles. Use this list to access specific RigidBody objects in the model. You can also call getBody to get a body by its name.

Names of rigid bodies, returned as a cell array of character vectors.

Name of robot base, returned as a string scalar or character vector.

Gravitational acceleration experienced by robot, specified as an [x y z] vector in meters per second squared. Each element corresponds to the acceleration of the base robot frame in that direction.

Input/output data format for kinematics and dynamics functions, specified as "struct", "row", or "column". To use dynamics functions, you must use either "row" or "column".

## Object Functions

 addBody Add body to robot addSubtree Add subtree to robot centerOfMass Center of mass position and Jacobian checkCollision Check if robot is in collision copy Copy robot model externalForce Compose external force matrix relative to base forwardDynamics Joint accelerations given joint torques and states geometricJacobian Geometric Jacobian for robot configuration gravityTorque Joint torques that compensate gravity getBody Get robot body handle by name getTransform Get transform between body frames homeConfiguration Get home configuration of robot inverseDynamics Required joint torques for given motion massMatrix Joint-space mass matrix randomConfiguration Generate random configuration of robot removeBody Remove body from robot replaceBody Replace body on robot replaceJoint Replace joint on body show Show robot model in figure showdetails Show details of robot model subtree Create subtree from robot model velocityProduct Joint torques that cancel velocity-induced forces writeAsFunction Create rigidBodyTree code generating function

## Examples

collapse all

Add a rigid body and corresponding joint to a rigid body tree. Each rigidBody object contains a rigidBodyJoint object and must be added to the rigidBodyTree using addBody.

Create a rigid body tree.

rbtree = rigidBodyTree;

Create a rigid body with a unique name.

body1 = rigidBody('b1');

Create a revolute joint. By default, the rigidBody object comes with a fixed joint. Replace the joint by assigning a new rigidBodyJoint object to the body1.Joint property.

jnt1 = rigidBodyJoint('jnt1','revolute');
body1.Joint = jnt1;

Add the rigid body to the tree. Specify the body name that you are attaching the rigid body to. Because this is the first body, use the base name of the tree.

basename = rbtree.BaseName;

Use showdetails on the tree to confirm the rigid body and joint were added properly.

showdetails(rbtree)
--------------------
Robot: (1 bodies)

Idx    Body Name   Joint Name   Joint Type    Parent Name(Idx)   Children Name(s)
---    ---------   ----------   ----------    ----------------   ----------------
1           b1         jnt1     revolute             base(0)
--------------------

Use the Denavit-Hartenberg (DH) parameters of the Puma560® robot to build a robot. Each rigid body is added one at a time, with the child-to-parent transform specified by the joint object.

The DH parameters define the geometry of the robot with relation to how each rigid body is attached to its parent. For convenience, setup the parameters for the Puma560 robot in a matrix[1]. The Puma robot is a serial chain manipulator. The DH parameters are relative to the previous row in the matrix, corresponding to the previous joint attachment.

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];

Create a rigid body tree object to build the robot.

robot = rigidBodyTree;

Create the first rigid body and add it to the robot. To add a rigid body:

1. Create a rigidBody object and give it a unique name.

2. Create a rigidBodyJoint object and give it a unique name.

3. Use setFixedTransform to specify the body-to-body transformation using DH parameters. The last element of the DH parameters, theta, is ignored because the angle is dependent on the joint position.

4. Call addBody to attach the first body joint to the base frame of the robot.

body1 = rigidBody('body1');
jnt1 = rigidBodyJoint('jnt1','revolute');

setFixedTransform(jnt1,dhparams(1,:),'dh');
body1.Joint = jnt1;

Create and add other rigid bodies to the robot. Specify the previous body name when calling addBody to attach it. Each fixed transform is relative to the previous joint coordinate frame.

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;

Verify that your robot was built properly by using the showdetails or show function. showdetails lists all the bodies in the MATLAB® command window. show displays the robot with a given configuration (home by default). Calls to axis modify the axis limits and hide the axis labels.

showdetails(robot)
--------------------
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)
--------------------
show(robot);
axis([-0.5,0.5,-0.5,0.5,-0.5,0.5])
axis off

References

[1] Corke, P. I., and B. Armstrong-Helouvry. “A Search for Consensus among Model Parameters Reported for the PUMA 560 Robot.” Proceedings of the 1994 IEEE International Conference on Robotics and Automation, IEEE Comput. Soc. Press, 1994, pp. 1608–13. DOI.org (Crossref), doi:10.1109/ROBOT.1994.351360.

Make changes to an existing rigidBodyTree object. You can get replace joints, bodies and subtrees in the rigid body tree.

Load example robots as rigidBodyTree objects.

View the details of the Puma robot using showdetails.

showdetails(puma1)
--------------------
Robot: (6 bodies)

Idx    Body Name   Joint Name   Joint Type    Parent Name(Idx)   Children Name(s)
---    ---------   ----------   ----------    ----------------   ----------------
1           L1         jnt1     revolute             base(0)   L2(2)
2           L2         jnt2     revolute               L1(1)   L3(3)
3           L3         jnt3     revolute               L2(2)   L4(4)
4           L4         jnt4     revolute               L3(3)   L5(5)
5           L5         jnt5     revolute               L4(4)   L6(6)
6           L6         jnt6     revolute               L5(5)
--------------------

Get a specific body to inspect the properties. The only child of the L3 body is the L4 body. You can copy a specific body as well.

body3 = getBody(puma1,'L3');
childBody = body3.Children{1}
childBody =
rigidBody with properties:

Name: 'L4'
Joint: [1x1 rigidBodyJoint]
Mass: 1
CenterOfMass: [0 0 0]
Inertia: [1 1 1 0 0 0]
Parent: [1x1 rigidBody]
Children: {[1x1 rigidBody]}
Visuals: {}
Collisions: {}

body3Copy = copy(body3);

Replace the joint on the L3 body. You must create a new Joint object and use replaceJoint to ensure the downstream body geometry is unaffected. Call setFixedTransform if necessary to define a transform between the bodies instead of with the default identity matrices.

newJoint = rigidBodyJoint('prismatic');
replaceJoint(puma1,'L3',newJoint);

showdetails(puma1)
--------------------
Robot: (6 bodies)

Idx    Body Name       Joint Name       Joint Type    Parent Name(Idx)   Children Name(s)
---    ---------       ----------       ----------    ----------------   ----------------
1           L1             jnt1         revolute             base(0)   L2(2)
2           L2             jnt2         revolute               L1(1)   L3(3)
3           L3        prismatic            fixed               L2(2)   L4(4)
4           L4             jnt4         revolute               L3(3)   L5(5)
5           L5             jnt5         revolute               L4(4)   L6(6)
6           L6             jnt6         revolute               L5(5)
--------------------

Remove an entire body and get the resulting subtree using removeBody. The removed body is included in the subtree.

subtree = removeBody(puma1,'L4')
subtree =
rigidBodyTree with properties:

NumBodies: 3
Bodies: {[1x1 rigidBody]  [1x1 rigidBody]  [1x1 rigidBody]}
Base: [1x1 rigidBody]
BodyNames: {'L4'  'L5'  'L6'}
BaseName: 'L3'
Gravity: [0 0 0]
DataFormat: 'struct'

Remove the modified L3 body. Add the original copied L3 body to the L2 body, followed by the returned subtree. The robot model remains the same. See a detailed comparison through showdetails.

removeBody(puma1,'L3');

showdetails(puma1)
--------------------
Robot: (6 bodies)

Idx    Body Name   Joint Name   Joint Type    Parent Name(Idx)   Children Name(s)
---    ---------   ----------   ----------    ----------------   ----------------
1           L1         jnt1     revolute             base(0)   L2(2)
2           L2         jnt2     revolute               L1(1)   L3(3)
3           L3         jnt3     revolute               L2(2)   L4(4)
4           L4         jnt4     revolute               L3(3)   L5(5)
5           L5         jnt5     revolute               L4(4)   L6(6)
6           L6         jnt6     revolute               L5(5)
--------------------

To use dynamics functions to calculate joint torques and accelerations, specify the dynamics properties for the rigidBodyTree object and rigidBody.

Create a rigid body tree model. Create two rigid bodies to attach to it.

robot = rigidBodyTree('DataFormat','row');
body1 = rigidBody('body1');
body2 = rigidBody('body2');

Specify joints to attach to the bodies. Set the fixed transformation of body2 to body1. This transform is 1m in the x-direction.

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

Specify dynamics properties for the two bodies. Add the bodies to the robot model. For this example, basic values for a rod (body1) with an attached spherical mass (body2) are given.

body1.Mass = 2;
body1.CenterOfMass = [0.5 0 0];
body1.Inertia = [0.001 0.67 0.67 0 0 0];

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

Compute the center of mass position of the whole robot. Plot the position on the robot. Move the view to the xy plane.

comPos = centerOfMass(robot);

show(robot);
hold on
plot(comPos(1),comPos(2),'or')
view(2)

Change the mass of the second body. Notice the change in center of mass.

body2.Mass = 20;
replaceBody(robot,'body2',body2)

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

Calculate the resultant joint accelerations for a given robot configuration with applied external forces and forces due to gravity. A wrench is applied to a specific body with the gravity being specified for the whole robot.

Load a predefined KUKA LBR robot model, which is specified as a RigidBodyTree object.

Set the data format to 'row'. For all dynamics calculations, the data format must be either 'row' or 'column'.

lbr.DataFormat = 'row';

Set the gravity. By default, gravity is assumed to be zero.

lbr.Gravity = [0 0 -9.81];

Get the home configuration for the lbr robot.

q = homeConfiguration(lbr);

Specify the wrench vector that represents the external forces experienced by the robot. Use the externalForce function to generate the external force matrix. Specify the robot model, the end effector that experiences the wrench, the wrench vector, and the current robot configuration. wrench is given relative to the 'tool0' body frame, which requires you to specify the robot configuration, q.

wrench = [0 0 0.5 0 0 0.3];
fext = externalForce(lbr,'tool0',wrench,q);

Compute the resultant joint accelerations due to gravity, with the external force applied to the end-effector 'tool0' when lbr is at its home configuration. The joint velocities and joint torques are assumed to be zero (input as an empty vector []).

qddot = forwardDynamics(lbr,q,[],[],fext);

Use the inverseDynamics function to calculate the required joint torques to statically hold a specific robot configuration. You can also specify the joint velocities, joint accelerations, and external forces using other syntaxes.

Load a predefined KUKA LBR robot model, which is specified as a RigidBodyTree object.

Set the data format to 'row'. For all dynamics calculations, the data format must be either 'row' or 'column'.

lbr.DataFormat = 'row';

Set the Gravity property to give a specific gravitational acceleration.

lbr.Gravity = [0 0 -9.81];

Generate a random configuration for lbr.

q = randomConfiguration(lbr);

Compute the required joint torques for lbr to statically hold that configuration.

tau = inverseDynamics(lbr,q);

Use the externalForce function to generate force matrices to apply to a rigid body tree model. The force matrix is an m-by-6 vector that has a row for each joint on the robot to apply a six-element wrench. Use the externalForce function and specify the end effector to properly assign the wrench to the correct row of the matrix. You can add multiple force matrices together to apply multiple forces to one robot.

To calculate the joint torques that counter these external forces, use the inverseDynamics function.

Load a predefined KUKA LBR robot model, which is specified as a RigidBodyTree object.

Set the data format to 'row'. For all dynamics calculations, the data format must be either 'row' or 'column'.

lbr.DataFormat = 'row';

Set the Gravity property to give a specific gravitational acceleration.

lbr.Gravity = [0 0 -9.81];

Get the home configuration for lbr.

q = homeConfiguration(lbr);

Set external force on link1. The input wrench vector is expressed in the base frame.

fext1 = externalForce(lbr,'link_1',[0 0 0.0 0.1 0 0]);

Set external force on the end effector, tool0. The input wrench vector is expressed in the tool0 frame.

fext2 = externalForce(lbr,'tool0',[0 0 0.0 0.1 0 0],q);

Compute the joint torques required to balance the external forces. To combine the forces, add the force matrices together. Joint velocities and accelerations are assumed to be zero (input as []).

tau = inverseDynamics(lbr,q,[],[],fext1+fext2);

You can import robots that have .stl files associated with the Unified Robot Description format (URDF) file to describe the visual geometries of the robot. Each rigid body has an individual visual geometry specified. The importrobot function parses the URDF file to get the robot model and visual geometries. The function assumes that visual geometry and collision geometry of the robot are the same and assigns the visual geometries as collision geometries of corresponsing bodies.

Use the show function to display the visual and collosion geometries of the robot model in a figure. You can then interact with the model by clicking components to inspect them and right-clicking to toggle visibility.

Import a robot model as a URDF file. The .stl file locations must be properly specified in this URDF. To add other .stl files to individual rigid bodies, see addVisual.

robot = importrobot('iiwa14.urdf');

Visualize the robot with the associated visual model. Click bodies or frames to inspect them. Right-click bodies to toggle visibility for each visual geometry.

show(robot,'visuals','on','collision','off');

Visualize the robot with the associated collision geometries. Click bodies or frames to inspect them. Right-click bodies to toggle visibility for each collision geometry.

show(robot,'visuals','off','collision','on');

expand all

## References

[1] Craig, John J. Introduction to Robotics: Mechanics and Control. Reading, MA: Addison-Wesley, 1989.

[2] Siciliano, Bruno, Lorenzo Sciavicco, Luigi Villani, and Giuseppe Oriolo. Robotics: Modelling, Planning and Control. London: Springer, 2009.

## Version History

Introduced in R2016b

expand all