Chomp trajectories are in self collision

Hello,

I've been using the CHOMP algorithm for a while, but I'm encountering persistent self-collisions in some trajectories. Specifically, when simulating a UR5e robot, links 4 and 6 often collide during the trajectory. I verified these collisions using the checkCollision function.

I've tried adjusting these collision options, but the issue persists. For now I have not modified other solver/collision options.

chomp.CollisionOptions.IgnoreSelfCollision = false;

chomp.CollisionOptions.CollisionCostWeight = 10;

Any insights to avoid self collisions ? Because I think it should not be impossible for robot to not to follow that specific path where the link 6 touches the link 4 so I am kinda lost.

Answer 1

[Karsh Tharyani]

Hi @Denizhan AKINCI,

manipulatorCHOMP is not a globally optimal planner. It is subject to a local minima, and maybe that is the case here i.e., it is not able to find a self-collision free path. Have you tried posing your problem to manipulatorRRT? Is manipulatorRRT able to find a self-collision free plan to the problem? If so, you can call manipulatorRRT and then feed that plan as an intial guess to manipulatorCHOMP/optimize.

For the Universal UR 5e, I have seen that setting the joint position limits to [-pi,pi] instead of the default [-2*pi,2*pi] helps in avoiding "passing through itself" trajectories for an initial guess.

I hope this helps.

If you still run into issues, I strongly suggest you share a simple reproducible of your problem with MathWorks Technical Support https://www.mathworks.com/support/contact_us.html to resolve this in a timely manner.

Best,

Karsh

[Denizhan AKINCI]

Thank you Karsh,

I see your point, but RRT takes a start and a goal configuration and outputs a trajectory, which isn't necessarily the optimal one. Let's say it generates a trajectory with 10 points in total, including the start and goal configurations. Can the CHOMP be used to optimize the trajectory by refining the consecutive points ?

In other words, if I'm not mistaken, CHOMP typically takes a start and a goal configuration and works to find an optimized path between them, rather than taking an entire pre-generated trajectory as input. In this context,can we adapt CHOMP to work with the trajectory provided by RRT by using it to smooth and optimize the intermediate points while preserving the overall structure of the path from start to goal ? If so, how does the CHOMP have a positive impact (by optimizing it) on the trajectory already generated by RRT ?

[Karsh Tharyani]

Hi @Denizhan AKINCI,

manipulatorCHOMP/optimize can be used to modify an entire trajectory. It accepts a stack (row wise) of configurations and corresponding time points.

Yes, manipulatorCHOMP will make this trajectory smoother while minimizing collisions.

I hope this helps,

Karsh