Detect a collision between 2 robotPlatform

Hello,

I imported 2 URDF files using the importrobot function. Then, I created a robotScenario by adding a robotPlatform for each robot. I want to check during the scenario that there are no collisions between the robots.

I tried using the checkCollision function between the 2 robotPlatforms, but this function does not accept non-rigidBodyTree-based platforms.

Then, I tried to create a robotPlatform 'hand' from an STL file and attach 'hand' to the 'robot2Scn' platform using the attach function, but the position of 'hand' does not update when I use the checkCollision(robotScn, 'hand', IgnoreSelfCollision="on") function. This function returns 1 when 'robotScn' is at the initial position of 'hand', otherwise it returns 0.

Is there a function or have you a suggestion to check for collisions between 2 robotPlatforms in a robotScenario?

Thank you.

robot = importrobot("robot.urdf");
robot2 = importrobot("robot_2.urdf");

scenario = robotScenario(UpdateRate=1,StopTime=10);
robotScn = robotPlatform("robot1",scenario,RigidBodyTree=robot);
robotScn2 = robotPlatform("robot2",scenario,RigidBodyTree=robot2);

% Trying to manually add a collision zone (Test with and without using clearCollision on robot2)
stlData = stlread('hand_link.stl');
vertices = stlData.Points;   
faces = stlData.ConnectivityList; 
handMesh = collisionMesh(vertices);
base = robotPlatform("hand",scenario,Collision=handMesh, initialBasePosition[0.2 0.2 0.2]);
attach(robotScn2,"hand","hand_link",ChildToParentTransform=trvec2tform([0 0 0]))
checkCollision(robotScn,"hand", IgnoreSelfCollision="on")

Answer 1

[Garmit Pant]

Hello Alexandre

From what I gather, you want to detect collisions between two robots that you are importing using “importRobot” function. As of MATLAB R2024a, there is not direct inbuilt function to check collisions between two separate robots stored as ‘RigidBodyTree’ or ‘robotPlatform’ objects. However, there are workaround workflows that can be used to detect collisions.

One way to work with multiple ‘rigidBodyTree’ objects is to combine them. You can combine multiple ‘rigidBodyTrees’ with the “addSubtree” command. Add joints or transforms as needed to ensure the spacing is correct. Once this is completed, you will have two objects in the environment -- the robots, built into one 'rigidBodyTree', and the collision meshes. You can then use the “checkCollision” function to check for self-collisions.

Another workaround is to use the “capsuleApproximation” function to approximates the collision geometries associated with every body of one of the ‘rigidBodyTree’ object by fitting collision capsules on each rigid body and then iterating over each capsule to check collisions using “checkCollision” function. The following code snippet demonstrates how to extract one capsule from one robot and check collision with another robot:

robotRBT1 = loadrobot("frankaEmikaPanda"); 
robotRBT1.DataFormat = "column" 

robotRBT1 = 
  rigidBodyTree with properties:

     NumBodies: 11
        Bodies: {1x11 cell}
          Base: [1x1 rigidBody]
     BodyNames: {1x11 cell}
      BaseName: 'panda_link0'
       Gravity: [0 0 0]
    DataFormat: 'column'

robotRBT2 = importrobot('iiwa14.urdf') 

robotRBT2 = 
  rigidBodyTree with properties:

     NumBodies: 10
        Bodies: {1x10 cell}
          Base: [1x1 rigidBody]
     BodyNames: {'iiwa_link_0'  'iiwa_link_1'  'iiwa_link_2'  'iiwa_link_3'  'iiwa_link_4'  'iiwa_link_5'  'iiwa_link_6'  'iiwa_link_7'  'iiwa_link_ee'  'iiwa_link_ee_kuka'}
      BaseName: 'world'
       Gravity: [0 0 0]
    DataFormat: 'struct'

capsRBT2 = capsuleApproximation(robotRBT2) 

capsRBT2 = 
  capsuleApproximation with properties:

    RigidBodyTree: [1x1 rigidBodyTree]

checkCollision(robotRBT1,randomConfiguration(robotRBT1),getCapsules(capsRBT2,"iiwa_link_1"),IgnoreSelfCollision="on") 

ans = logical
   1

For further understanding, kindly refer to the following MathWorks Documentation:

• Refer to the ‘Input Arguments’ section to understand the relevant input argument data types of the “checkCollision” method: https://www.mathworks.com/help/robotics/ref/rigidbodytree.checkcollision.html
• Refer to the ‘Examples’ to understand how to use the “addSubtree” function: https://www.mathworks.com/help/robotics/ref/rigidbodytree.addsubtree.html
• Refer to the ‘Object Functions’ to understand the various methods available for the “capsuleApproximation” object: https://www.mathworks.com/help/robotics/ref/capsuleapproximation.html

I hope you find the above explanation and suggestions useful!