Pick/Place demo fails when executing in humble

[DaniGarciaLopez]

I was experiencing problems with moveit_collision_detection in my setup and decided to try the pick/place demo. Planning is successful, but when executing, there is a collision between the gripper and the object, and the execution is preempted. Additionally, there is an error regarding an empty JointState message.

In my setup, I also encounter a collision of the attached object with itself (Found a contact between 'object' (type 'Object') and 'object' (type 'Robot attached')), which should have already been fixed here.

I'm building moveit and mtc from source using the latest humble branch. Any idea what's happening?

[move_group-4] [INFO] [1713480832.513559786] [moveit_ros.trajectory_execution_manager]: Starting trajectory execution ...
[move_group-4] [INFO] [1713480832.513591921] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1713480832.513601982] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [ERROR] [1713480832.513651890] [moveit_robot_state.conversions]: Found empty JointState message
[move_group-4] [INFO] [1713480832.513866048] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1713480832.513871182] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[ros2_control_node-5] [INFO] [1713480832.514144977] [panda_hand_controller]: Received & accepted new action goal
[move_group-4] [INFO] [1713480832.564803810] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1713480832.564822959] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1713480832.564888270] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to panda_arm_controller
[ros2_control_node-5] [INFO] [1713480832.565200435] [panda_arm_controller]: Received new action goal
[ros2_control_node-5] [INFO] [1713480832.565265437] [panda_arm_controller]: Accepted new action goal
[move_group-4] [INFO] [1713480832.565467992] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: panda_arm_controller started execution
[move_group-4] [INFO] [1713480832.565484011] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted!
[ros2_control_node-5] [INFO] [1713480834.293341539] [panda_arm_controller]: Goal reached, success!
[move_group-4] [INFO] [1713480834.316251404] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'panda_arm_controller' successfully finished
[move_group-4] [ERROR] [1713480834.316321089] [moveit_robot_state.conversions]: Found empty JointState message
[move_group-4] [INFO] [1713480834.316470961] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1713480834.316492694] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1713480834.316551981] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to panda_arm_controller
[ros2_control_node-5] [INFO] [1713480834.316678166] [panda_arm_controller]: Received new action goal
[ros2_control_node-5] [INFO] [1713480834.316734533] [panda_arm_controller]: Accepted new action goal
[move_group-4] [INFO] [1713480834.316795278] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: panda_arm_controller started execution
[move_group-4] [INFO] [1713480834.316798391] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted!
[ros2_control_node-5] [INFO] [1713480834.983417550] [panda_arm_controller]: Goal reached, success!
[move_group-4] [INFO] [1713480835.017084896] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'panda_arm_controller' successfully finished
[move_group-4] [ERROR] [1713480835.017135495] [moveit_robot_state.conversions]: Found empty JointState message
[move_group-4] [INFO] [1713480835.017204221] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1713480835.017212213] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [ERROR] [1713480835.017223539] [moveit_robot_state.conversions]: Found empty JointState message
[move_group-4] [INFO] [1713480835.017249949] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1713480835.017252666] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1713480835.017314011] [moveit_ros.plan_execution]: Trajectory component '7/19' is invalid
[move_group-4] [INFO] [1713480835.017322304] [moveit_collision_detection_fcl.collision_common]: Found a contact between 'object' (type 'Object') and 'panda_leftfinger' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[move_group-4] [INFO] [1713480835.017324905] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[move_group-4] [INFO] [1713480835.017327567] [moveit_ros.plan_execution]: Upcoming trajectory component '7/19' is invalid
[move_group-4] [INFO] [1713480835.017329728] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1713480835.017332353] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[ros2_control_node-5] [INFO] [1713480835.017435359] [panda_hand_controller]: Received & accepted new action goal
[move_group-4] [INFO] [1713480835.023644920] [moveit_ros.plan_execution]: Trajectory component '7/19' is invalid
[move_group-4] [INFO] [1713480835.023658700] [moveit_collision_detection_fcl.collision_common]: Found a contact between 'object' (type 'Object') and 'panda_leftfinger' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[move_group-4] [INFO] [1713480835.023661704] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[move_group-4] [INFO] [1713480835.023665009] [moveit_ros.plan_execution]: Trajectory component '7/19' is invalid after scene update
[move_group-4] [INFO] [1713480835.023666826] [moveit_ros.plan_execution]: Stopping execution because the path to execute became invalid(probably the environment changed)
[move_group-4] [INFO] [1713480835.023675637] [moveit.simple_controller_manager.gripper_controller_handle]: Cancelling execution for panda_hand_controller
[ros2_control_node-5] [INFO] [1713480835.023768156] [panda_hand_controller]: Got request to cancel goal
[ros2_control_node-5] [INFO] [1713480835.023798970] [panda_hand_controller]: Canceling active action goal because cancel callback received.
[move_group-4] [INFO] [1713480835.023931403] [moveit_ros.trajectory_execution_manager]: Stopped trajectory execution.
[move_group-4] [INFO] [1713480835.067635284] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status PREEMPTED ...

[Cavalletta98]

Have you solved It?

[DaniGarciaLopez]

No, I'm still looking for a solution

[Cavalletta98]

Write here when you found a solution. Thanks

[rhaschke]

The default settings from humble branch (commit 0857351) disable automatic execution:

moveit_task_constructor/demo/src/pick_place_demo_parameters.yaml

Lines 1 to 4 in 0857351

	pick_place_task_demo:
	execute:
	type: bool
	default_value: false

If I manually trigger execution from within rviz then, execution works just fine.
However, when enabling automatic execution, I do observe the exact same issue reported here as well.
Need to figure out why...

@DaniGarciaLopez, can you confirm this behavior? Did you use automatic execution to trigger the failure?

[DaniGarciaLopez]

@DaniGarciaLopez, can you confirm this behavior? Did you use automatic execution to trigger the failure?

Yes, @rhaschke, I found the issue when setting the execute parameter to true, so it uses task_->execute(*task_->solutions().front()). Our project experiences the same problem, and we call the ExecuteTaskSolution action server directly, so maybe there is no problem within the task_->execute function but in the action server itself.

If I manually trigger execution from within rviz then, execution works just fine.

I didn't know that it was possible to trigger the execution of a MTC task from Rviz. Using the MotionPlanning display and then clicking on execute? But how do you pass the task to it?

[rhaschke]

MTC has its own Motion Planning Tasks display and panel:

Select a top-level solution and click the execute button in the top-right corner.
It would be interesting to know whether execution works for you from there as well.

[DaniGarciaLopez]

You're right! I've been using MTC for over a year, and I still can't believe I didn't pay attention to that button before.

I can confirm that with the rviz panel I cannot reproduce the error mentioned here, and the execution ends successfully.

[rhaschke]

I pushed another commit (155ceb7) to the humble branch reverting a fix from the master branch that is not yet supported by humble's MoveIt. This resolves the issue for me.

[DaniGarciaLopez]

It's also working on my end. Thanks @rhaschke!