Connect: wrong robot start state using STOMP planner

[captain-yoshi]

Is the STOMP planner compatible with MTC?

Using the STOMP planner for the first sub-task works fine. But for each subsequent connect stages, the starting robot state is the default UR3 state from the ur3_moveit_config, and not the the previous robot state. Notice that there are some information retained like the object that is attached to the tcp.

Setup configuration:

• Master branch for MoveIt and MTC
• Latest branch for stomp_ros
• stomp_planning.yaml (same as panda except for stddev param)
• The initial starting robot pose is given in the initial parameter in fake_controllers.yaml

For the same task I have uploaded 2 gifs. One using OMPL where the task is planned correctly. The other using STOMP with the wrong robot starting state on the second connect stage. (Click to collapse)

OMPL

STOMP

[v4hn]

Thank you for reporting the issue. Could I ask you to prepare a minimal example?

[captain-yoshi]

I have created this repo that contains a minimal example of the problem. The instructions are in the readme.

[captain-yoshi]

The issue was with stomp_ros package. This pull request fixes this issue.

[v4hn]

Thank you for debugging this! I added my review to that pull-request! 👍