Controlling two Robots simultaneously via Moveit

[TE-2]

Hello everyone, I have two Yaskawa robots, MotoMini and GP4, in a robot module, each controlled by a YRC1000micro controller, and I would like to control them simultaneously via ROS2 Humble. The robots should not work directly together, but their workspaces overlap. They should also be able to execute trajectories simultaneously so that one robot does not have to wait for the other.

The control of a single robot via Moveit works without problems, I used the ros2-starter-for-yaskawa-robots repository and adapted the ros2 package motoXY to my two robots.

For the control of both robots I want a move_group node for each robot in a respective namespace e.g. (/moto_mini_moveit& /moto_gp4_moveit), because I read in a forum post that the move group node is not able to execute two trajectories at the same time.

For this I have modified xy_start.launch.py by passing run_move_group_nodeX the new name_space as an argument, as well as joint_state_publisherX and robot_state_publisherX and would like to start only these three nodes for the time being. With the help of rqt_graph I was able to identify the incorrectly assigned topics and then wrote the suitable remappings. The direct mapping of actions is unfortunately not yet possible according to this article, so I used the workaround and remapped the corresponding topics.

The node /moveit_simple_controller_manager, for example, even had /moto_mini_moveit/moto_mini/follow_joint_trajectory/_action/status after the action topic.

Customised xy_start.launch.py:

Click to expand

```python import os from launch import LaunchDescription from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory from moveit_configs_utils import MoveItConfigsBuilder import xacro def generate_launch_description(): # ======================================================================= # NameSpace configuration robot_name_space = "/moto_mini" name_space_moveit = "/moto_mini_moveit" # ======================================================================= motoman_directory = "moveit_resources_moto_motomini_moveit_config" #not Directory -> ROS Package # URDF motoman_robot_description_config = xacro.process_file( os.path.join( get_package_share_directory(motoman_directory), "config", "motoman_motomini.urdf.xacro", ), ) motoman_robot_description = { "robot_description": motoman_robot_description_config.toxml() } # ======================================================================= # MoveGroupNode moveit_config = ( MoveItConfigsBuilder("moveit_resources_moto_motomini") .robot_description(file_path="config/motoman_motomini.urdf.xacro") .robot_description_semantic(file_path="config/motoman_motomini.srdf") .trajectory_execution(file_path="config/moveit_controllers.yaml") .to_moveit_configs() ) planning_scene_monitor_parameters = { "publish_planning_scene": True, "publish_geometry_updates": True, "publish_state_updates": True, "publish_transforms_updates": True, "publish_robot_description": True, "publish_robot_description_semantic": True, } # Start the actual move_group node/action server # robot run_move_group_nodeX = Node( package="moveit_ros_move_group", executable="move_group", namespace= name_space_moveit, output="screen", parameters=[ moveit_config.to_dict(), planning_scene_monitor_parameters, ], remappings= [ ('joint_states', f'{robot_name_space}/joint_states'), # Action -> follow_joint_trajectory action (f'{name_space_moveit}{robot_name_space}/follow_joint_trajectory/_action/status', f'{robot_name_space}/follow_joint_trajectory/_action/status'), (f'{name_space_moveit}{robot_name_space}/follow_joint_trajectory/_action/feedback', f'{robot_name_space}/follow_joint_trajectory/_action/feedback'), (f'{name_space_moveit}/monitored_planning_scene', '/monitored_planning_scene'), (f'{name_space_moveit}/display_planned_path', '/display_planned_path'), ] ) # ======================================================================= joint_state_publisherX = Node( package='joint_state_publisher', executable='joint_state_publisher', name='joint_state_publisher', namespace= name_space_moveit, parameters=[ { "robot_description": motoman_robot_description, "rate": 43, "source_list": [f'{robot_name_space}/joint_states'], } ], ) # ======================================================================= robot_state_publisherX = Node( package="robot_state_publisher", executable="robot_state_publisher", name="robot_state_publisher", namespace= name_space_moveit, parameters=[ motoman_robot_description, { "robot_description": motoman_robot_description, "publish_frequency": 43.0, "frame_prefix": "", }, ] ) # ======================================================================= # Launch return LaunchDescription([ run_move_group_nodeX, joint_state_publisherX, robot_state_publisherX, ] ) ```

After this adjustment my ros graph looked correct again but with the new namespace:

Click to expand


Now I wanted to test it using a script of the hello_moveit package. For this it was important to add options to the constructor of the MoveGroupInterface in which the move_group_namespace was defined.

