checkCollision

Check collision between robot platform and target bodies

Since R2023a

Syntax

[iscolliding,separationdist,witnesspts] = checkCollision(platform,targetbody)

[___] = checkCollision(___,Name=Value)

Description

[iscolliding,separationdist,witnesspts] = checkCollision(platform,targetbody) checks for collision between the rigidBodyTree-based robot platform platform and a set of target bodies targetbody.

[___] = checkCollision(___,Name=Value) specifies options using one or more name-value arguments, in addition to all arguments from the previous syntax.

Examples

Perform Pick and Place in Robot Scenario

Create a robotScenario object.

scenario = robotScenario(UpdateRate=1,StopTime=10);

Create a rigidBodyTree object of the Franka Emika Panda manipulator using loadrobot.

robotRBT = loadrobot("frankaEmikaPanda");

Create a rigidBodyTree-based robotPlatform object using the manipulator model.

robot = robotPlatform("Manipulator",scenario, ...
                      RigidBodyTree=robotRBT);

Create a non-rigidBodyTree-based robotPlatform object of a box to manipulate. Specify the mesh type and size.

box = robotPlatform("Box",scenario,Collision="mesh", ...
                    InitialBasePosition=[0.5 0.15 0.278]);
updateMesh(box,"Cuboid",Collision="mesh",Size=[0.06 0.06 0.1])

Visualize the scenario.

ax = show3D(scenario,Collisions="on");
view(79,36)
light

Specify the initial and the pick-up joint configuration of the manipulator, to move the manipulator from its initial pose to close to the box.

initialConfig = homeConfiguration(robot.RigidBodyTree);
pickUpConfig = [0.2371 -0.0200 0.0542 -2.2272 0.0013 ...
                2.2072 -0.9670 0.0400 0.0400];

Create an RRT path planner using the manipulatorRRT object, and specify the manipulator model.

planner = manipulatorRRT(robot.RigidBodyTree,scenario.CollisionMeshes);
planner.IgnoreSelfCollision = true;

Plan the path between the initial and the pick-up joint configurations. Then, to visualize the entire path, interpolate the path into small steps.

rng("default")
path = plan(planner,initialConfig,pickUpConfig);
path = interpolate(planner,path,25);

Set up the simulation.

setup(scenario)

Check the collision before manipulator picks up the box.

checkCollision(robot,"Box", ...
               IgnoreSelfCollision="on")

ans = logical
   0

Move the joints of the manipulator along the path and visualize the scenario.

helperRobotMove(path,robot,scenario,ax)

Check the collision after manipulator picks up the box.

checkCollision(robot,"Box", ...
               IgnoreSelfCollision="on")

ans = logical
   1

Use the attach function to attach the box to the gripper of the manipulator.

attach(robot,"Box","panda_hand", ...
       ChildToParentTransform=trvec2tform([0 0 0.1]))

Specify the drop-off joint configuration of the manipulator to move the manipulator from its pick-up pose to the box drop-off pose.

dropOffConfig = [-0.6564 0.2885 -0.3187 -1.5941 0.1103 ...
                 1.8678 -0.2344 0.04 0.04];

Plan the path between the pick-up and drop-off joint configurations.

path = plan(planner,pickUpConfig,dropOffConfig);
path = interpolate(planner,path,25);

Move the joints of the manipulator along the path and visualize the scenario.

helperRobotMove(path,robot,scenario,ax)

Use the detach function to detach the box from the manipulator gripper.

detach(robot)

Plan the path between the drop-off and initial joint configurations to move the manipulator from its box drop-off pose to its initial pose.

path = plan(planner,dropOffConfig,initialConfig);
path = interpolate(planner,path,25);

Move the joints of the manipulator along the path and visualize the scenario.

helperRobotMove(path,robot,scenario,ax)

Helper function to move the joints of the manipulator.

