Check for Environmental Collisions with Manipulators

Generate a collision-free trajectory in a constrained workspace. This example shows users how to inspect trajectories for collisions and manually design trajectories. An alternative is to use a motion planner as in Pick and Place Using RRT for Manipulators.

Define Collision Environment

Create a simple environment using collision primitives. This example creates a scene where a robot is in a workspace and has to move objects from one table to another. The robot must also avoid a circular light fixture above the workspace. Model the tables as two boxes and a sphere and specify their pose in the world. More complex environments can be created using collisionMesh objects.

% Create two platforms
platform1 = collisionBox(0.5,0.5,0.25);
platform1.Pose = trvec2tform([-0.5 0.4 0.2]);

platform2 = collisionBox(0.5,0.5,0.25);
platform2.Pose = trvec2tform([0.5 0.2 0.2]);

% Add a light fixture, modeled as a sphere
lightFixture = collisionSphere(0.1);
lightFixture.Pose = trvec2tform([.2 0 1]);

% Store in a cell array for collision-checking
worldCollisionArray = {platform1 platform2 lightFixture};

Visualize the environment using a helper function that iterates through the collision array.

ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);

Add Robot To Environment

Load a Kinova manipulator model as a rigidBodyTree object using the loadrobot function.

robot = loadrobot("kinovaGen3","DataFormat","column","Gravity",[0 0 -9.81]);

Show the robot in the environment using the same axes as the collision objects. The robot base is fixed to the origin of the world.

show(robot,homeConfiguration(robot),"Parent",ax);

Generate a trajectory and check for collisions

Define a starting joint configuration above platform 1 and an end joint configuration above platform 2.

startConfig = [3.8906    1.1924    0.0000    0.0000    0.0001    1.9454    0.7491]';
endConfig = [-0.9240    1.2703    1.9865    1.2394    1.7457   -2.0500    0.4222]';

% Show initial and final positions
show(robot,startConfig);
show(robot,endConfig);

Use a trapezoidal velocity profile to generate a smooth trajectory from the home position to the start position, and then to the end position.

q = trapveltraj([homeConfiguration(robot),startConfig,endConfig],200,"EndTime",2);

Check for collisions with the obstacles in the environment using the checkCollision function. Enable the IgnoreSelfCollision and Exhaustive name-value arguments. Self collisions are ignored because the robot model joint limits prevent most self collisions. Exhaustive checking ensures the function calculates all separation distances and continues searching for collisions after detecting the first collision.

The sepDist output stores the distances between robot bodies and the world collision objects as a matrix. Each row corresponds to a specific world collision object. Each column corresponds to a robot body. Values of NaN indicate a collision. Store the indexes of the collision as a cell array.

% Initialize outputs
inCollision = false(length(q), 1); % Check whether each pose is in collision
worldCollisionPairIdx = cell(length(q),1); % Provide the bodies that are in collision

for i = 1:length(q)
    
    [inCollision(i),sepDist] = checkCollision(robot,q(:,i),worldCollisionArray,"IgnoreSelfCollision","on","Exhaustive","on","SkippedSelfCollisions","parent");
    
    [bodyIdx,worldCollisionObjIdx] = find(isnan(sepDist)); % Find collision pairs
    worldCollidingPairs = [bodyIdx,worldCollisionObjIdx]; 
    worldCollisionPairIdx{i} = worldCollidingPairs;
    
end
isTrajectoryInCollision = any(inCollision)

isTrajectoryInCollision = logical
   1

Inspect Detected Collisions

From the last step, two collisions are detected. Visualize these configurations to investigate further. Use the exampleHelperHighlightCollisionBodies function to highlight bodies based on the indices. You can see a collision occurs at the sphere and the table.

collidingIdx1 = find(inCollision,1);
collidingIdx2 = find(inCollision,1,"last");

% Identify the colliding rigid bodies.
collidingBodies1 = worldCollisionPairIdx{collidingIdx1}*[1 0]';
collidingBodies2 = worldCollisionPairIdx{collidingIdx2}*[1 0]';

% Visualize the environment.
ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);

% Add the robotconfigurations & highlight the colliding bodies.
show(robot,q(:,collidingIdx1),"Parent",ax,"PreservePlot",false);
exampleHelperHighlightCollisionBodies(robot,collidingBodies1 + 1,ax);
show(robot,q(:,collidingIdx2),"Parent"',ax);
exampleHelperHighlightCollisionBodies(robot,collidingBodies2 + 1,ax);

Generate Collision-Free Trajectory

To avoid these collisions, add intermediate joint configurations to ensure the robot navigates around the obstacle.

intermediateConfig1 = [-0.3644   -1.8365    0.0430    1.6606    0.2889    0.9386    0.1271]';
intermediateConfig2 =[-1.8895    0.6716    2.2225    1.4608    2.5503   -2.0500    0.1536]';

% Show the new intermediate poses
ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);
show(robot,intermediateConfig1,"Parent",ax,"PreservePlot",false);
show(robot,intermediateConfig2,"Parent",ax);

Generate a new trajectory.

[q,qd,qdd,t] = trapveltraj([homeConfiguration(robot),intermediateConfig1,startConfig,intermediateConfig2,endConfig],200,"EndTime",2);

Verify that it is collision-free.

%Initialize outputs
inCollision = false(length(q),1); % Check whether each pose is in collision
for i = 1:length(q)
    inCollision(i) = checkCollision(robot,q(:,i),worldCollisionArray,"IgnoreSelfCollision","on","SkippedSelfCollisions","parent");
end
isTrajectoryInCollision = any(inCollision)

isTrajectoryInCollision = logical
   0

Visualize the Generated Trajectory

Animate the result.

% Plot the environment
ax2 = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);

% Visualize the robot in its home configuration
show(robot,startConfig,"Parent",ax2);

% Update the axis size
axis equal

% Loop through the other positions
for i = 1:length(q)
    show(robot,q(:,i),"Parent",ax2,"PreservePlot",false);
    
    % Update the figure    
    drawnow
end

Plot the joint positions over time.

figure
plot(t,q)
xlabel("Time")
ylabel("Joint Position")

×

Open Example

You have a modified version of this example. Do you want to open this example with your edits?

No, overwrite the modified version Yes