search

moveit / moveit_task_constructor

A hierarchical multi-stage manipulation planner
https://moveit.github.io/moveit_task_constructor
BSD 3-Clause "New" or "Revised" License
184 stars 152 forks source link

Fail early when Connect stage start state is in collision #573

Closed MarqRazz closed 6 months ago

MarqRazz commented 6 months ago

I have a project that I'm working on that was having motion planning failures when the start state of a Connect stage was in collision. My MoveIt config is on Humble and was missing the request_adapter default_planner_request_adapters/FixStartStateBounds so I was not getting any info other than planning failed and the terminal was spammed with Motion planning start tree could not be initialized! for several seconds.

After I added the request_adapter default_planner_request_adapters/FixStartStateBounds I could see which links were causing the failure but the Connect stage still attempted to search for a long period of time even though it was not possible to find a solution (I believe the amount of time is # of connecting states * myConnectStage.setTimeout(Value)).

This PR is a bit of a hack but I wanted to see if you think checking the start state for collisions before planning would be a useful addition. I have modified the demo program to highlight the issue if you would like to test it out. I was surprised that I could not replicate the issue by simply removing the Forbid collision (object support) stage so I added a second table to the scene that causes collision after the cylinder is lifted.

Right now my proposed solution is failing much faster but still not working as expected because it is not properly reporting the failure's to the Rviz MTC widget.

MarqRazz commented 6 months ago

Okay I turned the stage timeout way up and it does not take the entire timeout to fail but it does still perform motion planning even though the start state is in collision. You can also see some nice comment messages of SUCCESS even though they fail. start_planning_in_collision

Maybe it's because I'm using the older planning adapter default_planner_request_adapters/FixStartStateBounds and the new one (default_planning_request_adapters/CheckStartStateCollision) might just return instead of trying to fix it.

rhaschke commented 6 months ago

I was surprised that I could not replicate the issue by simply removing the Forbid collision (object support) stage so I added a second table to the scene that causes collision after the cylinder is lifted.

I was surprised, that the Connect stage considers input states that should have failed beforehand (due to collision). However, you explicitly allowed collisions with the other table and then disallowed them. Maybe, it would make sense to check validity of the final state of the ModifyPlanningScene stage? Then, you would have an early failure there.

rhaschke commented 6 months ago

Maybe it's because I'm using the older adapter FixStartStateBounds instead of the new one CheckStartStateCollision.

Sure, you can fail "early" in the adapter as well. FixStartStateBounds attempts to fix those collisions and then plans from the fixed state.

MarqRazz commented 6 months ago

Sure, you can fail "early" in the adapter as well. FixStartStateBounds attempts to fix those collisions and then plans from the fixed state.

Then why do some of them not succeed? You can see in the comment that the trajectory was "fixed"

rhaschke commented 6 months ago

Then why do some of them not succeed?

FixStartStateBounds attempts to fix collisions. There is no guarantee that this will succeed. The search for valid poses is via random search, so it randomly fails or succeeds depending on the severity of the penetration.

MarqRazz commented 6 months ago

Then why do some of them not succeed?

FixStartStateBounds attempts to fix collisions. There is no guarantee that this will succeed. The search for valid poses is via random search, so it randomly fails or succeeds depending on the severity of the penetration.

I mean why did the MTC stage not succeed? You can see that the comment on some of the trajectories were SUCCESS but they were still marked as failed.

EDIT: Actually all of the trajectories that are marked as SUCCESS are empty :thinking:. The ones marked as INVALID_MOTION_PLAN were actually able to plan out of collision but they still get marked as failure with a comment like this in the command line:

[pick_place_demo-1] [moveit.ros_planning.planning_pipeline 1716475287.546484774]: Computed path is not valid. Invalid states at index locations: [ 0 1 2 3 ] out of 58. Explanations follow in command line. Contacts are published on /display_contacts
[pick_place_demo-1] [moveit_collision_detection_fcl.collision_common 1716475287.546494516]: Found a contact between 'MyTable' (type 'Object') and 'object' (type 'Robot attached'), which constitutes a collision. Contact information is not stored.
rhaschke commented 6 months ago

I checked your example and the results point at issues in MoveIt2.

With MoveIt1, your modified pick+place example works as expected, only failing once during planning: image

rhaschke commented 6 months ago

I implemented my suggestion from https://github.com/moveit/moveit_task_constructor/pull/573#issuecomment-2127155860 in master: 9c05305effa7173a94f3befdae09386763e04f0a I will attempt a merge of master into ros2 later today. I think, this PR can be closed then.

MarqRazz commented 6 months ago

Thanks for the help and advice here @rhaschke!