function helperRobotMove(path,robot,scenario,ax)
    for idx = 1:size(path,1)
        jointConfig = path(idx,:);
        move(robot,"joint",jointConfig)
        show3D(scenario,fastUpdate=true,Parent=ax,Collisions="on");
        drawnow
        advance(scenario);
    end
end

Input Arguments

platform — rigidBodyTree-based robot platform

robotPlatform object

rigidBodyTree-based robot platform, specified as a robotPlatform object.

targetbody — Non-rigidBodyTree-based target platforms

string scalar | character vector | string array | cell array of character vectors

Non-rigidBodyTree-based target platforms, specified as a string scalar, character vector, string array, or cell array of character vectors.

Example: "Box"

Example: 'Box'

Example: ["Box","Can"]

Example: {'Box','Can'}

Data Types: char | string

Name-Value Arguments

Specify optional pairs of arguments as Name1=Value1,...,NameN=ValueN, where Name is the argument name and Value is the corresponding value. Name-value arguments must appear after other arguments, but the order of the pairs does not matter.

Example: Exhaustive="on"

Exhaustive — Exhaustively check for all collisions

"off" (default) | "on"

Exhaustively check for all collisions, specified as "on" or "off". Setting this to "off" interrupts collision checking when the first collision is found. If returned early, separationdist and witnesspts values for incomplete checks are Inf. Setting this to "on" computes the separation distance and witness points for all pairs.

Example: Exhaustive="on"

Data Types: char | string

IgnoreSelfCollision — Skip checking for collisions between robot bodies and base

"off" (default) | "on"

Skip checking for collisions between robot bodies and the base, specified as "on" or "off".

Example: IgnoreSelfCollision="on"

Data Types: char | string

SkippedSelfCollisions — Body pairs skipped for checking self-collisions

"parent" (default) | "adjacent"

Body pairs skipped for checking self-collisions, specified as "parent" or "adjacent".

• "parent" — Skips collision checking between child and parent bodies.

• "adjacent" — Skips collision checking between bodies on adjacent indices.

Example: SkippedSelfCollisions="adjacent"

Data Types: char | string

Output Arguments

iscolliding — Indicator for self-collision and collision with target bodies

two-element logical vector | logical scalar

Indicator for self-collision and collision with target bodies, returned as a two-element logical vector or logical scalar. If IgnoreSelfCollision is set to "on", then iscolliding is a logical scalar that indicates whether the robot platform collides with target bodies.

Data Types: logical

separationdist — Separation distance between target bodies and robot platform bodies including base

(N+1)-by-(N+M+1) matrix | (N+1)-by-M matrix

Separation distance between target bodies and the robot platform bodies including the base, returned as a (N+1)-by-(N+M+1) matrix. N is the number of robot platform bodies. M is the number of target bodies. If IgnoreSelfCollision is set to "on", the separationdist is an (N+1)-by-M matrix with column indices corresponding to the target bodies.

Returns Inf for incomplete checks.

Data Types: double

witnesspts — Witness points between bodies of robot platform including base

3(N+1)-by-2(N+1) matrix

Witness points between bodies of the robot platform including the base, returned as a 3(N+1)-by-2(N+1) matrix. N is the number of bodies. The matrix has the format:

$$\left[\begin{array}{cc}\begin{array}{cccc}W{r}_{1\_1}& W{r}_{1\_2}& \cdots & W{r}_{1\_(N)}\\ W{r}_{2\_1}& W{r}_{2\_2}& \cdots & W{r}_{2\_(N)}\\ ⋮& ⋮& ⋮& ⋮\\ W{r}_{(N+1)\_1}& W{r}_{(N+1)\_2}& \cdots & W{r}_{(N+1)\_(N)}\end{array}& \begin{array}{c}W{r}_{1\_(N+1)}\\ W{r}_{2\_(N+1)}\\ ⋮\\ W{r}_{(N+1)\_(N+1)}\end{array}\end{array}\right]$$

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

Returns Inf for incomplete checks.

Data Types: double

Version History

Introduced in R2023a