FixStartStateCollision is not compatible with AddIterativeSplineParameterization

[Hugal31]

Description

When using the AddIterativeSplineParameterization with FixStartStateCollision, it can insert an invalid state between the first and the second, which is then rejected by the PlanningPipeline.

Your environment

• ROS Distro: Noetic
• OS Version: Ubuntu 20.04
• Binary build 1.1.12-1focal.20230524.155710

Steps to reproduce

• Have a planning pipeline with AddIterativeSplineParameterization and FixStartStateCollision, in this order.
• Put the robot in a collision state, somewhat deep (just so FixStartStateCollision can find a solution but not too close to a valid state so the state inserted by AddIterativeSplineParameterization is invalid).
• Make a move request out of the collision

Expected behaviour

FixStartStateCollision should find a valid state and plan with it, and the robot should move outside the collision zone.

Actual behaviour

AddIterativeSplineParameterization adds a intermediate state between the invalid prefix state and the first valid state, which makes the Planning pipeline deems the trajectory invalid, because it only accepts the first state to be invalid.

By the way, AddIterativeSplineParameterization doesn't add the indexes of the added states in the added_path_index.

Backtrace or Console output

[ INFO] [1687878986.339796306]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1687878986.339989409]: Planning attempt 1 of at most 1
[ INFO] [1687879060.153374920]: Found a contact between 'ur5e/wrist_3_link' (type 'Robot link') and 'ur5e/safety_box/front' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1687879060.153418706]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1687879060.153463362]: Start state appears to be in collision with respect to group arm_group
[ INFO] [1687879060.168462697]: Found a valid state near the start state at distance 0.754572 after 17 attempts
[ INFO] [1687879062.587812474]: Planner configuration 'arm_group' will use planner 'geometric::PRMstar'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1687879062.636798276]: arm_group/arm_group: Starting planning with 9306 states already in datastructure
[ INFO] [1687879064.675725815]: arm_group/arm_group: Created 27 states
[ INFO] [1687879064.675801579]: Solution found in 2.087803 seconds
[ INFO] [1687879065.220953437]: SimpleSetup: Path simplification took 0.545075 seconds and changed from 5 to 3 states
[ INFO] [1687879719.836724376]: Planning adapters have added states at index positions: [ 0 ]
[ERROR] [1687879719.856631480]: Computed path is not valid. Invalid states at index locations: [ 0 1 ] out of 134. Explanations follow in command line. Contacts are published on /move_group/display_contacts
[ INFO] [1687879719.856868268]: Found a contact between 'ur5e/wrist_3_link' (type 'Robot link') and 'ur5e/safety_box/front' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1687879719.856915090]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1687879719.857254085]: Found a contact between 'ur5e/wrist_3_link' (type 'Robot link') and 'ur5e/safety_box/front' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1687879719.857289804]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ERROR] [1687879719.857505497]: Completed listing of explanations for invalid states.

Possible solutions

I'm not actually sure what to do. I could think of:

• Somehow accepting more than the first state as invalid in the PlanningPipeline
• Add the possibility to make AddIterativeSplineParameterization not insert a waypoint after the first one (which will have side effects on the acceleration if I understand correctly).
• Make an adapter that removes the second waypoint if invalid, to be run after the AddIterativeSplineParameterization.
• Invert the place of AddIterativeSplineParameterization and FixStartStateCollision in the pipeline, but that would make this part of the trajectory not respect acceleration and velocity constraints.

[Hugal31]

By reading more closely the PlanningPipeline, I understood that it already accepts invalid states if they were added by the adapters. However, since the AddIterativeSplineParameterization doesn't publish its added states indices, this behavior is not triggered.

I tried to create a fixed version of AddIterativeSplineParameterization that publish the added indices, but because of what seems to be another bug in the PlanningRequestAdapterChain, the indices are wrongly incremented.

[rhaschke]

Fixed via #3464.