Moveit: CHOMP is not planning to avoid obstacles

[beatrizleon]

I am testing the chomp planner and I was able to run the demo without problems, but when I add a scene with a few boxes the planner fails because it creates plans that move through the boxes. Here is a video showing an example and the console output.

@ksatyaki am I missing something?

https://www.opentest.co/share/13747ae096d511e6a4b9d5997606e058

[ INFO] [1476974468.639840465]: The following collision detectors are active in the planning scene.
[ INFO] [1476974468.639924948]: HYBRID
[ INFO] [1476974468.639979873]: Active collision detector is: HYBRID
[ INFO] [1476974468.640567252]: First coll check took 0.000500102
[ INFO] [1476974468.744905938]: Optimization took 0.105349 sec to create
[ INFO] [1476974468.744969567]: Optimization took 0.105437 sec to create
[ INFO] [1476974468.771808283]: Forward kinematics took 0.026809078
[ INFO] [1476974468.772828601]: iteration: 0
[ INFO] [1476974468.774232479]: Chomp Got mesh to mesh safety at iter 0. Breaking out early.
[ INFO] [1476974468.774245614]: Chomp path is collision free
[ INFO] [1476974468.774396609]: Terminated after 1 iterations, using path from iteration -1
[ INFO] [1476974468.774427175]: Optimization core finished in 0.029445 sec
[ INFO] [1476974468.774440784]: Time per iteration 0.029459
[ INFO] [1476974468.774455331]: Optimization actually took 0.134922 sec to run
[ INFO] [1476974468.774466672]: Output trajectory has 6 joints
[ INFO] [1476974468.774645821]: Joint 0 -0.538908
[ INFO] [1476974468.774682969]: Joint 1 0.634558
[ INFO] [1476974468.774719493]: Joint 2 -0.562051
[ INFO] [1476974468.774764935]: Joint 3 1.66659
[ INFO] [1476974468.774782979]: Joint 4 0.287965
[ INFO] [1476974468.774813866]: Joint 5 1.54044
[ INFO] [1476974468.774844534]: Bottom took 0.000374 sec to create
[ INFO] [1476974468.774858459]: Serviced planning request in 0.136556 wall-seconds, trajectory duration is 0.000000
[ERROR] [1476974468.803435454]: Computed path is not valid. Invalid states at index locations: [ 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 ] out of 101. Explanations follow in command line. Contacts are published on /move_group/display_contacts
[ INFO] [1476974468.803553807]: Found a contact between 'box3' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.803569449]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.804289481]: Found a contact between 'box3' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.804303584]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.805150013]: Found a contact between 'box3' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.805163527]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.806011763]: Found a contact between 'box3' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.806024644]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.806740759]: Found a contact between 'box3' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.806754904]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.807481881]: Found a contact between 'box3' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.807501242]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.808332384]: Found a contact between 'box3' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.808402092]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.809548394]: Found a contact between 'box3' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.809643334]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.810681836]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.810725935]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.811529206]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.811579223]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.812392312]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.812417862]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.813165344]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.813184003]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.813952516]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.813975127]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.814728991]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.814798348]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.815357510]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.815371657]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.816007614]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.816039457]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.816640326]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.816653224]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.817378242]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.817395617]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.817900791]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.817913954]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.818410373]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.818423277]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.818930254]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.818943007]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.819526134]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.819538624]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.819986408]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.819999386]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.820425996]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.820439051]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.820917938]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.820934152]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.821348467]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.821361052]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.821748672]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.821761086]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.822233960]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.822247319]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.822662780]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.822675671]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.823295018]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.823307675]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.823863268]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.823876128]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.824216354]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.824228854]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.824613682]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.824632323]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.825033285]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.825047317]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.825560180]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.825573192]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.826126634]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.826140544]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.826691716]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.826704502]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.828726355]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.828766610]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1476974468.830704512]: Found a contact between 'box2' (type 'Object') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1476974468.830732941]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ERROR] [1476974468.832529229]: Completed listing of explanations for invalid states.
[ WARN] [1476974468.841645837]: Fail: ABORTED: Motion plan was found but it seems to be invalid (possibly due to postprocessing). Not executing.

[ksatyaki]

Hello,

I really haven't tested the planner with obstacles. I had only tried to get it to run with the new API since the old version was very out-dated.

I can help fix the problem. I was busy lately, sorry.

