Main Content

capsuleApproximation

Approximate collision geometries of rigid body tree with capsules

    Description

    The capsuleApproximation object approximates the collision geometries associated with every body of a rigidBodyTree object by fitting collision capsules on each rigid body. This object enables you to query, modify, and visualize the collision capsules associated with rigid bodies of a rigid body tree.

    Creation

    Description

    example

    capapprox = capsuleApproximation(robot) creates a capsule approximation capapprox of the input rigid body tree robot.

    Input Arguments

    expand all

    Rigid body tree robot model to approximate with capsules, specified as a rigidBodyTree object. To use a provided robot model, see loadrobot. To import Unified Robot Description Format (URDF) models, see the importrobot function.

    Properties

    expand all

    Capsule-approximated rigid body tree robot model, stored as a rigidBodyTree object.

    Object Functions

    addCapsuleAdd collision capsule to rigid body
    removeCapsuleRemove collision capsule from rigid body
    getCapsulesGet collision capsules of rigid body
    showVisualize capsule approximation of rigid body tree
    updateGeometryUpdate geometry of collision capsule of rigid body
    updatePoseUpdate pose of collision capsule of rigid body

    Examples

    collapse all

    Load a robot into the workspace and visualize it.

    robot = loadrobot("universalUR16e");
    show(robot);

    {"String":"Figure contains an axes object. The axes object contains 28 objects of type patch, line. These objects represent base_link, base, base_link_inertia, shoulder_link, upper_arm_link, forearm_link, wrist_1_link, wrist_2_link, wrist_3_link, flange, tool0, base_link_inertia_mesh, shoulder_link_mesh, upper_arm_link_mesh, forearm_link_mesh, wrist_1_link_mesh, wrist_2_link_mesh, wrist_3_link_mesh.","Tex":[],"LaTex":[]}

    Create a capsule approximation of the robot, and visualize the capsule-approximated robot model.

    capsUR16 = capsuleApproximation(robot);
    show(capsUR16,homeConfiguration(robot));

    {"String":"Figure contains an axes object. The axes object contains 35 objects of type patch, line. These objects represent base_link, base, base_link_inertia, shoulder_link, upper_arm_link, forearm_link, wrist_1_link, wrist_2_link, wrist_3_link, flange, tool0, base_link_inertia_mesh, shoulder_link_mesh, upper_arm_link_mesh, forearm_link_mesh, wrist_1_link_mesh, wrist_2_link_mesh, wrist_3_link_mesh, base_link_inertia_coll_mesh, shoulder_link_coll_mesh, upper_arm_link_coll_mesh, forearm_link_coll_mesh, wrist_1_link_coll_mesh, wrist_2_link_coll_mesh, wrist_3_link_coll_mesh.","Tex":[],"LaTex":[]}

    Load a robot into the workspace and visualize it.

    robotIRB = loadrobot("abbIrb120");
    show(robotIRB);

    {"String":"Figure contains an axes object. The axes object contains 24 objects of type patch, line. These objects represent base_link, base, link_1, link_2, link_3, link_4, link_5, link_6, tool0, link_1_mesh, link_2_mesh, link_3_mesh, link_4_mesh, link_5_mesh, link_6_mesh, base_link_mesh.","Tex":[],"LaTex":[]}

    Create a capsule approximation of the robot, and visualize the capsule-approximated robot model.

    capsIRB = capsuleApproximation(robotIRB);
    figure
    show(capsIRB,homeConfiguration(capsIRB.RigidBodyTree));

    {"String":"Figure contains an axes object. The axes object contains 31 objects of type patch, line. These objects represent base_link, base, link_1, link_2, link_3, link_4, link_5, link_6, tool0, link_1_mesh, link_2_mesh, link_3_mesh, link_4_mesh, link_5_mesh, link_6_mesh, base_link_mesh, link_1_coll_mesh, link_2_coll_mesh, link_3_coll_mesh, link_4_coll_mesh, link_5_coll_mesh, link_6_coll_mesh, base_link_coll_mesh.","Tex":[],"LaTex":[]}

    Use the getCapsules function to see if the end effector, "tool0", has any collision capsules. Because tool0 is just a frame, it has no collision mesh to approximate as a collision capsule.

    capsulesTool = getCapsules(capsIRB,"tool0")
    capsulesTool =
    
      1x0 empty cell array
    

    Add a capsule to tool0, at a position 0.15 meters along the x-axis, with a radius of 0.15 and a length of 0.

    addCapsule(capsIRB,"tool0",[0.15 0],trvec2tform([0.15 0 0]))
    show(capsIRB,homeConfiguration(capsIRB.RigidBodyTree));

    {"String":"Figure contains an axes object. The axes object contains 32 objects of type patch, line. These objects represent base_link, base, link_1, link_2, link_3, link_4, link_5, link_6, tool0, link_1_mesh, link_2_mesh, link_3_mesh, link_4_mesh, link_5_mesh, link_6_mesh, base_link_mesh, link_1_coll_mesh, link_2_coll_mesh, link_3_coll_mesh, link_4_coll_mesh, link_5_coll_mesh, link_6_coll_mesh, base_link_coll_mesh, tool0_coll_mesh.","Tex":[],"LaTex":[]}

    Again check tool0 for a collision capsule, and verify the properties of the detected capsule.

    capsulesTool = getCapsules(capsIRB,"tool0")
    capsulesTool = 1x1 cell array
        {1x1 collisionCapsule}
    
    
    capsulesTool{1}
    ans = 
      collisionCapsule with properties:
    
        Radius: 0.1500
        Length: 0
          Pose: [4x4 double]
    
    

    Remove the capsule from the base link. Then, reduce the collision capsule size of tool0, and move it -0.05 meters from the previous position along the x-axis.

    removeCapsule(capsIRB,"base_link",1)
    updatePose(capsIRB,"tool0",trvec2tform([-0.05 0 0]),1)
    updateGeometry(capsIRB,"tool0",[.1 0.01],1)
    show(capsIRB,homeConfiguration(capsIRB.RigidBodyTree));

    {"String":"Figure contains an axes object. The axes object contains 31 objects of type patch, line. These objects represent base_link, base, link_1, link_2, link_3, link_4, link_5, link_6, tool0, link_1_mesh, link_2_mesh, link_3_mesh, link_4_mesh, link_5_mesh, link_6_mesh, base_link_mesh, link_1_coll_mesh, link_2_coll_mesh, link_3_coll_mesh, link_4_coll_mesh, link_5_coll_mesh, link_6_coll_mesh, base_link_coll_mesh, tool0_coll_mesh.","Tex":[],"LaTex":[]}

    Extended Capabilities

    C/C++ Code Generation
    Generate C and C++ code using MATLAB® Coder™.

    Version History

    Introduced in R2022b