主要内容

Rigid Body Tree Check Collision

Check if robot is in collision

Since R2026a

  • Rigid Body Tree Check Collision block

Libraries:
Robotics System Toolbox / Collision Detection

Description

The Rigid Body Tree Check Collision block checks if the specified rigid body tree robot model is in collision with itself or a specified set of collision objects in the world.

Examples

expand all

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.

This example shows how to visualize and understand the separation distance and witness point matrices from collision checking. The example uses model from the Check Robot for Collisions between Itself and World example.

open_system("RBTCheckCollisionModel");
out = sim("RBTCheckCollisionModel");

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

selfSepDist = out.selfSepDist.Data{1};
selfWitPts = out.selfWitPts.Data{1};

For readability and illustrative purposes, convert the self-collision distance and self-witness points matrices to tables. You can use this table format to understand how the separation distance matrix is structured.

bodyNames = [robot.BodyNames robot.BaseName];        % Define this to use for table variables and rows
selfDistTable = array2table(selfSepDist, ... % Convert to table for readability
    VariableNames=bodyNames, ...
    RowNames=bodyNames)
selfDistTable=9×9 table
                 base    link_1      link_2      link_3      link_4      link_5      link_6     tool0    base_link
                 ____    _______    ________    ________    ________    ________    ________    _____    _________

    base         Inf         Inf         Inf         Inf         Inf         Inf         Inf     Inf          Inf 
    link_1       Inf         Inf         Inf      0.1485      0.2616     0.29193     0.41354     Inf          Inf 
    link_2       Inf         Inf         Inf         Inf    0.095562     0.18479     0.29681     Inf     0.040051 
    link_3       Inf      0.1485         Inf         Inf         Inf    0.073013     0.17308     Inf      0.27774 
    link_4       Inf      0.2616    0.095562         Inf         Inf         Inf    0.014859     Inf      0.31364 
    link_5       Inf     0.29193     0.18479    0.073013         Inf         Inf         Inf     Inf      0.31957 
    link_6       Inf     0.41354     0.29681     0.17308    0.014859         Inf         Inf     Inf      0.43981 
    tool0        Inf         Inf         Inf         Inf         Inf         Inf         Inf     Inf          Inf 
    base_link    Inf         Inf    0.040051     0.27774     0.31364     0.31957     0.43981     Inf          Inf 

selfWitPtsCell = exampleHelperWitPtsMatrixToCellArray(selfWitPts);
selfWitPtsTable = array2table(selfWitPtsCell, ... % Convert to table for readability
    VariableNames=bodyNames, ...
    RowNames=bodyNames)
selfWitPtsTable=9×9 table
                     base           link_1          link_2          link_3          link_4          link_5          link_6          tool0         base_link  
                 ____________    ____________    ____________    ____________    ____________    ____________    ____________    ____________    ____________

    base         {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}
    link_1       {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}
    link_2       {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}
    link_3       {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}
    link_4       {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}
    link_5       {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}
    link_6       {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}
    tool0        {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}
    base_link    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}    {3×2 double}

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

worldSepDist = out.worldSepDist.Data{1};
worldWitPts = out.worldWitPts.Data{1};

For readability and illustrative purposes, convert the world-collision distance and world-witness points matrices to tables. You can see the world object columns are in the order that you specify to the Rigid Body Tree Check Collision block.

worldDistTable = array2table(worldSepDist, ... % Convert to table for readability
    VariableNames=["cylinder","sphere"], ...
    RowNames=bodyNames)
worldDistTable=9×2 table
                 cylinder    sphere 
                 ________    _______

    base             Inf         Inf
    link_1       0.33855     0.19803
    link_2       0.20029     0.16722
    link_3       0.06468     0.30162
    link_4           NaN     0.48262
    link_5           NaN     0.56702
    link_6           NaN     0.68978
    tool0            Inf         Inf
    base_link    0.32791      0.3133

worldWitPtsCell = exampleHelperWitPtsMatrixToCellArray(out.worldWitPts.Data{1});
worldWitPtsTable = array2table(worldWitPtsCell, ... % Convert to table for readability
    VariableNames=["cylinder","sphere"], ...
    RowNames=bodyNames)
worldWitPtsTable=9×2 table
                   cylinder         sphere   
                 ____________    ____________

    base         {3×2 double}    {3×2 double}
    link_1       {3×2 double}    {3×2 double}
    link_2       {3×2 double}    {3×2 double}
    link_3       {3×2 double}    {3×2 double}
    link_4       {3×2 double}    {3×2 double}
    link_5       {3×2 double}    {3×2 double}
    link_6       {3×2 double}    {3×2 double}
    tool0        {3×2 double}    {3×2 double}
    base_link    {3×2 double}    {3×2 double}

Ports

Input

expand all

Rigid body tree, specified as a bus.

Use the Rigid Body Tree block to create this bus.

Rigid body tree joint configuration, specified as an N-element column vector. N is the position number of the rigid body tree.

Collision geometries in the world, specified as a bus created by any of these collision geometry blocks:

You can concatenate these buses using the Matrix Concatenate (Simulink) block.

