Main Content

Check for Environmental Collisions with Manipulators

Generate a collision-free trajectory in a constrained workspace.

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

Figure contains an axes. The axes contains 3 objects of type patch.

Add Robot To SEnvironment

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

Figure contains an axes. The axes contains 28 objects of type patch, line. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh.

Generate a trajectory and check for collisions

Define a start and end pose using position and orientation vectors that are combined using transformation matrix multiplication.

startPose = trvec2tform([-0.5,0.5,0.4])*axang2tform([1 0 0 pi]);
endPose = trvec2tform([0.5,0.2,0.4])*axang2tform([1 0 0 pi]);

Use inverseKinematics to solve for the joint positions based on the desired poses. Inspect manually to verify that the configurations are valid.

% Use a fixed random seed to ensure repeatable results
rng(0);
ik = inverseKinematics("RigidBodyTree",robot);
weights = ones(1,6);
startConfig = ik("EndEffector_Link",startPose,weights,robot.homeConfiguration);
endConfig = ik("EndEffector_Link",endPose,weights,robot.homeConfiguration);

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

Figure contains an axes. The axes contains 78 objects of type patch, line. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh.

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 nad Exhaustivename-value arguments. Self collisions are ignored because the robot model joint limits prevent most self collisions. Exhausitive checking ensures the function calculates all seperation 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");
    
    [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);

Figure contains an axes. The axes contains 53 objects of type patch, line. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh.

Generate Collision-Free Trajectory

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

intermediatePose1 = trvec2tform([-.3 -.2 .6])*axang2tform([0 1 0 -pi/4]); % Out and around the sphere
intermediatePose2 = trvec2tform([0.2,0.2,0.6])*axang2tform([1 0 0 pi]); % Come in from above

intermediateConfig1 = ik("EndEffector_Link",intermediatePose1,weights,q(:,collidingIdx1));
intermediateConfig2 = ik("EndEffector_Link",intermediatePose2,weights,q(:,collidingIdx2));

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

Figure contains an axes. The axes contains 53 objects of type patch, line. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh.

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

Figure contains an axes. The axes contains 28 objects of type patch, line. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh.

Plot the joint positions over time.

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

Figure contains an axes. The axes contains 7 objects of type line.