Rigid Body Tree

Create rigid body tree robot model

Since R2026a

expand all in page
  • Rigid Body Tree block

Libraries:
Robotics System Toolbox / Collision Detection

Description

The Rigid Body Tree block outputs a rigid body tree robot model. A rigid body tree is a representation of the connectivity of rigid bodies with joints.

A rigid body tree model is represented in MATLAB® as a rigidBodyTree object, 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.

You can use the loadrobot function to load a rigid body tree model from the Robot Library, or use the importrobot function to import robot models from Simscape™ Multibody™ models or robot model files such as URDFs.

Examples

expand all

Open Live Script

Use the loadrobot function to load a robot model from the Robot Library and set up the collision environment.

robot = loadrobot("abbIrb120",DataFormat="row");
rad = 0.08;
len = 0.75;
pose1 = trvec2tform([0.4 -0.35 0.3]);
pose2 = trvec2tform([0 0.3 0.5]);
cylinder = collisionCylinder(rad,len,Pose=pose1);
sphere = collisionSphere(rad,Pose=pose2);

Show the robot with the collision environment. In this case, you can see the robot collides with the cylinder.

show(robot,[-pi/4 pi/4 0 0 -pi/4 0]);
hold on
showCollisionArray({cylinder,sphere});
title("Robot in Collision Environment")
hold on

Figure contains an axes object. The axes object with title Robot in Collision Environment, xlabel X, ylabel Y contains 26 objects of type patch, line.

open_system("RBTCheckCollisionModel");

out = sim("RBTCheckCollisionModel");

Self-Collision Data

Check the self-collision status for the robot at the first time step.

isSelfColliding = out.isSelfColl.Data(1)
isSelfColliding = logical
   0

Get the self-separation distance and self-witness points matrices for the first time step.

selfSepDist = out.selfSepDist.Data{1}
selfSepDist = 9×9

       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf
       Inf       Inf       Inf    0.1485    0.2616    0.2919    0.4135       Inf       Inf
       Inf       Inf       Inf       Inf    0.0956    0.1848    0.2968       Inf    0.0401
       Inf    0.1485       Inf       Inf       Inf    0.0730    0.1731       Inf    0.2777
       Inf    0.2616    0.0956       Inf       Inf       Inf    0.0149       Inf    0.3136
       Inf    0.2919    0.1848    0.0730       Inf       Inf       Inf       Inf    0.3196
       Inf    0.4135    0.2968    0.1731    0.0149       Inf       Inf       Inf    0.4398
       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf
       Inf       Inf    0.0401    0.2777    0.3136    0.3196    0.4398       Inf       Inf


selfWitPts = out.selfWitPts.Data{1}
selfWitPts = 27×18

       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf
       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf
       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf
       Inf       Inf       Inf       Inf       Inf       Inf    0.0371    0.1129    0.0517    0.2244    0.0797    0.2790    0.0797    0.3625       Inf       Inf       Inf       Inf
       Inf       Inf       Inf       Inf       Inf       Inf   -0.0309   -0.1015   -0.0516   -0.2213   -0.0797   -0.2707   -0.0797   -0.3625       Inf       Inf       Inf       Inf
       Inf       Inf       Inf       Inf       Inf       Inf    0.3377    0.4441    0.2877    0.3867    0.1909    0.2858    0.1909    0.2959       Inf       Inf       Inf       Inf
       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf    0.1695    0.2246    0.1579    0.2645    0.1752    0.3649       Inf       Inf   -0.0048   -0.0047
       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf   -0.1702   -0.2253   -0.1839   -0.2905   -0.1703   -0.3600       Inf       Inf    0.0044    0.0044
       Inf       Inf       Inf       Inf       Inf       Inf       Inf       Inf    0.4586    0.4034    0.4608    0.3539    0.4644    0.3376       Inf       Inf    0.1662    0.2062
       Inf       Inf    0.0371    0.1129       Inf       Inf       Inf       Inf       Inf       Inf    0.2261    0.2645    0.2709    0.3649       Inf       Inf    0.0638    0.1257
       Inf       Inf   -0.0309   -0.1015       Inf       Inf       Inf       Inf       Inf       Inf   -0.2580   -0.2905   -0.2681   -0.3600       Inf       Inf   -0.0669   -0.1260
       Inf       Inf    0.3377    0.4441       Inf       Inf       Inf       Inf       Inf       Inf    0.4067    0.3539    0.4501    0.3376       Inf       Inf    0.1660    0.4302
       Inf       Inf    0.0517    0.2244    0.1695    0.2246       Inf       Inf       Inf       Inf       Inf       Inf    0.3376    0.3481       Inf       Inf    0.0634    0.2244
       Inf       Inf   -0.0516   -0.2213   -0.1702   -0.2253       Inf       Inf       Inf       Inf       Inf       Inf   -0.3664   -0.3769       Inf       Inf   -0.0673   -0.2213
       Inf       Inf    0.2877    0.3867    0.4586    0.4034       Inf       Inf       Inf       Inf       Inf       Inf    0.3147    0.3147       Inf       Inf    0.1660    0.3867
      ⋮

Get the separation distance and corresponding witness points between link_1 and link_6. Use find and strcmp functions to get the indices of these bodies in the rigid body tree.

body1Idx = find(strcmp(robot.BodyNames,"link_1"));
body2Idx = find(strcmp(robot.BodyNames,"link_6"));
link1_link6_dist = selfSepDist(body1Idx,body2Idx)
link1_link6_dist = 
0.4135

row = body1Idx*3-2; % body index * three dimensions (XYZ), -2 to get starting index of the submatrix
col = body2Idx*2-1; % body index * two witness points, -1 to get starting index of the submatrix
link1_link6_witpts = selfWitPts(row:row+2,col:col+1)
link1_link6_witpts = 3×2

    0.0797    0.3625
   -0.0797   -0.3625
    0.1909    0.2959

Plot a line between the witness points.

plot3(link1_link6_witpts(1,:),link1_link6_witpts(2,:),link1_link6_witpts(3,:),LineWidth=2);

Figure contains an axes object. The axes object with title Robot in Collision Environment, xlabel X, ylabel Y contains 27 objects of type patch, line.

World-Collision Data

Check the world-collision status for the robot at the first time step.

isWorldColliding = out.isWorldColl.Data(1)
isWorldColliding = logical
   1

Get the separation distances and witness points between the robot and the world objects for the first time step.

worldSepDist = out.worldSepDist.Data{1}
worldSepDist = 9×2

       Inf       Inf
    0.3385    0.1980
    0.2003    0.1672
    0.0647    0.3016
       NaN    0.4826
       NaN    0.5670
       NaN    0.6898
       Inf       Inf
    0.3279    0.3133


worldWitPts = out.worldWitPts.Data{1}
worldWitPts = 27×4

       Inf       Inf       Inf       Inf
       Inf       Inf       Inf       Inf
       Inf       Inf       Inf       Inf
    0.0920    0.3411    0.0122    0.0036
   -0.0666   -0.2959    0.0726    0.2346
    0.1634    0.1634    0.3405    0.4541
    0.2018    0.3433    0.0258    0.0083
   -0.1518   -0.2935    0.1245    0.2431
    0.4918    0.4918    0.3278    0.4444
    0.2819    0.3347    0.1426    0.0300
   -0.2664   -0.3037   -0.0537    0.2259
    0.4579    0.4579    0.4868    0.4972
       NaN       NaN    0.2664    0.0378
       NaN       NaN   -0.1837    0.2312
       NaN       NaN    0.3924    0.4847
      ⋮

Get the separation distance and corresponding witness points between link_4 and the sphere collision geometry.

bodyIdx = find(strcmp(robot.BodyNames,"link_3"));
worldObjIdx = 2;
link2_sphere_dist = worldSepDist(bodyIdx,worldObjIdx)
link2_sphere_dist = 
0.3016

