# 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

### Syntax

``robot = rigidBodyTree``
``robot = rigidBodyTree("MaxNumBodies",N,"DataFormat",dataFormat)``

### 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

This property is read-only.

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

This property is read-only.

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.

This property is read-only.

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 r`igidBody` object contains a `rigidBodyJoint` object and must be added to the r`igidBodyTree` 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 r`igidBody` 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; addBody(rbtree,body1,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; addBody(robot,body1,'base')```

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; addBody(robot,body2,'body1') addBody(robot,body3,'body2') addBody(robot,body4,'body3') addBody(robot,body5,'body4') addBody(robot,body6,'body5')```

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.

`load exampleRobots.mat`

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'); addBody(puma1,body3Copy,'L2') addSubtree(puma1,'L3',subtree) 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]; addBody(robot,body1,'base'); addBody(robot,body2,'body1');```

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.

`load exampleRobots.mat lbr`

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.

`load exampleRobots.mat lbr`

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.

`load exampleRobots.mat lbr`

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