Are you sure the obstacles are part of the planning scene? Did you use moveit to add these obstacles to the planning scene? I guess if it's part of the planning scene, then it should work in theory. If it still doesn't work we need to figure out why the planner is not taking these into account.

EDIT: I mis-read your message. I think the planner (at the moment) creates new plannings scenes. Maybe it should obtain the planning scene from move_group instead of creating it's own scene. That way we have the same planning scene which is modified by you (with the boxes) and the which is used by the planner. I am only guessing at this point. Can you send me the code that you use to test this set-up?

Chittaranjan

[beatrizleon]

Thanks for your reply @ksatyaki

I am launching: roslaunch moveit_resources demo_chomp.launch

And running this python code:
https://github.com/shadow-robot/sr_interface/blob/indigo-devel/sr_multi_moveit/sr_moveit_planner_benchmarking/src/sr_moveit_planner_benchmarking/collision_scene_1.py

Then I use rviz to generate random goal robot poses to test it.

[dyouakim]

Hello @ksatyaki, @beatrizleon
I donno if this problem has been solved, for me CHOMP is still failing to plan with obstacle avoidance.
I investigated a bit more, and created a new planning scene at the time a request is set (in chomp_plugin getPlanningContext) and set it to the scene available at this time, and I made sure it has the right number of world obstacles, also removed the creation of a new one in chomp_planning_context constructor. Still CHOMP is failing to plan avoiding an obstacle. I checked the code and I see that CHOMP optimizer is the one that checked for collision and actually the collision is found but as far as I understand no alternative action is taken... I don't fully understand how CHOMP works, but I have impression that the problem is outside the planning scene updates, any advice ?

Thanks.

[ksatyaki]

I haven't really been working on chomp for a long time now. I am not sure how this can be fixed. Try increasing the "planning_time_limit" parameter. May be this has an effect...

[130s]

Partially addressed by https://github.com/ros-planning/moveit/issues/546 as the author of that PR says.

[BryceStevenWilley]

Note this is still an issue, it can replicated by running roslaunch moveit_resources demo_chomp.launch and this script. After looking at some CHOMP paths, it looks like paths at higher iterations are moving towards the boundaries of the obstacles, but they are still always in collision.

[bmagyar]

In #626 I added a couple of pre- and post-planning checks. It doens't make CHOMP plan around obstacles just yet but at least it fails when there's a collision. Could you confirm please if at least now your colliding plans fail? :)

[BryceStevenWilley]

Oh, I understand; I wasn't sure how much functionality #626 was meant to restore. I can confirm that the colliding plans fail however!

[bmagyar]

It was an effort to make the current CHOMP planner be less incorrect with the tools at hand. If you have specific requirements, please list them up on a new issue and we may even pick them up for the Google Summer of Code!

[davetcoleman]

Whats the latest on this issue? We have a GSoC student looking into CHOMP this summer. @raghavendersahdev

[raghavendersahdev]

Hi all, I think I might have found a solution to this. The important variable is the obstacle_weight_cost value in the chomp_planning.yaml file, once the cost is reduced, CHOMP finds the paths successfully. A youtube video which shows obstacle free paths being planned with CHOMP is attached here! I set the obstacle_weight to 0.2 and CHOMP could plan successfully, initially it was set to 1.0 due to which it was failing as the planner wanted to keep a longer distance from the obstacles, which lead to CHOMP not finding successful paths. Could anyone verify that this works @ksatyaki @BryceStevenWilley @dyouakim

I used a modified version of @beatrizleon python script to set up the obstacle environment.
collision_scene_1.txt (change extension to .py).

[davetcoleman]

@raghavendersahdev do you have a pull request with the suggested fix? that is the easiest way to review and test it

[bmagyar]

It feels a little counter-intuitive that if collision distance is set to too high, CHOMP generates paths that ignore all collisions. I don't have a solution off the top of my head but we should aim to catch this issue with an error other than "CHOMP path has collisions".

[raghavendersahdev]

@davetcoleman I made a pull request, but I don't think it is solved yet.
I agree with you @bmagyar to address this issue with something concrete other than the error message.

Some more cases: in this video .
This video shows that CHOMP does indeed considers the obstacles into account fails as it couldn't find a path here.

In some cases CHOMP finds the path with obstacles and in some cases it says when it fails whenever there is no collision free path (maybe there is a possibility that there is no collision free path in such cases),
Do we want CHOMP to always be able to find a collision free path if there exists one?

