Speed up collision checking

I am using Matlab's "checkCollision" function with four additional collision objects and a UR5 robot. Function is working great and returns logical 1 or 0, which is later used in the program (I am checking overall reachability of the workpiece).

The only problem I am facing is computation time. I already limited points on the workpiece but it still takes too long, 80% of the time is used only for collision checking.

Is there a way to speed things up? If not, is there any other available collision checking library (maybe with use of voxels or spheres) which works with Matlab in similar fashion?

Answer 1

[Karsh Tharyani]

Hi RoboTomo,

Thanks for your question.

• There were some improvements made in performance for checkCollision in R2021b. Try and see if it meets your expectation.
• You can also try turning "off" self-collision checking if you are confident that the configurations you pass to the collision checking routine don't result in the robot colliding with itself. See ("IgnoreSelfCollision") here: https://www.mathworks.com/help/robotics/ref/rigidbodytree.checkcollision.html#namevaluepairarguments
• Alternatively you can try generating a MEX for an entry point function (See rbtCheckCollision) that accepts a configuration and returns whether the robot is in collision.

function isColliding=rbtCheckCollision(config)

% Use persistence if the robot and the environment are not changing.

persistent rbt env

if(isempty(rbt))

rbt=importrobot("universalUR5.urdf","DataFormat","row");

end

if(isempty(env))

c1=collisionBox(0.1,0.1,0.1);

c1.Pose=trvec2tform([0.2,0.2,0.4]);

c2=collisionSphere(0.3);

c2.Pose=trvec2tform([0.4,-0.2,0.4]);

env={c1,c2};

end

isColliding=rbt.checkCollision(config,env);

end

>> codegen rbtCheckCollision -args {zeros(1,6)}

I would be glad if you could also reach out to Technical Support and convey any performance requirements that you have for your use case, and we would be happy to enhance checkCollision in a future release of MATLAB.

Best,

Karsh

[RoboTomo]

Thank you for your fast reply and answer. I am already using R2021b and self collision checking is necessary. Your posted function doesn't check self collision right? When I changed:

rbt = loadrobot("universalUR5e","DataFormat","row");

to

rbt = importrobot("universalUR5.urdf","DataFormat","row");

then selfcollision checking is activated and compuation time is similar as before.

I do not have such experience with MEX files, but I generated the file with your command (codegen rbtCheckCollision -args {zeros(1,6)}) and got rbtCheckCollision_mex.mexw64. When calling function again, time is not shorter. I am not sure if mex function is prioritized over original .m function.

[Karsh Tharyani]

Hi RoboTomo,

- I updated my answer soon after I posted to use “universalUR5e” with a “loadrobot” call. I visualized the robot’s collision geometries using the show function of the “rigidBodyTree”.

- The code above does check for self-collisions.

- You should replace your call with rbtCheckCollison_mex to make sure the MEXed function is being called.

Best,

Karsh

[RoboTomo]

In my case it says that the input 'universalUR5e' did not match any of the valid values (model for ur5e doesn't exist), If I use 'universalUR5' its working, but it only checks self collsiions with "importrobot" command not "loadrobot". Am I missing something?

[Karsh Tharyani]

Hi RoboTomo,

I apologize for the confusion. Yes "the input 'universalUR5e' did not match any of the valid values" is an expected error. I updated my answer, accordingly.

If robot imported via "importrobot" provides you with collision data useful for self-collision checks, please continue to use it as it is also supported for code generation, and you will be able to generate a MEXed version of rbtCheckCollision. You can verify if the imported robot has collision data useful for self-collisions, by using show

rbt=importrobot("universalUR5.urdf");

show(rbt,"Visuals","off","Collisions","on");

Question: Are you able to see collision data for the imported robot?

There seems to be an issue with "loadrobot" for the "universalUR5" wherein the populated collision geometries aren't useful for self collision checking. I have reported this issue to the development team at MathWorks. Should the issue be addressed in a future/update release of MATLAB, you shall be notified.

Please reach out to Technical Support if you are still facing issues, or have clarifying questions.

Best,

Karsh

[RoboTomo]

Ok thank you for clarification, I was already thinking that there was new UR5e model somewhere. Yes, visuals are working great, I was using this at the beginning to verify objects and collision detection.

I modified my program a little bit and separated collision detection. For calculating one configuration it now needs approx. 0.01 seconds which I think is good enough.