checkCollision

Check if robot is in collision

collapse all in page

Syntax

[isSelfColliding,selfSeparationDist,selfWitnessPts] = checkCollision(robot,config)
[isColliding,separationDist,witnessPts] = checkCollision(robot,config,worldObjects)
[___] = checkCollision(___,Name,Value)

Description

example

[isSelfColliding,selfSeparationDist,selfWitnessPts] = checkCollision(robot,config) checks if the specified rigid body tree robot model robot is in self-collision at the specified configuration config. Add collision objects to the rigid body tree robot model using the addCollision function. The checkCollision function also returns the closest separation distance selfSeparationDist and the witness points selfWitnessPts as points on each body.

The function ignores adjacent bodies when checking for self-collisions.

[isColliding,separationDist,witnessPts] = checkCollision(robot,config,worldObjects) checks if the specified rigid body tree robot model is in collision with itself or a specified set of collision objects in the world worldObjects.

[___] = checkCollision(___,Name,Value) specifies additional options using one or more name-value pair arguments in addition to any of argument combinations from previous syntaxes.

Examples

collapse all

Open Live Script

Load a robot model and modify the collision meshes. Clear existing collision meshes, add simple collision object primitives, and check whether certain configurations are in collision.

Load Robot Model

Load a preconfigured robot model into the workspace using the loadrobot function. This model already has collision meshes specified for each body. Iterate through all the rigid body elements and clear the existing collision meshes. Confirm that the existing meshes are gone.

robot = loadrobot('kukaIiwa7','DataFormat','column');

for i = 1:robot.NumBodies
    clearCollision(robot.Bodies{i})
end

show(robot,'Collisions','on','Visuals','off');

Add Collision Cylinders

Iteratively add a collision cylinder to each body. Skip some bodies for this specific model, as they overlap and always collide with the end effector (body 10).

collisionObj = collisionCylinder(0.05,0.25);

for i = 1:robot.NumBodies
    if i > 6 && i < 10
        % Skip these bodies.
    else
        addCollision(robot.Bodies{i},collisionObj)
    end
end

show(robot,'Collisions','on','Visuals','off');

Check for Collisions

Generate a series of random configurations. Check whether the robot is in collision at each configuration. Visualize each configuration that has a collision.

figure
rng(0) % Set random seed for repeatability.
for i = 1:20
    config = randomConfiguration(robot);
    isColliding = checkCollision(robot,config);
    if isColliding
        show(robot,config,'Collisions','on','Visuals','off');
        title('Collision Detected')
    else
        % Skip non-collisions.
    end
end

Input Arguments

collapse all

Rigid body tree robot model, specified as a rigidBodyTree object. To use the checkCollision function, the DataFormat property of the rigidBodyTree object must be either 'row' or 'column'.

Joint configuration of the rigid body tree, specified as an n-element numeric vector, where n is the number of nonfixed joints in the robot model. Each element of the vector is a specific joint position for a joint in the robot model.

Data Types: single | double

List of collision objects in the world, specified as a cell array of collision objects with any combination of collisionBox, collisionCylinder, collisionSphere, and collisionMesh objects. The function assumes that the Pose property of each object is relative to the base of the rigid body tree robot model.

Name-Value Pair Arguments

Specify optional comma-separated pairs of Name,Value arguments. Name is the argument name and Value is the corresponding value. Name must appear inside quotes. You can specify several name and value pair arguments in any order as Name1,Value1,...,NameN,ValueN.

Example: 'Exhaustive','on' enables exhaustive checking for collisions and causes the function to calculate all separation distances and witness points.

Exhaustively check for all collisions, specified as the comma-separated pair consisting of 'Exhaustive' and 'on' or 'off'. By default, the function finds the first collision and stops, returning the separation distances and witness points for incomplete checks as Inf.

If this name-value pair argument is specified as 'on', the function instead continues checking for collisions until it has exhausted all possibilities.

Data Types: char | string