I think, the problem still might not be solved OR it might be solved if (CHOMP genuinely takes the obstacle into consideration while planning and just cannot find a path), I will look more into the chomp code/setting up the environment code and try to figure this out. Will see how the control is flowing through the code and where it might be breaking..

Note: OMPL finds a path in almost all cases and successfully avoids obstacles meaning CHOMP should also find it.

[BryceStevenWilley]

@raghavendersahdev I wasn't able to recreate your result of avoid collisions with scene objects. I've attached a screenshot that shows CHOMP directly moving through a column. I'm using MoveIt! from kinetic-devel and your branch of panda_moveit_config. The pictured scene is collision_scene_test_1.py with box4 removed.

I set the obstacle_weight to 0.2 and CHOMP could plan successfully, initially it was set to 1.0 due to which it was failing as the planner wanted to keep a longer distance from the obstacles, which lead to CHOMP not finding successful paths.

From my understanding, this isn't necessarily correct. The obstacle_cost_weight and smoothness_cost_weight parameters should be controlling the weights of those costs in the final cost that CHOMP is actually optimizing over. The parameter that controls how far CHOMP stays from obstacles should be collision_clearance or collision_threshold, not quite sure which one it actually is. If it is collision_clearance, maybe lowering that value would help, as 0.2 meters seems high, normally it's set to something around 0.02 to 0.07 meters, see this CHOMP implementation for an example.

So by decreasing obstacle_cost_weight, you are actually decreasing the amount that CHOMP cares about obstacle collisions, making it more likely to ignore collisions completely. The fact that CHOMP takes many more iterations to solve a problem when the obstacle cost is high suggests that CHOMP knows that it can improve the obstacle cost, but there is something wrong in the update step when it actually tries to modify its trajectory. This is similar to what I saw when I did some stuff with this back in January: the final trajectories that CHOMP was returning were very similar to the initial straight line path.

In some cases CHOMP finds the path with obstacles and in some cases it says when it fails whenever there is no collision free path (maybe there is a possibility that there is no collision free path in such cases),

I highly doubt that there are no collision free paths in these cases, unless the start or goal configuration is already in collision.

Do we want CHOMP to always be able to find a collision free path if there exists one?
Note: OMPL finds a path in almost all cases and successfully avoids obstacles meaning CHOMP should also find it.

Ideally, yes. A lack of completeness is the main deficit of optimization planners. However, CHOMP doesn't have the best track record of finding collision free paths. See the results on page 8 and 9 here. I wouldn't suggest trying to get CHOMP to solve every problem that's thrown in front of it, but at least some.

[dyouakim]

I have followed a different approach, not sure it is the correct one..I noticed that the problem is the planning scene instance passed to CHOMP, actually a new one is created so it does not include the obstacles are there in the original scene. So I modified the "chomp_plugin.cpp" in the getPlanningContext to retrieve the current scene and set the collision detector to be used. I was able to plan around obstacles, yet I would not say it succeeds 100% in complex environments.

[BryceStevenWilley]

@dyouakim That's interesting, I'm assuming that you are talking about these few lines in your MoveIt fork? After #546, it looks like the only difference is that you are making a copy instead of using the original object, which shouldn't matter because the planning scene is stored as a const pointer, and shouldn't be modifiable anyway. It could still be the issue somehow though, definitely a good start, to ensure that #546 is working as expected.

[raghavendersahdev]

Thanks @BryceStevenWilley for the comments

Yes I also get the same thing, it moves through the collision object but it shows failed in the planner box in RViz and on the terminal shows that CHOMP found a collision between a particular joint link and the obstacle:

[ INFO] [1527536921.132339487]: Collision checking is considered complete (collision was found and 0 contacts are stored)
and some times it shows chomp path is not collision free and fails

[ INFO] [1527537024.866709983]: Forward kinematics took 0.027334872
Thanks for letting me know about the obstacle_cost_weight , collision_clearance parameters and other information about CHOMP.

I have tried/trying the following and it does not work:

• changed the IK solver to trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin AND panda_arm_panda_arm_kinematics/IKFastKinematicsPlugin DID NOT WORK
• currently Trying to change the collision detector to FCL from Hybrid by changing lines 1, 2 , 3, and other places wherever hybrid related stuff is there.... and changing the value to IndustrialFCL in the launch file 4 . I think hybrid collision checker should also be good enough for a simple scene with just one obstacle.

Things I am/will be trying next to tackle this issue:

