CHOMP optimizer exits after first iteration as collision-free

[stephanie-eng]

Description

I've been trying to use CHOMP to post-process some trajectories, but I found it was always exiting after the first iteration, despite having a collision cost over the threshold.

To reproduce, add chomp/OptimizerAdapter to the ompl_planning.yaml for MoveIt Resources and try planning. Here are the logs from doing this:

[move_group-4] [INFO] [1656605094.550581986] [chomp_planner]: CHOMP trajectory initialized using method: quintic-spline 
[move_group-4] [INFO] [1656605094.550623655] [chomp_optimizer]: Active collision detector is: HYBRID
[move_group-4] [INFO] [1656605094.611060046] [chomp_optimizer]: First coll check took 0.060426 sec
[move_group-4] [INFO] [1656605094.630097411] [chomp_optimizer]: Collision cost 0.990141, smoothness cost: 15043.425202
[move_group-4] [INFO] [1656605094.630252191] [chomp_optimizer]: iteration: 0
[move_group-4] [INFO] [1656605094.631522113] [chomp_optimizer]: Chomp Got mesh to mesh safety at iter 0. Breaking out early.
[move_group-4] [INFO] [1656605094.631532675] [chomp_optimizer]: cCost 0.990141 over threshold 0.070000
[move_group-4] [INFO] [1656605094.631537393] [chomp_optimizer]: Chomp path is collision free
[move_group-4] [INFO] [1656605094.631541388] [chomp_optimizer]: Terminated after 1 iterations, using path from iteration 0

Is this the expected behaviour for CHOMP post-processing?

Reading through ChompOptimizer::optimize() here, it looks like if the path is collision-free as determined by isCurrentTrajectoryMeshToMeshCollisionFree(), we set the should_break_out_ flag before we check the collision cost. What is the difference between the isCurrentTrajectoryMeshToMeshCollisionFree() check is compared to the collision cost check - it possible to be have isCurrentTrajectoryMeshToMeshCollisionFree() return false but have the collision cost under the collision threshold?

It also seems like the termination conditions are only achieved by collision checking, without regard for smoothness cost or velocities or accelerations.

[AndyZe]

It also seems like the termination conditions are only achieved by collision checking, without regard for smoothness cost or velocities or accelerations.

I had a thought -- there is no way for CHOMP to optimize velocities or accelerations as it's used in MoveIt, because time-parameterization hasn't run yet. Thus there is no timing info on the waypoints. It's just a path. So I guess that using CHOMP for vel/accel optimization was not realistic.

[lcford2]

I had a thought -- there is no way for CHOMP to optimize velocities or accelerations as it's used in MoveIt, because time-parameterization hasn't run yet. Thus there is no timing info on the waypoints. It's just a path. So I guess that using CHOMP for vel/accel optimization was not realistic.

I am fairly new to the motion planning space so please let me know if I am incorrect or misunderstanding.

I thought that one of the selling points of CHOMP is that it is invariant to parameterization (https://www.ri.cmu.edu/pub_files/2013/5/CHOMP_IJRR.pdf).

Also, since CHOMP uses the numerical gradients of the joint positions to define velocity, acceleration, and jerk, would it not still work to smooth the path? I think the dynamic quantities would just be off from their actual value by a scalar factor, which would still allow for path optimization.

[henningkayser]

@stephanie-eng have there been any results for this issue yet?

[stephanie-eng]

I haven't been actively working on this, but I remember finding some specific changes to consider for the termination condition. I don't remember fully solving the problem, but I can take another look at it, and at the least, add my findings here.