Main Content

checkCollision

Check collision between robot platform and target bodies

Since R2023a

    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.

    example

    Examples

    collapse all

    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)

    Figure contains an axes object. The axes object with xlabel East (m), ylabel North (m) contains 47 objects of type patch, line. These objects represent panda_link1_coll_mesh, panda_link2_coll_mesh, panda_link3_coll_mesh, panda_link4_coll_mesh, panda_link5_coll_mesh, panda_link6_coll_mesh, panda_link7_coll_mesh, panda_hand_coll_mesh, panda_leftfinger_coll_mesh, panda_rightfinger_coll_mesh, panda_link0_coll_mesh.

    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

    collapse all

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

    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"

    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

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

    Example: IgnoreSelfCollision="on"

    Data Types: char | string

    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

    collapse all

    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

    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

    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:

    [Wr1_1Wr1_2Wr1_(N)Wr2_1Wr2_2Wr2_(N)Wr(N+1)_1Wr(N+1)_2Wr(N+1)_(N)Wr1_(N+1)Wr2_(N+1)Wr(N+1)_(N+1)]

    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