dual-robot built by myself planning succeeds but execution fails

zhangbo9426426 commented 9 months ago

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

[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

sea-bass commented 9 months ago

Seems the controller is not activated.

[both_arm_controller]: Can't accept new action goals. Controller is not running.

zhangbo9426426 commented 9 months ago

Thank you for the tip. I browsed the ros2_controllers.yaml file and it seems to be working fine.The following is the contents of the file：

controller_manager: ros__parameters: update_rate: 100
left_arm_controller:
  type: joint_trajectory_controller/JointTrajectoryController

right_arm_controller:
  type: joint_trajectory_controller/JointTrajectoryController

both_arm_controller:
  type: joint_trajectory_controller/JointTrajectoryController

joint_state_broadcaster:
  type: joint_state_broadcaster/JointStateBroadcaster
left_arm_controller:

ros__parameters: joints:

robot_joint1_l

robot_joint2_l

robot_joint3_l

robot_joint4_l

robot_joint5_l

robot_joint6_l command_interfaces:

position state_interfaces:

position

velocity right_arm_controller: ros__parameters: joints:

robot_joint1_r

robot_joint2_r

robot_joint4_r

robot_joint5_r

robot_joint6_r

robot_joint7_r command_interfaces:

position state_interfaces:

position

velocity both_arm_controller: ros__parameters: joints:

robot_joint1_l

robot_joint2_l

robot_joint3_l

robot_joint4_l

robot_joint5_l

robot_joint6_l

robot_joint1_r

robot_joint2_r

robot_joint4_r

robot_joint5_r

robot_joint6_r

robot_joint7_r command_interfaces:

position state_interfaces:

position

velocity

In addition, when demo.launch is just started, I check the terminal output information about both_arm_controllers as follows:

[spawner-7] [INFO] [1708439166.190132793] [spawner_both_arm_controller]: Loaded both_arm_controller [ros2_control_node-4] [INFO] [1708439166.191525742] [controller_manager]: Configuring controller 'both_arm_controller' [ros2_control_node-4] [INFO] [1708439166.191724376] [both_arm_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. [ros2_control_node-4] [INFO] [1708439166.191740117] [both_arm_controller]: Command interfaces are [position] and state interfaces are [position velocity]. [ros2_control_node-4] [INFO] [1708439166.191786248] [both_arm_controller]: Using 'splines' interpolation method. [ros2_control_node-4] [INFO] [1708439166.192174940] [both_arm_controller]: Controller state will be published at 50.00 Hz. [ros2_control_node-4] [INFO] [1708439166.192842625] [both_arm_controller]: Action status changes will be monitored at 20.00 Hz. [ros2_control_node-4] [ERROR] [1708439166.208465603] [controller_manager]: Resource conflict for controller 'both_arm_controller'. Command interface 'robot_joint1_l/position' is already claimed. [spawner-7] [INFO] [1708439166.219404553] [spawner_both_arm_controller]: Configured and activated both_arm_controller

Therer is a error：[controller_manager]: Resource conflict for controller 'both_arm_controller'. Command interface 'robot_joint1_l/position' is already claimed. I wonder if this error is to blame.

zhangbo9426426 commented 9 months ago

It seems that the problem is indeed resource conflict. After I delete the two separate planning groups left_arm and right_arm and keep only the planning group both_arm, I can successfully plan and execute the dual-robot at the same time.

However, when I have three planning groups in ROS1, left_arm, right_arm and both_arm, this problem does not occur. I can plan and execute for single robot or dual-robot.

Is there a solution to this resource conflict problem in moveit2?

sea-bass commented 9 months ago

Right! Because both_arm_controller claims the same hardware interfaces as the other ones, they can't both be activated at the same time. So you'll need to call into the ros2_control API to activate/deactivate the right controllers.

If you were running your robot from MoveIt2 code (and not RViz), there are ways around it where MoveIt will take care of this for you, like explicitly specifying the controller names you want in your planning request.

Move Group Interface: https://github.com/ros-planning/moveit2/pull/2257

MoveItCpp: https://github.com/ros-planning/moveit2/pull/1838

zhangbo9426426 commented 9 months ago

Thank you for your answer, I will go to learn about these methods.

moveit / moveit2_tutorials