dual-robot built by myself planning succeeds but execution fails
Closed this issue · 3 comments
Description
There is a dual-robot urdf model that used to work normally in ros1, and now the function pack is built in ros2 with moveit2_setup_assistant.
Three planning groups are set: left_arm,right_arm, and both_arm(including left_arm and right_arm). Run demo.launch.py, the planning and execution of a single robot are all normal.
However, when both_arm is selected to plan dual-robot, the planning succeeds but the execution fails.
Your environment
- ROS Distro: Humble
- OS Version: Ubuntu 22.04
- Source or Binary build? Source
- If source, which branch? Sorry, I'm not sure. I followed these instructions to clone, I think these instructions can tell which branch I am installed.
git clone https://github.com/ros-planning/moveit2_tutorials -b humble --depth 1 moveit2_ws/src/moveit2_tutorials
vcs import < moveit2_tutorials/moveit2_tutorials.repos
Backtrace or Console output
Select both_arm planning group, then
When I click plan in motionplanning in rviz, the terminal output：
[rviz2-1] [INFO] [1708418338.752921265] [move_group_interface]: MoveGroup action client/server ready
[move_group-4] [INFO] [1708418338.753496048] [moveit_move_group_default_capabilities.move_action_capability]: Received request
[move_group-4] [INFO] [1708418338.753663531] [moveit_move_group_default_capabilities.move_action_capability]: executing..
[rviz2-1] [INFO] [1708418338.753760086] [move_group_interface]: Planning request accepted
[move_group-4] [WARN] [1708418338.759509483] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Unable to transform object from frame 'panda_link0' to planning frame'Link_foot' (Could not find a connection between 'Link_foot' and 'panda_link0' because they are not part of the same tree.Tf has two or more unconnected trees.)
[move_group-4] [WARN] [1708418338.759556910] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Unable to transform object from frame 'world' to planning frame'Link_foot' (Could not find a connection between 'Link_foot' and 'world' because they are not part of the same tree.Tf has two or more unconnected trees.)
[move_group-4] [INFO] [1708418338.763617423] [moveit_move_group_default_capabilities.move_action_capability]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[move_group-4] [INFO] [1708418338.763781260] [moveit_move_group_capabilities_base.move_group_capability]: Using planning pipeline 'ompl'
[move_group-4] [INFO] [1708418338.764837213] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'both_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[move_group-4] [INFO] [1708418338.883188176] [moveit_move_group_default_capabilities.move_action_capability]: Motion plan was computed successfully.
[rviz2-1] [INFO] [1708418338.883991212] [move_group_interface]: Planning request complete!
[rviz2-1] [INFO] [1708418338.884287871] [move_group_interface]: time taken to generate plan: 0.033796 seconds
When I click execute in motionplanning in rviz, the terminal output：
[move_group-4] [INFO] [1708418344.962483992] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]:
Received goal request
[move_group-4] [INFO] [1708418344.962715454] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution request received
[move_group-4] [INFO] [1708418344.962773777] [moveit.plugins.moveit_simple_controller_manager]: Returned 3 controllers in list
[move_group-4] [INFO] [1708418344.962813566] [moveit.plugins.moveit_simple_controller_manager]: Returned 3 controllers in list
[move_group-4] [INFO] [1708418344.962912272] [moveit_ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01
[rviz2-1] [INFO] [1708418344.962814371] [move_group_interface]: Execute request accepted
[move_group-4] [INFO] [1708418344.971817769] [moveit_ros.trajectory_execution_manager]: Starting trajectory execution ...
[move_group-4] [INFO] [1708418344.971897334] [moveit.plugins.moveit_simple_controller_manager]: Returned 3 controllers in list
[move_group-4] [INFO] [1708418344.971925412] [moveit.plugins.moveit_simple_controller_manager]: Returned 3 controllers in list
[move_group-4] [INFO] [1708418344.972062002] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending continuation for the currently executed trajectory to both_arm_controller
[ros2_control_node-5] [INFO] [1708418344.972338561] [both_arm_controller]: Received new action goal
[ros2_control_node-5] [ERROR] [1708418344.972380111] [both_arm_controller]: Can't accept new action goals. Controller is not running.
[move_group-4] [INFO] [1708418344.972556436] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: both_arm_controller started execution
[move_group-4] [WARN] [1708418344.972578812] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request rejected
[move_group-4] [ERROR] [1708418344.972564293] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal was rejected by server
[move_group-4] [ERROR] [1708418344.972621807] [moveit_ros.trajectory_execution_manager]: Failed to send trajectory part 1 of 1 to controller both_arm_controller
[move_group-4] [INFO] [1708418344.972633087] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status ABORTED ...
[move_group-4] [INFO] [1708418344.972708750] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution completed: ABORTED
[rviz2-1] [INFO] [1708418344.973115743] [move_group_interface]: Execute request aborted
[rviz2-1] [ERROR] [1708418344.974029036] [move_group_interface]: MoveGroupInterface::execute() failed or timeout reached
Pretty sure you need to remove your "both_arm_controller" from your configurations. The joints are already controlled by each individual controller, and MoveIt knows which planning group uses which controller.
Can't accept new action goals. Controller is not running.
If you looked further up in the trace, the both_arm_controller would have failed to spawn in because the joints it wants to control are already accounted for with the other controllers
Thank you very much for your help. As you said, when I run demo.launch.py, I checked the terminal information output and found an error, which is more evidence that it is indeed a resource conflict.
[controller_manager]: Resource conflict for controller 'both_arm_controller'. Command interface 'robot_joint1_l/position' is already claimed.
Then after I removed "both_arm_controller" from my configurations as instructed by you, the both_arm planning group can now function properly.
Thank you again for your help in solving this problem for me.
@reidchristopher thank you for providing the solution. I'm closing this.