New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

CHOMP is not planning to avoid obstacles #305

Open
beatrizleon opened this Issue Oct 20, 2016 · 25 comments

Comments

Projects
None yet
9 participants
@beatrizleon
Copy link
Contributor

beatrizleon commented Oct 20, 2016

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

This comment has been minimized.

Copy link
Contributor

ksatyaki commented Oct 20, 2016

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

This comment has been minimized.

Copy link
Contributor

beatrizleon commented Oct 20, 2016

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

This comment has been minimized.

Copy link

dyouakim commented Feb 28, 2017

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

This comment has been minimized.

Copy link
Contributor

ksatyaki commented Feb 28, 2017

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...

@simonschmeisser simonschmeisser referenced this issue Jul 12, 2017

Merged

Chomp use PlanningScene #546

2 of 2 tasks complete
@130s

This comment has been minimized.

Copy link
Member

130s commented Aug 7, 2017

Partially addressed by #546 as the author of that PR says.

cschindlbeck pushed a commit to cschindlbeck/moveit that referenced this issue Aug 16, 2017

@BryceStevenWilley

This comment has been minimized.

Copy link
Contributor

BryceStevenWilley commented Jan 25, 2018

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

This comment has been minimized.

Copy link
Member

bmagyar commented Jan 26, 2018

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

This comment has been minimized.

Copy link
Contributor

BryceStevenWilley commented Jan 26, 2018

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

This comment has been minimized.

Copy link
Member

bmagyar commented Jan 27, 2018

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

This comment has been minimized.

Copy link
Member

davetcoleman commented May 10, 2018

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

@raghavendersahdev

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented May 23, 2018

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

This comment has been minimized.

Copy link
Member

davetcoleman commented May 24, 2018

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

@bmagyar

This comment has been minimized.

Copy link
Member

bmagyar commented May 24, 2018

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

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented May 24, 2018

@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

This comment has been minimized.

Copy link
Contributor

BryceStevenWilley commented May 24, 2018

@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.
screenshot_2018-05-24_17-10-00

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

This comment has been minimized.

Copy link

dyouakim commented May 25, 2018

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

This comment has been minimized.

Copy link
Contributor

BryceStevenWilley commented May 25, 2018

@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

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented May 28, 2018

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)
[ INFO] [1527536921.132406060]: Found a contact between 'box2' (type 'Object') and 'panda_link3' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1527536921.132420001]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ERROR] [1527536921.132481316]: Completed listing of explanations for invalid states.
[ WARN] [1527536921.157941331]: Fail: ABORTED: Motion plan was found but it seems to be invalid (possibly due to postprocessing). Not executing.

and some times it shows chomp path is not collision free and fails

[ INFO] [1527537024.866709983]: Forward kinematics took 0.027334872
[ERROR] [1527537024.867895942]: Chomp path is not collision free!
[ INFO] [1527537024.867986267]: Terminated after 500 iterations, using path from iteration 499
[ INFO] [1527537024.868002196]: Optimization core finished in 14.320115 sec
[ INFO] [1527537024.868026752]: Time per iteration 0.0286403
[ WARN] [1527537024.873715201]: Fail: ABORTED: No motion plan found. No execution attempted.

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

This comment has been minimized.

Copy link
Member

bmagyar commented May 28, 2018

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

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented May 29, 2018

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

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented May 31, 2018

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

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented May 31, 2018

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

localminima1

localminima2

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

local_minima2

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

adding_noise

@raghavendersahdev

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented Jun 4, 2018

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?

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 the ridge factor is 0.001, when there is no obstacle, the trajectories are smooth.

@mamoll

This comment has been minimized.

Copy link
Contributor

mamoll commented Jun 5, 2018

@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

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented Jun 6, 2018

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.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment