Humble branch demo fails on execution

MarqRazz commented 5 months ago

I'm seeing a few issues when trying to execute the Demo pick and place program.

Execution fails part way through the task
I get a few Found empty JointState message's when executing (seems to not be causing any issues)
I can't execute the pickup because I would collide with the object and move_group fails
The controller for each task is not specified and I get warnings
I get a bunch of warnings about the trajectory controller is not listed

If I ask the demo config to execute the solution with the mock_components: execute: true The robot can only execute up to the pick point and then collision detection with the object causes it to stop.

[pick_place_demo-1] [moveit_task_constructor_demo 1717364678.467567393]: Planning succeded
[pick_place_demo-1] [moveit_task_constructor_demo 1717364678.467579243]: Executing solution trajectory
[pick_place_demo-1] [moveit_task_constructor_executor_110240201996832 1717364681.185096688]: Goal was aborted or canceled
[pick_place_demo-1] [moveit_task_constructor_demo 1717364681.206986352]: Task execution failed and returned: 99999
[pick_place_demo-1] [moveit_task_constructor_demo 1717364681.207010792]: Execution complete

I'm also seeing a few empty JointState messages while it approaches the pick point:

[move_group-4] [moveit_robot_state.conversions 1717364680.333089713]: Found empty JointState message
[move_group-4] [moveit.plugins.moveit_simple_controller_manager 1717364680.333216522]: Returned 2 controllers in list
[move_group-4] [moveit.plugins.moveit_simple_controller_manager 1717364680.333223566]: Returned 2 controllers in list
[move_group-4] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle 1717364680.333295994]: sending trajectory to panda_arm_controller
[ros2_control_node-5] [panda_arm_controller 1717364680.333428349]: Received new action goal
[ros2_control_node-5] [panda_arm_controller 1717364680.333455996]: Accepted new action goal
[ros2_control_node-5] [panda_arm_controller 1717364681.041431964]: Goal reached, success!

and if you inspect the Rviz terminal we can see that we get a bunch of warnings about which controller is going to execute each stage:

[move_group-4] [moveit_task_constructor_visualization.execute_task_solution 1717364678.470975575]: The trajectory of stage '4' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.
[move_group-4] [moveit_task_constructor_visualization.execute_task_solution 1717364678.471033496]: The trajectory of stage '5' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.
////
Same for stage 7, 11, 14,...

and then the robot is stopped because it would collide with the target object.

[move_group-4] [moveit_ros.plan_execution 1717364681.134433464]: Trajectory component '10/19' is invalid
[move_group-4] [moveit_collision_detection_fcl.collision_common 1717364681.134438323]: Found a contact between 'object' (type 'Object') and 'object' (type 'Robot attached'), which constitutes a collision. Contact information is not stored.
[move_group-4] [moveit_collision_detection_fcl.collision_common 1717364681.134440510]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[move_group-4] [moveit_ros.plan_execution 1717364681.134442674]: Upcoming trajectory component '10/19' is invalid

I tried to look into how the planning scene is passed to the executor action (I think this is how it's set when making the goal) but can't see the source of the issues with it not being properly passed to move_group to execute.

rhaschke commented 5 months ago

This seems to be a duplicate of #561, which was fixed with an additional revert on the Humble branch. Please checkout the latest commit 9c3655dec2f8e3dae199a76f5be4f5c81bcfc7bc.

MarqRazz commented 5 months ago

Not sure how I missed that commit on my fork, sorry about the sloppy workspace management!

I still get these warnings. Should the controllers be explicitly set in each Stage?

[move_group-4] [moveit_task_constructor_visualization.execute_task_solution 1717427532.106751682]: The trajectory of stage '4' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.
[move_group-4] [moveit_task_constructor_visualization.execute_task_solution 1717427532.106813314]: The trajectory of stage '5' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.
[move_group-4] [moveit_task_constructor_visualization.execute_task_solution 1717427532.106851123]: The trajectory of stage '7' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.
[move_group-4] [moveit_task_constructor_visualization.execute_task_solution 1717427532.106876960]: The trajectory of stage '11' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.
[move_group-4] [moveit_task_constructor_visualization.execute_task_solution 1717427532.106931256]: The trajectory of stage '14' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.
[move_group-4] [moveit_task_constructor_visualization.execute_task_solution 1717427532.106975585]: The trajectory of stage '16' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.
[move_group-4] [moveit_task_constructor_visualization.execute_task_solution 1717427532.107018325]: The trajectory of stage '18' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.
[move_group-4] [moveit_task_constructor_visualization.execute_task_solution 1717427532.107050142]: The trajectory of stage '21' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.
[move_group-4] [moveit_task_constructor_visualization.execute_task_solution 1717427532.107091647]: The trajectory of stage '24' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.
[move_group-4] [moveit_task_constructor_visualization.execute_task_solution 1717427532.107175876]: The trajectory of stage '25' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.

MarqRazz commented 5 months ago

Something like #582 fixes that issue for me.

moveit / moveit_task_constructor

Humble branch demo fails on execution #579