Unexpected behaviour of checkCollision for robot collision detection

I am using the function checkCollision (R2020b version) to detect wheter a robot is in collision with itself. The basic code I use for this is:

robot = importrobot(robotPath,'MeshPath',collisionMeshPath);
isCollision = checkCollision(robot,jointAngles); % joint angles = [a1 a2 a3 a4 a5 a6]'

This function works great when the robot model does not have a tool attached. However, with a tool attached, the behaviour is not as expected anymore since it always returns true.

I need to shift the mesh of the tool away from the robot so far that no point of the robot sticks into the bottom of the tool even though there is a cut out. I have now removed the entire bottom part covering the tool and still get this problem. You can clearly see that the robot arm is far away from touching the tool but still the output is always true. In the image below, I added 18mm of vertical offset and still get the error. Once I add 19mm, no part of the robot stick into the tool and the error goes away. I want to use the function with no or very small z-offset in order to do a realistic collision detection.

Is this just the normal behaviour of this function or am I doing something wrong here? Is there maybe a workaround?

[fixusc]

Here is another strange one. It almost looks like the checkCollision function applies a certain safety margin. When I let the robot move towards the floor, the position in the attached image is about the first one to be detected as a collision, so a few mm above the actual surface.

Is there actually a safety margin built into this function and can it be controlled?

Answer 1

[Karsh Tharyani]

It will help to view the robot's collision geometry data instead of the visual data as they might be different. To do so,

show(robot, config, "Visuals", "off", "Collisions", "on");

You can also view the end-effector body's collision data by inspecting the Collisions property of the rigidBody

The next good step will be to find out which bodies are in collision. This can be done by viewing the separation distances between the bodies.

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

In order to find out which bodies are colliding, you can view the entries in the sepDist matrix which are NaNs

[b1, b2] = find(isnan(sepDist));

robot.BodyNames{b1}

robot.BodyNames{b2}

Note that the rows and columns correspond to the body indices. The last row and column corresponds to the robot.Base body's collision with other bodies.

Adjacent bodies are ignored for collisions hence their separation distances are Inf.

Once you have found out which bodies are in collision, you can modify the collision data associated with that body to make the robot self-collision free for that configuration.

I suggest reaching out to Technical Support https://www.mathworks.com/support/contact_us.html in case you still see any unexpected results

[fixusc]

Thank you very much for your help!

It was quite revealing to see the actual collision mesh being used. Actually, I already distinguish between visuals and collision geometry in my robot model and the import function does account for that (two different file paths in the tree structure of the robot). The geometry that you can see in my figures is the desired collision mesh, not the visuals which would be much more refined. However, it seems that Matlab again simplifies the collision geometry leading now to an overkill in safety margin that limits my allowed movement of the robot. Is there any way to prevent Matlab from further simplifying the geometry and actually make use of the imported collision mesh?

So far, my workaround has been using the sepDist matrix to filter out collisions that do not involve the tool or the base, so I can at least move the other joints without prematurely getting a collision alert. Ignoring adjacent bodies is not enough in my case, because e.g. the tool actually has two adjacent bodies (the inner revolute joint that rotates the tool around its z-axis and the short arm that pivots the tool).

[Karsh Tharyani]

Ah! I see. It might be the case that the simplification of the tool's mesh might be a result of making the mesh geometry convex. It is hard to point out the cause of this behavior without actually looking into the mesh file. Thus, I recommend to reach out to Technical Support and share your model's mesh file and other info.

You might benefit by adding multiple collision geometries to your tool's rigidBody using addCollision https://www.mathworks.com/help/robotics/ref/rigidbody.addcollision.html

Thus, you can consider making a composite of meshes for your tool in order to make your collision alerts more accurate. The checkCollision function does account for the multiple collision geometries added to a rigidBody.

Also, I see that you want to ignore any collisions involving the base and the end-effector. If that's the case, consider clearCollisions. https://www.mathworks.com/help/robotics/ref/rigidbody.clearcollision.html This should clear all the collision data related to the body.

[fixusc]

Yes it looks very much like thise is the case. However, it's not just the tool but also the base of the robot which gets a weird shape, leading to premature collision detection with the ground. So is it normal for Matlab to modify the geometry in such a way? Is there no other way to retain the original geometry other than splitting up the mesh into many parts?

I actually do not want to remove any collision mesh entirely but only look at certain cases. E.g. one arm colliding with the ground or the tool colliding with the ground or an arm. But arms colliding with arms is already taken care of by the joint angle limits. This is why I work with the sepDist matrix. But thanks anyway for the hint, I did not know about this function.

I will reach out to support now, thank you for helping me out so far.