Properly support cost thresholding in STOMP

[henningkayser]

STOMP currently categorizes trajectories as invalid if they produce any penalty cost. This prevents us from optionally optimizing waypoints inside a valid constraint space, let's say for hitting position targets within tolerance or for maximizing clearance within acceptable limits. #2631 introduces a 0 cost for satisfied constraints to at least accept valid trajectories. A proper cost thresholding approach would instead still compute the cost, but set the validity flag depending on constraint tolerances.

[patrickKXMD]

AFAIK there are two use cases for STOMP, one for optimizing the result from other planner, the other is standalone planner.

In my case I use standalone STOMP for all cartesian and joint space planning (not for optimizing the result from OMPL).
So any penalty cost in constraint space will result as a planning failed.

Maybe different cost counter for two cases?

[github-actions]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

[patrickKXMD]

@henningkayser
Hi, I'm a little worried for how to decide a proper cost threshold.

And I just looked up the code quickly, there is a config num_iterations_after_valid with default config 0.
If it is greater than 0, the STOMP planner it self will repeat iterations for that times and returns one solution with the lowest cost. So is this the right way if we want to continue optimizing?

What do you think?

[patrickKXMD]

@henningkayser
Hi, any updates?