search

moveit / moveit2

:robot: MoveIt for ROS 2
https://moveit.ai/
BSD 3-Clause "New" or "Revised" License
1.06k stars 516 forks source link

Move multiple arms simultaneously #3037

Open BingPeng0 opened 2 days ago

BingPeng0 commented 2 days ago

Hi all, I managed to control two arms together in a single file using moveit_py with separate controllers: left_arm_controller and right_arm_controller. However, the execution currently happens one arm after the other. I wanted to ask if it is possible to move both arms simultaneously. If so, how can this be done? Thank you.

BingPeng0 commented 2 days ago

I tried to use threading to implement both execution together but it failed. 2 Scenarios will occur.

1: Only One Arm is Executed

[dual_arm_panda.py-1] [INFO] [1729238942.120851031] [moveit_py]: Started Simultaneous Execution [dual_arm_panda.py-1] [INFO] [1729238942.121780532] [moveit_py]: Planning arm for left side [dual_arm_panda.py-1] [INFO] [1729238942.121986106] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' [dual_arm_panda.py-1] [INFO] [1729238942.122017358] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' [dual_arm_panda.py-1] [WARN] [1729238942.122020888] [moveit_2775093837.moveit.ros.validate_workspace_bounds]: It looks like the planning volume was not specified. Using default values. [dual_arm_panda.py-1] [INFO] [1729238942.122028849] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' [dual_arm_panda.py-1] [INFO] [1729238942.122056025] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' [dual_arm_panda.py-1] [WARN] [1729238942.122105781] [moveit_2775093837.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'left_panda_arm' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. [dual_arm_panda.py-1] [INFO] [1729238942.122257637] [moveit_2775093837.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'left_panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. [dual_arm_panda.py-1] [INFO] [1729238942.122303547] [moveit_py]: Calling Planner 'OMPL' [dual_arm_panda.py-1] [INFO] [1729238942.125266114] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' [dual_arm_panda.py-1] [INFO] [1729238942.125819702] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' [dual_arm_panda.py-1] [INFO] [1729238942.126051673] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' [dual_arm_panda.py-1] [INFO] [1729238942.126177857] [moveit_py]: Planning arm for right side [dual_arm_panda.py-1] [INFO] [1729238942.126326132] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' [dual_arm_panda.py-1] [INFO] [1729238942.126349842] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' [dual_arm_panda.py-1] [WARN] [1729238942.126353447] [moveit_2775093837.moveit.ros.validate_workspace_bounds]: It looks like the planning volume was not specified. Using default values. [dual_arm_panda.py-1] [INFO] [1729238942.126360933] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' [dual_arm_panda.py-1] [INFO] [1729238942.126372253] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' [dual_arm_panda.py-1] [WARN] [1729238942.126416247] [moveit_2775093837.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'right_panda_arm' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. [dual_arm_panda.py-1] [INFO] [1729238942.126526303] [moveit_2775093837.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'right_panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. [dual_arm_panda.py-1] [INFO] [1729238942.126571585] [moveit_py]: Calling Planner 'OMPL' [dual_arm_panda.py-1] [INFO] [1729238942.139903526] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' [dual_arm_panda.py-1] [INFO] [1729238942.140482845] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' [dual_arm_panda.py-1] [INFO] [1729238942.140710871] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' [dual_arm_panda.py-1] [INFO] [1729238942.141469484] [moveit_py]: Executing plan [dual_arm_panda.py-1] [INFO] [1729238942.141540307] [moveit_py]: Executing plan [dual_arm_panda.py-1] [ERROR] [1729238942.141571649] [moveit_2775093837.moveit.ros.moveit_cpp]: Execution failed! No active controllers configured for group 'right_panda_arm' [dual_arm_panda.py-1] [INFO] [1729238942.141922272] [moveit_2775093837.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 [dual_arm_panda.py-1] [INFO] [1729238942.150875043] [moveit_2775093837.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... [dual_arm_panda.py-1] [INFO] [1729238942.151043830] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to /left_arm_controller [ros2_control_node-7] [INFO] [1729238942.151245949] [left_arm_controller]: Received new action goal [ros2_control_node-7] [INFO] [1729238942.151298186] [left_arm_controller]: Accepted new action goal [dual_arm_panda.py-1] [INFO] [1729238942.151778950] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: /left_arm_controller started execution [dual_arm_panda.py-1] [INFO] [1729238942.151801488] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! [ros2_control_node-7] [INFO] [1729238943.910635701] [left_arm_controller]: Goal reached, success! [dual_arm_panda.py-1] [INFO] [1729238943.951772400] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller '/left_arm_controller' successfully finished [dual_arm_panda.py-1] [INFO] [1729238943.980825504] [moveit_2775093837.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... [dual_arm_panda.py-1] [INFO] [1729238944.982170007] [moveit_py]: Both arms finished execution

2: No execution done on both arms & the process died

[dual_arm_panda.py-1] [INFO] [1729239065.102723609] [moveit_py]: Started Simultaneous Execution [dual_arm_panda.py-1] [INFO] [1729239065.104250713] [moveit_py]: Planning arm for left side [dual_arm_panda.py-1] [INFO] [1729239065.105334370] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' [dual_arm_panda.py-1] [INFO] [1729239065.105595681] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' [dual_arm_panda.py-1] [WARN] [1729239065.105637617] [moveit_2775093837.moveit.ros.validate_workspace_bounds]: It looks like the planning volume was not specified. Using default values. [dual_arm_panda.py-1] [INFO] [1729239065.105672797] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' [dual_arm_panda.py-1] [INFO] [1729239065.105728631] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' [dual_arm_panda.py-1] [WARN] [1729239065.105893850] [moveit_2775093837.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'left_panda_arm' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. [dual_arm_panda.py-1] [INFO] [1729239065.106258360] [moveit_2775093837.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'left_panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. [dual_arm_panda.py-1] [INFO] [1729239065.106371209] [moveit_py]: Calling Planner 'OMPL' [dual_arm_panda.py-1] [INFO] [1729239065.130819079] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' [dual_arm_panda.py-1] [INFO] [1729239065.131523087] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' [dual_arm_panda.py-1] [INFO] [1729239065.132097271] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' [dual_arm_panda.py-1] [INFO] [1729239065.133808614] [moveit_py]: Executing plan [dual_arm_panda.py-1] [INFO] [1729239065.133978636] [moveit_py]: Planning arm for right side [dual_arm_panda.py-1] [INFO] [1729239065.134258101] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' [dual_arm_panda.py-1] [INFO] [1729239065.134290454] [moveit_2775093837.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 [dual_arm_panda.py-1] [INFO] [1729239065.134307563] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' [dual_arm_panda.py-1] [WARN] [1729239065.134318521] [moveit_2775093837.moveit.ros.validate_workspace_bounds]: It looks like the planning volume was not specified. Using default values. [dual_arm_panda.py-1] [INFO] [1729239065.134336929] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' [dual_arm_panda.py-1] [INFO] [1729239065.134365330] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' [dual_arm_panda.py-1] [WARN] [1729239065.134541624] [moveit_2775093837.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'right_panda_arm' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. [dual_arm_panda.py-1] [INFO] [1729239065.135030636] [moveit_2775093837.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'right_panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. [dual_arm_panda.py-1] [INFO] [1729239065.135195003] [moveit_py]: Calling Planner 'OMPL' [dual_arm_panda.py-1] [INFO] [1729239065.140655851] [moveit_2775093837.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... [dual_arm_panda.py-1] [INFO] [1729239065.140832970] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to /left_arm_controller [ros2_control_node-7] [INFO] [1729239065.141011797] [left_arm_controller]: Received new action goal [ros2_control_node-7] [INFO] [1729239065.141051450] [left_arm_controller]: Accepted new action goal [dual_arm_panda.py-1] [INFO] [1729239065.141145730] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: /left_arm_controller started execution [dual_arm_panda.py-1] [INFO] [1729239065.141156972] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! [dual_arm_panda.py-1] [INFO] [1729239065.150591231] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' [dual_arm_panda.py-1] [INFO] [1729239065.151476098] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' [dual_arm_panda.py-1] [INFO] [1729239065.151868987] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' [dual_arm_panda.py-1] [INFO] [1729239065.152330404] [moveit_py]: Executing plan [dual_arm_panda.py-1] [ERROR] [1729239065.152426169] [moveit_2775093837.moveit.ros.trajectory_execution_manager]: Cannot push a new trajectory while another is being executed [dual_arm_panda.py-1] [INFO] [1729239065.152442892] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Cancelling execution for /left_arm_controller [ros2_control_node-7] [INFO] [1729239065.152605687] [left_arm_controller]: Got request to cancel goal [ros2_control_node-7] [INFO] [1729239065.152631430] [left_arm_controller]: Canceling active action goal because cancel callback received. [dual_arm_panda.py-1] [INFO] [1729239065.152808977] [moveit_2775093837.moveit.ros.trajectory_execution_manager]: Stopped trajectory execution. [dual_arm_panda.py-1] [INFO] [1729239065.191413503] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller '/left_arm_controller' successfully finished [dual_arm_panda.py-1] [INFO] [1729239065.191448356] [moveit_2775093837.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status PREEMPTED ... [dual_arm_panda.py-1] [INFO] [1729239065.191532762] [moveit_2775093837.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... [ERROR] [dual_arm_panda.py-1]: process has died [pid 67358, exit code -11, cmd '/root/ws_moveit/install/moveit2_tutorials/lib/moveit2_tutorials/dual_arm_panda.py --ros-args -r __node:=moveit_py --params-file /tmp/launch_params_t0abzwke'].

radhen commented 2 days ago

@BingPeng0 one option is to have a single planning group for both the arms. Suppose you have two two 6 DOFs arms, then create a single planning group with all the 12 DOFs in your URDF/SRDF file for the robot, in this manner. This will allow to set a joint angle goal for all the 12 DOFs, and execute the planned motion such that both the arms move simultaneously, but note you can only plan in joint space with this approach.

I have a small suggestion. It would be helpful if your first comment could include details about what you’ve already tried and links to any files that worked for you. This will give others a clearer understanding of the problem and your troubleshooting efforts, allowing them to provide more relevant suggestions or solutions.