Fail early when Connect stage start state is in collision

[MarqRazz]

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.

[rhaschke]

With this change, you don't really fail earlier, do you?
Still, for all possible pairs of start and end states, the compute() method is called, immediately failing if the start state is in collision.
The only thing you "save", is the instantiation of the planning infrastructure, which should immediately notice that the start state is in collision as well.
If that takes a significant amount of time, it should be better fixed in MoveIt's planning pipeline.

To me, it looks like that you just introduce yet another collision check, which seems redundant.

You mentioned that your console is spammed with error messages from the planning pipeline. I expect, that now the console is spammed with your generated error messages. Is simply printing these messages too slow?

I didn't yet considered the modified example. Will do that later, if I find time.

[MarqRazz]

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.

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]

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]

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]

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]

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]

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

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

• If planning fails with INVALID_MOTION_PLAN, the FixStartState adapter actually succeeded, but the final validation of the planned trajectory fails, because the first few waypoints are in collision. Looks like interpolated waypoints were introduced between the invalid start state and the new state. However, these waypoints are in collision too.
  MoveIt1 doesn't behave like that, it jumps to the collision-free state (and ignores the start state during validation).
• If planning fails with SUCCESS, the adapter actually failed, but erroneously reports SUCCESS?!?

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

[rhaschke]

I implemented my suggestion from #573 (comment) in master: 9c05305
I will attempt a merge of master into ros2 later today. I think, this PR can be closed then.

[MarqRazz]

Thanks for the help and advice here @rhaschke!