How to check collision between several robots?

I have created four robots (rigidBodyTrees) with the Robotics Toolbox.

In reality, this is one robot with four arms.

I chose to create four different robots, because I want to ignore self collision (the self being the same arm).

I want to generate configurations for each arm (each robot) and then check whether there is collision between them.

How do I do this? The documentation says:

checkCollision(robot,config,worldObjects)

This suggests that I can only enter the configuration for one robot and the worldObjects cannot be placed depending on the configuration that is generated but are 'fixed'.

I really hope someone can help me!

[Karsh Tharyani]

Hi Eva,

Thanks for your question. You can consider modelling your four-arms robot as a single rigidBodyTree with four sub-trees.

Next, if you feel like you want to ignore self collisions, you can set the "IgnoreSelfCollisions" to "on".

Let me know if this helps.

Best,

Karsh

(Edit)

I think I might have misunderstood your question. Sounds like (and correct me if I am wrong), you want to find out the collision between bodies of two arms, but not of bodies within an arm. Based on your current approach, you would have to update the poses of the world objects (which will correspond to links on the other arms) using "getTransform".

Alternatively, you can model your robot as, like I mentioned above, a four sub-trees "rigidBodyTree", and use "checkCollision" for a configuration and set "Exhaustive" "on".

[isColliding, separationDistance, witnessPts] = checkCollision(robot, config, "Exhaustive", "on");

Next, you can deduce based on the "separationDistance" (see here) values to decide whether the configuration is in collision or not. For example, if the (1,3) entry of "separationDistance" is NaN and that bodies at indices 1 and 3 form the same arm, this is not a collision and this can be filtered out.

[Eva]

Thank you for your answer! I used getTransform and I think this worked. However, something in checkCollision is not working. In my code, I create a lot of random configurations and for each one I check the collision with the robot being one of the arms and the worldObjects being the rest of the arms. I check this for each of the arms. However, when I show one without collision, I still see that collision occurs between the arms (which I modeled as different robots) (see picture). I am sure that I am showing the correct configuration, which I tested with the homeConfiguration. I also find that sometimes there is collision for 1 out of 4 robot arms being used as the 'checkCollision-for-robot-arm'. Since I ignore self collision, this should not be possible; if arm 1 collides with arm 2, arms 2 also collides with arm 1 and thus the collision count should be 2.

To check collision, I use:

isColliding = checkCollision(robot,config,worldCollisionArray,'IgnoreSelfCollision','on')

For showing the robot, which only consists of collisionCylinders and collisionBoxes, I use:

show(robot,config,'Collisions','on','Visuals','off','Frames','on')

Is there a bug in the checkCollision function or am I doing something wrong?

[Yiping Liu]

Can you share your script so we can take a detailed look?

[Karsh Tharyani]

Hi Eva,

Please reach out to Technical Support with your question. You can provide them with the supporting files that reproduce the issue.

Best,

Karsh