• trying to visualize using rviz_visual_tools: there is a lot of commented code for visualization in the chomp_optimizer.cpp file, will uncomment parts of it for visualization results and finding the source of the problem
• making sure everything is set-up correctly with respect to the planning scene, If everything is setup correctly and the obstacles are part of the planning scene while chomp plans the path, this should technically work, right?... assuming IK solver, cost-functions and collision checkers are good. I am trying this with a 7 DOF panda robot arm, so IK should not be an issue.
• take an initial path obtained from OMPL and trying to optimize that using CHOMP.

[bmagyar]

These may help:

• the IK solver won't help solving your problem since CHOMP only uses that at the top layer, before the optimization takes place
• planning scene is the one taken from the global environment, it should be fine but you can set up a local publisher to double check
• If you are already at it, it would be great to have a truly configurable collision checking mechanism, so far only Hybrid worked due to some of the hardcoded methods although some parts of the code takes the param into account (as far as I remember)
• I'd set up a script that runs a couple of tests and assembles some statistics of the current state. "some times it shows chomp path is not collision free and fails" may improve over time but if you don't have numbers to check you likely won't feel that you are getting closer to a solution. I think the issue here is a complex one made up of multiple sources.

[raghavendersahdev]

Thanks @bmagyar for the comments (these are helpful).
I agree about the IK solver, understood the fact that it is only being used in the initial part.

If you are already at it, it would be great to have a truly configurable collision checking mechanism, so far only Hybrid worked due to some of the hardcoded methods although some parts of the code takes the param into account (as far as I remember)

Yes this is true, there are hardcoded methods specific to hybrid like this, this, this, this, etc. I am not able to find a function corresponding to hy_world_->getCollisionGradients for fcl_world_ .. Will look into it more and try to complete this.

About CHOMP Failing:

I have a feeling that CHOMP starts with an initial guess (some where in one of its ChompTrajectory constructors) for the trajectory and then tries to optimize it, however for making that initial guess it does not consider any obstacles, and while optimization step, it is never able to find a path around the obstacle, if there is a path which avoids the obstacle, CHOMP would actually take the same path irrespective of the presence of the obstacle too.
Also wanted to point out that the collisionProximity related code is commented throughout the ChompOptimzer.cpp file here, here, and the code which searches for a different solution (to avoid getting caught in the local minima) is commented here .
If CHOMP would get stuck in a local minima, currently (in the ChompOptimizer::optimize() method) there seems to be no way to get out of the local minima, which might be the reason, it is not able to avoid the obstacles, as it might be stuck in the local minima forever with the initial guess which does not consider the obstacle at all.

I am currently looking into this and seeing if I get anything concrete out of it by uncommenting the local minima avoidance code.

[raghavendersahdev]

Things I have tried / done so far :

• Uncommented the local minima avoidance code and set the parameter add_randomness to true , so this falls into the code which should be getting out of the local minima, currently whenever I add an obstacle it always falls into the local minima avoidance code and is stuck in the loop forever and is never able to come out of local minima and eventually it times out . The reason for this is there is another bug in the code; I print out the original cost and new cost in each iteration of the this loop and the cost is always the same, so turns out who ever wanted to implement the local minima avoidance code did not implement it correctly (or I am missing something possibly uncommenting HMC part, read below), probably thats why it is commented by the author. The author of this code perturbs the trajectory , updates the full trajectory and performs forward kinematics again and tries to get a new cost. These steps however do not impact the cost in any way. Hence this attempt to get out of the local minima is not successful. Also the initial parameters ( which controls entering the local minima avoidance block), averageCostVelocity and minimaThreshold have a bug, the averageCostVelocity is always zero (I printed this) and minimaThreshold is hard-coded to 0.5 which makes the flow of control always enter the local minima avoidance code.

