Create Collision Objects for Manipulator Collision Checking

Open Live Script

This example shows the three different ways to create collision objects for manipulator collision checking. To see more in-depth examples that check for self-collisions or environment collision detection, try the following examples:

Overview of Collision Checking for Manipulators

The checkCollisions function is generic to any application or specific mesh configuration. This means:

To check for collisions, you need to represent each collision object as a primitive or mesh, and specify the pose.
For manipulators, the poses can be obtained from the rigid body tree, but the poses must be assigned to the collision object.

This example shows two example helpers that automate this process for building a cell array for storing the collision objects and their poses. The options are:

Rigid body collision array: cell array of 1x2 cells, where each cell has the collision primitive corresponding to the ith body in the vector [base robot.Bodies] and the transform that relates that collision object's origin to the associated rigid body joint.
World collision array: cell array of world collision objects.

There are three suggested ways to initialize the rigid body collision array:

Using meshes imported from a known directory.
Extracting the meshes from a rigidBodyTree object.
Using collision primitives to represent your geometry.

Method 1: Use Meshes in a Known Directory

Many robots come with collision meshes specified in their Unified Robot Definition Format (URDF) file.

The IIWA robot comes with a set of collision meshes, which are simplified versions of the visualization meshes. Call importrobot to generate a rigidBodyTree object from the URDF file. Set the output format for configurations to "column".

iiwa = importrobot('iiwa14.urdf');
iiwa.DataFormat = 'column';

Create Collision-Checking Tools using Collision Meshes

Once the rigid body tree has been created, generate a set of collision meshes for each link of the rigid body tree. This will be achieved by generating an cell array of n+1 for the n bodies in the rigidBodyTree object. The extra element stores the collision object for the base.

collisionArrayFromMesh = cell(iiwa.NumBodies+1, 2);

Create a rigidBodyTree object and specify the path of the folder containing the collision meshes. This syntax associated the rigid body objects with the STL mesh files based on their specified name.

% Create a rigid body tree using the collision mesh path to associate rigid
% bodies with collision mesh STL files
collisionMeshPath = fullfile(matlabroot, 'toolbox', 'robotics', ...
        'robotexamples', 'robotmanip', 'data', 'iiwa_description', ...
        'meshes', 'iiwa14', 'collision');
iiwaCollision = importrobot('iiwa14.urdf','MeshPath', collisionMeshPath);
iiwaCollision.DataFormat = 'column';

Create the collision mesh definitions for each rigid body from the source directory:

Convert each STL file to faces and vertices by reading the STL: stldata = stlread(file.STL)
Create a collisionMesh from the vertices: mesh = collisionMesh(stldata.Points);
Assign the mesh to the ith body by placing it in the (i+1)th element of the collisionArray cell array. The base mesh occupies the first element of the cell array.

% For each body, read the corresponding STL file
robotBodies = [{iiwaCollision.Base} iiwaCollision.Bodies];
for i = 1:numel(robotBodies)
    if ~isempty(robotBodies{i}.Visuals)
        % Assumes the first Visuals element is the correct one.
        visualDetails = robotBodies{i}.Visuals{1};
        
        % Extract the part of the visual that actually specifies the STL name
        visualParts = strsplit(visualDetails, ':');
        stlFileName = visualParts{2};
        
        % Read the STL file
        stlData = stlread(fullfile(collisionMeshPath, stlFileName));
        
        % Create a collisionMesh object from the vertices
        collisionArrayFromMesh{i,1} = collisionMesh(stlData.Points);
        
        % Transform is always identity
        collisionArrayFromMesh{i,2} = eye(4);
    end
end

Check for Self-Collisions at a Specified Configuration

Given an array of collision meshes, the provided exampleHelperManipCheckCollisions function iterates through all the bodies to check for collisions and returns the index of the colliding pair.

config = [0 -pi/4 pi 0.9*pi 0 -pi/2 0]';
[isCollision, selfCollisionPairIdx] = exampleHelperManipCheckCollisions(iiwaCollision, collisionArrayFromMesh, {}, config, true);
disp(isCollision)

Visualize the robot configuration and highlight colliding bodies using exampleHelperHighlightCollisionBodies.

show(iiwa, config);
exampleHelperHighlightCollisionBodies(iiwaCollision, selfCollisionPairIdx, gca);