row = bodyIdx*3-2;     % body index * three dimensions (XYZ), -2 to get starting index of the submatrix
col = worldObjIdx*2-1; % body index * two witness points, -1 to get starting index of the submatrix
link2_sphere_witpts = worldWitPts(row:row+2,col:col+1)
link2_sphere_witpts = 3×2

    0.1426    0.0300
   -0.0537    0.2259
    0.4868    0.4972

Plot a line between the witness points.

plot3(link2_sphere_witpts(1,:),link2_sphere_witpts(2,:),link2_sphere_witpts(3,:),LineWidth=2);

Figure contains an axes object. The axes object with title Robot in Collision Environment, xlabel X, ylabel Y contains 28 objects of type patch, line.

Ports

Output

expand all

Rigid body tree, returned as a bus slRigidBodyTree with these elements:

  • Bodies — Rigid bodies described as bus slRigidBody with the elements:

    • Name — Name of the rigid body as a uint8 vector.

    • NameLength — Length of the rigid body name.

    • SpatialInertia — Mass and inertia of the body.

    • Collisions — Collision geometries described as a bus slCollision containing the elements:

      • m_Type — Type of collision geometry:

        • 0 — Collision Box

        • 1 — Collision Cylinder

        • 2 — Collision Sphere

        • 3 — Collision Capsule

        • 4 — Collision Mesh

      • m_X — x-dimension size of the collision geometry in meters.

      • m_Y — y-dimension size of the collision geometry in meters.

      • m_Z — z‑dimension size of the collision geometry in meters.

      • m_Radius — Radius of the collision geometry in meters.

      • m_Height — Height of the collision geometry in meters.

      • m_Vertices — Vertices of the collision geometry in local frame.

      • m_Position — xyz-position of the collision geometry in meters.

      • m_Quaternion — Orientation of the collision geometry described as a quaternion.

    • NumCollisions — Number of collision geometries used to describe the collision boundaries of the rigid body.

    • Joint — Rigid body joint in described as bus slRigidBodyJoint with the elements:

      • PositionNumber — Position number of the joint.

      • VelocityNumber — Velocity number of the joint.

      • JointType — Joint type of the joint.

      • JointAxis — Joint axis of the joint.

      • JointToParentTransform — Joint to parent rigid body transform as an homogeneous transformation matrix flattened to a 16-element row vector.

      • MotionSubspace — Motion subspace as a 36-element row vector.

      • ChildToJointTransform — Child-body to rigid body joint transform as an homogeneous transformation matrix flattened to a 16-element row vector.

    • ParentIndex — Body index of parent rigid body in the rigid body tree.

    • ParentFrameTransform — Transformation of the parent frame with respect to the parent body root as an homogeneous transformation matrix flattened to a 16-element row vector..

  • NumBodies — Number of bodies in the rigid body tree.

  • PositionNumber — Position number of the rigid body tree.

  • VelocityNumber — Velocity number of the rigid body tree.

  • VelocityDoFMap — Velocity degree-of-freedom map.

  • PositionDoFMap — Position degree-of-freedom map.

  • Base — Base body described as a bus slRigidBody.

  • Gravity — Gravitational acceleration experienced by robot.

Parameters

expand all

To edit block parameters interactively, use the Property Inspector. From the Simulink® Toolstrip, on the Simulation tab, in the Prepare gallery, select Property Inspector.

Specify a rigid body tree as a rigidBodyTree object.

You can use the loadrobot function to load a model from the Robot Library, or use the importrobot function to import robot models from Simscape Multibody models or robot model files such as URDFs.

The default value, twoJointRigidBodyTree is a helper function that generates a rigid body tree model with two bodies and two joints.

  • Code generation — Simulate model using generated C code. The first time you run a simulation, Simulink generates C code for the block. The block reuses the C code for subsequent simulations, as long as the model does not change.

  • Interpreted execution — Simulate model using the MATLAB interpreter. For more information, see Interpreted Execution vs. Code Generation (Simulink).

More About

expand all

Extended Capabilities

expand all

Version History

Introduced in R2026a

See Also

| |