• About testing individual iterations: I printed the costs (collision cost and smoothness cost) in each iteration, the smoothness cost does not change in any iteration (e.g., 3125.57 in iteration 1 to 3125.57 in iteration 500), while the collision cost reduces but minimally (e.g., from 0.1535 in iteration 1 to 0.153483 in iteration 500) . When there is no obstacle present in almost all cases, a plan/solution is found in the first iteration it self. The next step I want to test is if I start with a collision free guess initially (possibly hard code this in the constructor or get OMPL's path) and have CHOMP optimize that path to verify if CHOMP avoids the obstacle, I feel in this case CHOMP should be able to avoid the obstacle successfully, but is this even of any use, would this not be considered cheating for the CHOMP planner or does this qualify as an acceptable solution?

• I am not sure of this particular point but maybe since in almost all cases (without obstacles) (I tried a bunch of start/goal states), CHOMP finds the solution in the first iteration itself, sometimes in 11th ( 11 because of possibly this block ) Maybe there is a bad thing (shortcut) happening that should not happen. I will try to fully understand the optimizer code and remove this possibility too.

• changed the use_stochastic_descent parameter to false so it uses a random point from the trajectory but is guaranteed to converge.

• None of the HMC based code is currently used, so the corresponding HMC parameters in the chomp_planning.yaml also don't make sense coz they are never used.

• Maybe the hmc based code (stochastic version) and the local minima avoidance code together make sense, else it is of not much use.... will try this next to see if the old and new costs are updated or not, the trajectory does get changed while updated here.

• None of this code (HMC Initialization for momentum_, random_momentum_, random_joint_momentum_) is actually used anywhere, everything is commented.

• Addressing 3 out of the 4 pending checklist items that were mentioned here, will make a pull request by end of day today Thursday (May 31).

Things I am planning to do next to solve this:

• Uncomment the HMC (hamiltonian montecarlo) stochastic version of the code. TO see if this makes a difference.

• Need to uncomment the visualization code and make it work to visualize each iteration.

• Need to change the collision checker.

[raghavendersahdev]

some theoretical insights from the CHOMP paper, might be useful:

Highlighted text seems interesting:

_Page 1: takes an initial guess and optimizes it towards a local optima, if initial guess is bad in a simple environment, will it find a good solution, probably YES_

_Page 3: falls prey to local minima for difficult situations, but in simple environments too falling prey, meaning there is a bug in code._

_Page 6: addition of noise to a matrix diagonal which might cause collisions due to preference to smoothness, need to verify if they do this in the CHOMP MoveIt implementation too_

[raghavendersahdev]

It works now and is avoiding obstacles in some cases (not avoiding in 100% cases but a lot of cases work which earlier were not working), can some-one verify this?

• video showing obstacle avoidance in a simple scene (with 1 obstacle)
• video showing obstacle avoidance in a complex scene (with 4 obstacles)

Reason:

I changed the ridge_factor and learning_rate in the chomp_planning.yaml file to 0.001 and 0.04 respectively and it avoids obstacles not 100% of the time, but Yes it does avoid them in a lot of cases which were previously not getting avoided. For reference, I used the CHOMP parameters as in this
chomp_planning .yaml.txt file. This behaviour (increasing the ridge_factor) is correlated to the statement in the CHOMP paper (page 6, see previous comment): that

We found that adding a small amount (0.001) to the diagonal of A improved performance by avoiding situations where preferring smooth trajectories caused additional collisions

The noise is added in the ChompCost constructorhere to the total cost which gets called here. This noise (ridge_factor) makes the obstacle avoidance behavior work. Together with having a higher learning rate. I used learning_rate=0.04

Also it can be seen from the video that smooth trajectories are actually not preferred when there is an obstacle and theridge factor is 0.001, when there is no obstacle, the trajectories are smooth.

[mamoll]

@raghavendersahdev, the paths are not only not so smooth in the more challenging examples, the states that form the path seem to also "bunch up" and have large gaps in different parts of the path. You may want to add a path validity check that interpolates the path produced by CHOMP at a higher resolution.

The behavior you are seeing may actually be expected and not a bug; perhaps it's just a function of the cost function. In that case, try to just document how one would go about finding reasonable parameters to maximize the odds of finding a high-quality feasible path.

[raghavendersahdev]

The behavior you are seeing may actually be expected and not a bug; perhaps it's just a function of the cost function. In that case, try to just document how one would go about finding reasonable parameters to maximize the odds of finding a high-quality feasible path

Thanks @mamoll I think this seems to be happening (likely maybe coz it correlates with the paper's footnote), I will double check the code again though and would document this as a paragraph for some intuition in the choice of parameters to be chosen for CHOMP.

[tsijs]

Sorry to spam the moveit github, but since a lot of people here seem to be a little deeper into chomp than I currently am: could somebody confirm/disprove me that the current chomp plugin only uses a unsigned distance field?
I printed all values < 0 with obstacles in it, and although some of the values are actually 0.0, it seems that there are no negative values in the field.
I'd liked to check it myself, it is just that I do not understand the structure that's used to create the hybrid collision detector.