Body pairs skipped for checking self-collisions, specified as one of these options:

  • -1 — Ignore all self-collisions.

  • 0 — Skip collision checking between child and parent bodies.

  • 2 — Check for all body pair combinations.

  • P-by-2 matrix — Check collisions between all body pair combinations except for the specified body index pairs, where P is the number of body pairs to exclude from collision checking. For example, [1 2; 4 5] skips collision checking between body 1 and body 2 and between body 4 and body 5. To identify the indices for each body in the rigid body tree, use the showdetails object function of the rigidBodyTree object specified to the Rigid body tree parameter of the Rigid Body Tree block connected to the Tree port.

Exhaustively check for all collisions, specified as 0 or a nonzero value. If you specify 0 at this port, then the block finds the first collision and stops, returning the separation distances and witness points for ignored checks as Inf. If you specify a nonzero value at this port, then the block continues checking for collisions until it has exhausted all possibilities.

Output

expand all

Rigid body tree configuration is in self-collision, returned as a Boolean scalar. A true value indicates that one or more rigid bodies in the rigid body tree is in collision with another rigid body in the rigid body tree. A false value indicates that no rigid bodies are in collision with any other rigid bodies in the rigid body tree.

Minimum separation distance between the bodies of the robot, returned as an (N+1)-by-(N+1) matrix. N is the number of bodies in the robot model. The matrix contains an additional row and column to account for the robot base. Units are in meters.

The matrix takes the form:

[d1,1d1,2d1,Nd1,N+1d2,1d2,2d2,Nd2,N+1dN,1dN,2dN,NdN,N+1dN+1,1dN+1,2dN+1,NdN+1,N+1]

Each element db1,b2 is the minimum separation distance between the body b1 and the world object b2. The indices b1 and b2 are in this set:

b{1,2,,N,N+1}

Note that N+1 is the index of the robot base.

For example, d1,N+1 represents the minimum separation distance between the first body and base of the rigid body tree.

If a pair is in collision, the block returns the separation distance for the associated element as NaN.

If collision geometries do not exist on a body, or that pair was skipped during collision checking, the block returns the separation distance for the associated element as Inf.

Witness points between the robot bodies including the base, returned as a 3(N+1)-by-2(N+1) matrix, where N is the number of bodies. Witness points are the points on any two bodies that are closest to one another for a given configuration. The matrix takes the form:

[Wit1,1Wit1,2Wit1,NWit1,N+1Wit2,1Wit2,2Wit2,NWit2,N+1WitN,1WitN,2WitN,NWitN,N+1WitN+1,1WitN+1,2WitN+1,NWitN+1,N+1]

Each element Witb1,b2 is a submatrix containing the witness points between the body b1 and the body b2.

Witb1,b2=[xb1xb2yb1yb2zb1zb2]

The indices b1 and b2 are in the set:

b{1,2,,N,N+1}

Note that N+1 is the index of the robot base.

For example, Wit1,N+1 is the submatrix containing the witness points between the first body and base of the rigid body tree.

If a pair is in collision, the block returns the witness points for that element as NaN(3,2).

If collision geometries do not exist on a body or that pair was skipped during collision checking, the block returns the witness points for that element as Inf(3,2).

Rigid body tree configuration is in collision with world objects, returned as a Boolean scalar. A true value indicates that the rigid body tree is in collision with one or more world objects at the specified configuration. A false value indicates that the rigid body tree is not in collision with any world objects at the specified configuration.

Minimum separation distance between the rigid body tree bodies and the collision objects, returned as an (N+1)-by-W matrix. N is the number of bodies and W is the number of world objects. The first N rows correspond to the robot bodies, while row (N+1) corresponds to the base.

The matrix takes the form:

[d1,1d1,2d1,Wd2,1d2,2d2,WdN,1dN,2dN,WdN+1,1dN+1,2dN+1,W]

Each element db,w is a the minimum separation distance between the body b and the world object w. The indices b and w are in these sets:

b{1,2,,N,N+1}w{1,2,,W}

Note that N+1 is the index of the robot base.

For example, dN+1,3 is the minimum separation distance between the base of the rigid body tree and the third world collision object.

If a pair is in collision, the block returns the separation distance for the associated element as NaN.

If collision geometries do not exist on a body, the block returns the separation distance for the associated element as Inf.

Witness points between the rigid body tree and world collision objects, specified as a 3(N+1)-by-2(W) matrix, where N is the number of bodies and W is the number of world objects. Witness points are the points on any two bodies that are closest to one another for a given configuration. The matrix takes the form:

[Wit1,1Wit1,2Wit1,WWit2,1Wit2,2Wit2,WWitN,1WitN,2WitN,WWitN+1,1WitN+1,2WitN+1,W]

Each element Witb,w is a submatrix containing the witness points between the body b and the world object w.

Witb,w=[xbxwybywzbzw]

The indices b and w are in these sets:

b{1,2,,N,N+1}w{1,2,,W}

Note that N+1 is the index of the robot base.

For example, WN+1,3 is the submatrix containing the witness points between the base of the rigid body tree robot model and the third world collision object.

If a pair is in collision, the block returns the witness points for that element as NaN(3,2).

If collision geometries do not exist on a body, the block returns the witness points for that element as Inf(3,2).

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 the sample time of the block as a positive scalar, in seconds.

If you set the sample time to -1, the block inherits its sample time from the model.

  • 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).

Extended Capabilities

expand all

Version History

Introduced in R2026a