Integrate MoveIt2

[irisxu02]

No description provided.

[irisxu02]

In the case where only having a single UR robot defined in arms.json, one robot is spawned. Then by including MoveIt configuration provided in FindPackageShare("ur_moveit_config"), "/launch", "/ur_moveit.launch.py", and running "Plan and Execute", the following error occurs:

[move_group-4] [INFO] [1724949312.434461309] [moveit_move_group_default_capabilities.move_action_capability]: executing..
[move_group-4] [INFO] [1724949312.434540395] [moveit_move_group_default_capabilities.move_action_capability]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[move_group-4] [INFO] [1724949312.434646204] [moveit_ros.plan_execution]: Planning attempt 1 of at most 1
[move_group-4] [INFO] [1724949312.434659328] [moveit_move_group_capabilities_base.move_group_capability]: Using planning pipeline 'move_group'
[rviz2-5] [INFO] [1724949312.434629624] [move_group_interface]: Plan and Execute request accepted
[move_group-4] [INFO] [1724949312.435025858] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[move_group-4] [INFO] [1724949312.536025352] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1724949312.536048504] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1724949312.536091013] [moveit_ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01
[move_group-4] [INFO] [1724949312.536162944] [moveit_ros.trajectory_execution_manager]: Starting trajectory execution ...
[move_group-4] [INFO] [1724949312.536180866] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1724949312.536191227] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [ERROR] [1724949312.536226715] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Action client not connected to action server: joint_trajectory_controller/follow_joint_trajectory
[move_group-4] [ERROR] [1724949312.536230365] [moveit_ros.trajectory_execution_manager]: Failed to send trajectory part 1 of 1 to controller joint_trajectory_controller
[move_group-4] [INFO] [1724949312.536233435] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status ABORTED ...
[move_group-4] [INFO] [1724949312.546225631] [moveit_move_group_default_capabilities.move_action_capability]: Solution found but controller failed during execution
[rviz2-5] [INFO] [1724949312.546446489] [move_group_interface]: Plan and Execute request aborted
[rviz2-5] [ERROR] [1724949312.547265989] [move_group_interface]: MoveGroupInterface::move() failed or timeout reached