Rigid Body Tree Check Collision

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

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

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

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
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 theshowdetailsobject function of therigidBodyTreeobject 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
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:
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:
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:
Each element Witb1,b2 is a submatrix containing the witness points between the body b1 and the body b2.
The indices b1 and b2 are in the set:
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:
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:
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:
Each element Witb,w is a submatrix containing the witness points between the body b and the world object w.
The indices b and w are in these sets:
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
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
The Rigid Body Tree Check Collision block supports code generation with dynamic memory allocation disabled. For more information about disabling dynamic memory allocation, see Dynamic memory allocation in MATLAB functions (Simulink).
Note
To deploy code to real-time targets containing this block using Embedded Coder®, you must select the variable-size signals parameter in the Configuration Parameters dialog box.

Version History
Introduced in R2026a
See Also
MATLAB Command
You clicked a link that corresponds to this MATLAB command:
Run the command by entering it in the MATLAB Command Window. Web browsers do not support MATLAB commands.
选择网站
选择网站以获取翻译的可用内容,以及查看当地活动和优惠。根据您的位置,我们建议您选择:。
您也可以从以下列表中选择网站:
如何获得最佳网站性能
选择中国网站(中文或英文)以获得最佳网站性能。其他 MathWorks 国家/地区网站并未针对您所在位置的访问进行优化。
美洲
- América Latina (Español)
- Canada (English)
- United States (English)
欧洲
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)