Controlling two Yaskawa Robots via Moveit

[TE-2]

Hello everyone, I have two Yaskawa robots, MotoMini and GP4, in a robot module and would like to control them simultaneously via ROS2. 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:

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:

Now I wanted to test the whole thing 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.

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:

[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