Skip checking for robot self-collisions,, specified as the comma-separated pair consisting of 'IgnoreSelfCollision' and 'on' or 'off'. When this argument is enabled, the function ignores collisions between the collision objects of the rigid body tree robot model bodies and other collision objects of the same model or its base.

This name-value pair argument affects the size of the separationDist and witnessPts output arguments.

Data Types: char | string

Output Arguments

collapse all

Self Collisions

Robot configuration is in self-collision returned as a logical 1 (true) or 0 (false). If the function returns a value of true for this argument, that means that one of the rigid body collision objects is touching another collision object in the robot model. Add collision objects to your rigid body tree robot model using the addCollision function.

Data Types: logical

Minimum separation distance between the bodies of the robot, returned as an (m+1) -by-(m+1) matrix, where m is the number of bodies. The final row and column correspond to the robot base. Units are in meters.

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

Data Types: double

Witness points between the robot bodies including the base, returned as an 3(m+1)-by-2(m+1) matrix, where m 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:

The matrix is divided into 3-by-2 sections that represent the xyz-coordinates of witness point pairs in the form:

[x1 x2 y1 y2 z1 z2](1)

Each section corresponds to a separation distance in the selfSeparationDist output matrix. Use these equations to determine where the section of the selfWitnessPts matrix that corresponds to a specific separation distance begins:

Wr = 3Sr – 2(2)
Wc = 2Sc – 1 (3)

Where (Sr,Sc) is the index of a separation distance in the separation distance matrix and (Wr,Wc) is the index in the witness point matrix at which the corresponding witness points begin.

If a pair is in collision, the function returns each coordinate of the witness points for that element as NaN.

Data Types: double

World Collisions

Robot configuration is in collision, returned as a two-element logical vector. The first element indicates whether the robot is in self-collision. The second element indicates whether the robot model is in collision with any world objects.

Data Types: logical

Minimum separation distance between the collision objected, returned as an (m+w+1)-by-(m+w+1) matrix, where m is the number of bodies and w is the number of world objects. The final row and column correspond to the robot base.

The matrix is divided into 3-by-2 sections that represent the xyz-coordinates of witness point pairs in the form:

[x1 x2 y1 y2 z1 z2](4)

Each section corresponds to a separation distance in the separationDist output matrix. Use these equations to determine where the section of the witnessPts matrix that corresponds to a specific separation distance begins:

Wr = 3Sr – 2(5)
Wc = 2Sc – 1 (6)

Where (Sr,Sc) is the index of a separation distance in the separation distance matrix and (Wr,Wc) is the index in the witness point matrix at which the corresponding witness points begin.

If a pair is in collision, the function returns each coordinate of the witness points for that element as NaN.

If a pair is in collision, the function returns the separation distance as NaN.

Dependencies

If you specify the 'IgnoreSelfCollision' name-value pair argument as 'on', then the matrix does not contain values for the distances between any given body and other bodies in the robot model.

Data Types: double

Witness points between collision objects, specified as a 3(m+w+1)-by-2(m+w+1) matrix, where m 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:

[Wr1_1       Wr1_2     ...    Wr1_(N+1)     Wo1_1     Wo1_2      ... W1_M;
 Wr2_1       Wr2_2     ...    Wr2_(N+1)     Wo2_1     Wo2_2      ... W2_M;
 .           .         .      .             .         .          .   .
 .           .         .      .             .         .          .   .
 .           .         .      .             .         .          .   .
 Wr(N+1)_1   Wr(N+1)_2 ...    Wr(N+1)_(N+1) Wo(N+1)_1 Wo(N+1)_2  ... W(N+1)_M]

Each element in the above matrix is a 2-by-3 matrix that gives the nearest [x y z] points on the two corresponding bodies or world objects. The final row and column correspond to the robot base.

If a pair are in collision, witness points for that element are returned as NaN(3,2).

Dependencies

If the "IgnoreSelfCollision" name-value pair is set to "on", then the matrix contains no Wr elements.

Data Types: double

See Also

| |

Introduced in R2020b

Robotics System Toolbox Documentation

Support