Method 2: Extract the Meshes from the Rigid Body Tree

Given a rigid body tree with combined visual and collision meshes, create meshes from the Visuals property of each rigid body in the tree. Use createCollisionArray from the exampleHelperManipCollisionsFromVisuals class. The helper function makes the assumption that in cases of multiple visuals, the first one should be used. If there is no visual for a specific body, the associated cell array is left empty.

collisionArrayFromVisuals = exampleHelperManipCollisionsFromVisuals(iiwa);

config = [0 -pi/4 pi 0.9*pi 0 -pi/2 0]';
[isCollision, selfCollisionPairIdx] = exampleHelperManipCheckCollisions(iiwa, collisionArrayFromVisuals, {}, config, true);
disp(isCollision)

Visualize the robot and highlight the objects in collision.

show(iiwa,config);
exampleHelperHighlightCollisionBodies(iiwa, selfCollisionPairIdx, gca);

Method 3: Define Collision Primitives

Create collision primitives that use simplified geometry. Manually specify the dimensions. This method is generally less accurate than using higher fidelity mesh definitions.

For this example, use collisionCylinder objects, which have a height and radius. Specify each rigid body size as an array.

dimensionArray = [...
    .1 0; ... % Base
    .15, 0.1575; ... % iiwa_link_0
    .12, 0.3; ... % iiwa_link_1
    .13, 0.3; ... % iiwa_link_2
    .1, 0.3; ... % iiwa_link_3
    .1, 0.25; ... % iiwa_link_4
    .1, 0.2155; ... % iiwa_link_5
    .08, 0.17; ... % iiwa_link_6
    .05, 0.06; ... % iiwa_link_7
    .01, 0; ... % iiwa_link_ee_kuka
    .01, 0;]; % iiwa_link_ee

Build the collision array from the given dimensions. Create a collisionCylinder object and specify the transformation between each based on their geometries.

primitiveCollisionArray = { ...
    [] eye(4); ... %Base (world)
    collisionCylinder(dimensionArray(2,1), dimensionArray(2,2)) trvec2tform([0 0 dimensionArray(2,2)/2]); ... % iiwa_link_0
    collisionCylinder(dimensionArray(3,1), dimensionArray(3,2)) trvec2tform([0 0 dimensionArray(3,2)/2]); ... % iiwa_link_1
    collisionCylinder(dimensionArray(4,1), dimensionArray(4,2)) axang2tform([1 0 0 -pi/2])*trvec2tform([0 0 dimensionArray(4,2)/2]); ... % iiwa_link_2
    collisionCylinder(dimensionArray(5,1), dimensionArray(5,2)) trvec2tform([0 .025 dimensionArray(5,2)/2]); ... % iiwa_link_3
    collisionCylinder(dimensionArray(6,1), dimensionArray(6,2)) axang2tform([1 0 0 -pi/2])*trvec2tform([0 -.02 dimensionArray(6,2)/2]); ... % iiwa_link_4
    collisionCylinder(dimensionArray(7,1), dimensionArray(7,2)) trvec2tform([0 0 dimensionArray(7,2)/2]); ... % iiwa_link_5
    collisionCylinder(dimensionArray(8,1), dimensionArray(8,2)) axang2tform([1 0 0 -pi/2]); ... % iiwa_link_6
    collisionCylinder(dimensionArray(9,1), dimensionArray(9,2)) trvec2tform([0 0 dimensionArray(9,2)/2]); ... % iiwa_link_7
    [] eye(4); ... % iiwa_link_ee_kuka
    [] eye(4); ... % iiwa_link_ee
    };

Check for collisions. Since these are less precise, collision-checking returns more collisions in this instance.

config = [0 -pi/4 pi 0.9*pi 0 -pi/2 0]';
show(iiwa, config);
[isCollision, selfCollisionPairIdx] = exampleHelperManipCheckCollisions(iiwa, primitiveCollisionArray, {}, config, true);
exampleHelperHighlightCollisionBodies(iiwa, selfCollisionPairIdx, gca);

Visualize the collision primitives to show they are less accurate.

exampleHelperShowCollisionTree(iiwa, primitiveCollisionArray, config);
title('Collision Array from Collision Primitives')

Documentation