std::string group_name = "manipulator";
std::string move_group_namespace = "/moto_mini_moveit";
moveit::planning_interface::MoveGroupInterface::Options options(group_name, moveit::planning_interface::MoveGroupInterface::ROBOT_DESCRIPTION, move_group_namespace);
using moveit::planning_interface::MoveGroupInterface;
auto move_group_interface = MoveGroupInterface(node, options);

In addition, I had to start the node via the following launch file in which I again specified the new namespace and the mapping had to be adapted. Without the launch file and running the script via ros2 run I could not even load the robot model even if I gave the argument namespace "moto_mini_moveit" to the constructor of the "hello_motomini" node.

Click to expand

```python from launch import LaunchDescription from launch_ros.actions import Node def generate_launch_description(): name_space_moveit = "/moto_mini_moveit" return LaunchDescription([ Node( package='hello_moveit', executable='hello_motomini', name='hello_motomini', namespace= name_space_moveit, output="screen", remappings= [ # Action -> move_action ('/move_action/_action/feedback', f'{name_space_moveit}/move_action/_action/feedback'), ('/move_action/_action/status', f'{name_space_moveit}/move_action/_action/status'), # Action -> execute_trajectory ction ('/execute_trajectory/_action/feedback', f'{name_space_moveit}/execute_trajectory/_action/feedback'), ('/execute_trajectory/_action/status', f'{name_space_moveit}/execute_trajectory/_action/status'), ('/trajectory_execution_event', f'{name_space_moveit}/trajectory_execution_event'), ('/attached_collision_object', f'{name_space_moveit}/attached_collision_object'), ] ) ]) ```

With the help of the launch file, the node started correctly and the planning was also executed. Only the execution of the trajectory fails and I don't understand why.

In the Terminal ofxy_start.launch I get the following logs:

Click to expand

``` [move_group-1] You can start planning now! [move_group-1] [joint_state_publisher-2] [INFO] [1710347636.450236976] [moto_mini_moveit.joint_state_publisher]: Waiting for robot_description to be published on the robot_description topic... [move_group-1] [INFO] [1710349284.843051471] [moveit_move_group_default_capabilities.move_action_capability]: Received request [move_group-1] [INFO] [1710349284.843127327] [moveit_move_group_default_capabilities.move_action_capability]: executing.. [move_group-1] [INFO] [1710349284.869346679] [moveit_move_group_default_capabilities.move_action_capability]: Planning request received for MoveGroup action. Forwarding to planning pipeline. [move_group-1] [INFO] [1710349284.870056364] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. [move_group-1] [WARN] [1710349284.900956562] [moveit_trajectory_processing.time_optimal_trajectory_generation]: Joint acceleration limits are not defined. Using the default 1 rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml. [move_group-1] [INFO] [1710349284.906044218] [moveit_move_group_default_capabilities.move_action_capability]: Motion plan was computed successfully. [move_group-1] [INFO] [1710349288.907850779] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Received goal request [move_group-1] [INFO] [1710349288.907909092] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution request received [move_group-1] [INFO] [1710349288.907929054] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list [move_group-1] [INFO] [1710349288.907937800] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list [move_group-1] [INFO] [1710349288.907985322] [moveit_ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 [move_group-1] [INFO] [1710349288.920418642] [moveit_ros.trajectory_execution_manager]: Starting trajectory execution ... [move_group-1] [INFO] [1710349288.920457918] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list [move_group-1] [INFO] [1710349288.920464835] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list [move_group-1] [ERROR] [1710349288.920511311] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Action client not connected to action server: moto_mini/follow_joint_trajectory [move_group-1] [ERROR] [1710349288.920513743] [moveit_ros.trajectory_execution_manager]: Failed to send trajectory part 1 of 1 to controller moto_mini [move_group-1] [INFO] [1710349288.920516026] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status ABORTED ... [move_group-1] [INFO] [1710349288.920660570] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution completed: ABORTED ```

It seems to fail because of the following error message:

[moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Action client not connected to action server: moto_mini/follow_joint_trajectory

But I don't understand this, because according to rqt_graph and "ros2 node info ..." the moveit_simple_controller_manager is a client of /moto_mini/follow_joint_trajectory/_action ? (see ros graph picture above)

I would appreciate any helpful input. Thanks

[gavanderhoorn]

For context: Yaskawa-Global/motoros2#225.

[github-actions[bot]]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

[github-actions[bot]]

This issue was closed because it has been stalled for 45 days with no activity.

[gavanderhoorn]

Resolution posted here: https://github.com/Yaskawa-Global/motoros2/discussions/225#discussioncomment-9859474.