Cannot use MoveItConfigsBuilder

[Leopold-Klotz]

I'm encountering difficulties while trying to set up and use the xarm6 robot with MoveIt in ROS 2 Humble. My main issues are:

1. The MoveItConfigsBuilder function from uf_ros_lib doesn't seem to be working correctly for the xarm6 configuration. Following the example use, I get an xacro error that one input file is needed.
2. I'm unable to get external code working with the xarm_moveit_config package. I would like to not use the xarm_planner wrapper as it limits some functionality that I need and I would like to keep my personal code hardware agnostic.
3. My personal nodes, which have worked with other robotic arms using the MoveGroup interface, are not interacting properly (ex. when launching with ros2 launch xarm_moveit_config xarm6_moveit_fake.launch.py).

Background: I have a node that typically interacts with MoveIt using the MoveGroup interface. This code has worked successfully with other robotic arms in the past.

Expected Behavior: My node's code should be able to:

1. Connect to the MoveGroup interface for the xarm6.
2. Plan and execute trajectories using the xarm6's end-effector.
3. Receive current robot state information (joint positions, end-effector pose).
4. Send movement commands to the robot.

Current Issues:

1. The MoveItConfigsBuilder is not generating a working configuration for the xarm6.
2. I'm unable to launch or properly configure the xarm_moveit_config package.
3. My nodes are not establishing a connection with the xarm6 MoveGroup.

Typical Interaction Flow:

1. My node creates a MoveGroupInterface object for the xarm6's arm group.
2. It sets the end-effector link and planning frame.
3. It generates waypoints for a path.
4. It calls the computeCartesianPath function to plan the trajectory.
5. It sends the planned trajectory to the arm for execution.
6. Throughout the process, it queries the current robot state and end-effector pose.

Questions:

1. Are there known issues with the MoveItConfigsBuilder for xarm6?
2. Is there a recommended way to set up the xarm6 with MoveIt that I might be missing?
3. Are there any xarm6-specific considerations for the MoveGroup interface that differ from other arms?

Any guidance on resolving these issues or suggestions for alternative approaches would be greatly appreciated. I'm happy to provide more details or specific code snippets if needed.

Code used to attempt the use of moveitconfigsbuilder:

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from uf_ros_lib.moveit_configs_builder import MoveItConfigsBuilder

def generate_launch_description():
    # Declare launch arguments
    prefix = DeclareLaunchArgument('prefix', default_value='')
    hw_ns = DeclareLaunchArgument('hw_ns', default_value='xarm')
    add_gripper = DeclareLaunchArgument('add_gripper', default_value='False')
    add_bio_gripper = DeclareLaunchArgument('add_bio_gripper', default_value='False')
    dof = DeclareLaunchArgument('dof', default_value='6')
    robot_type = DeclareLaunchArgument('robot_type', default_value='xarm')

    # Create MoveItConfigsBuilder
    moveit_configs = MoveItConfigsBuilder(
        controllers_name='fake_controllers',
        robot_type=LaunchConfiguration('robot_type'),
        dof=LaunchConfiguration('dof'),
        prefix=LaunchConfiguration('prefix'),
        hw_ns=LaunchConfiguration('hw_ns'),
        add_gripper=LaunchConfiguration('add_gripper'),
        add_bio_gripper=LaunchConfiguration('add_bio_gripper'),
    ).to_moveit_configs()

    # Create move_group node
    move_group_node = Node(
        package='moveit_ros_move_group',
        executable='move_group',
        output='screen',
        parameters=[moveit_configs.to_dict()],
    )

    # Return LaunchDescription
    return LaunchDescription([
        prefix,
        hw_ns,
        add_gripper,
        add_bio_gripper,
        dof,
        robot_type,
        move_group_node
    ])

I've looked at issue #64, but I haven't been able to get proper use of the moveitconfigsbuilder function.

[vimior]

@Leopold-Klotz

Any error reported? Try specifying the parameter ros2_control_plugin='uf_robot_hardware/UFRobotFakeSystemHardware' when generating moveit_configs.

moveit_configs = MoveItConfigsBuilder(
    controllers_name='fake_controllers',
    robot_type=LaunchConfiguration('robot_type'),
    dof=LaunchConfiguration('dof'),
    prefix=LaunchConfiguration('prefix'),
    hw_ns=LaunchConfiguration('hw_ns'),
    add_gripper=LaunchConfiguration('add_gripper'),
    add_bio_gripper=LaunchConfiguration('add_bio_gripper'),
    ros2_control_plugin='uf_robot_hardware/UFRobotFakeSystemHardware'
).to_moveit_configs()

[Leopold-Klotz]

@vimior

This is the error message reported. It gives this error with or without the ros2_control_plugin definition. It is confusing to me because this seems to be an error with the way the package is setup since I'm starting with a fresh clone.

[ERROR] [launch]: Caught exception in launch (see debug for traceback): executed command failed. Command: /opt/ros/humble/bin/xacro /home/leo/moveit_planning/install/xarm_description/share/xarm_description/urdf/xarm_device.urdf.xacro prefix:= hw_ns:=xarm limited:=false effort_control:=false velocity_control:=false ros2_control_plugin:=uf_robot_hardware/UFRobotFakeSystemHardware ros2_control_params:= mesh_suffix:=stl attach_to:=world attach_xyz:=0 0 0 attach_rpy:=0 0 0 add_gripper:=False add_vacuum_gripper:=false add_bio_gripper:=False model1300:=false dof:=6 robot_ip:= robot_type:=xarm robot_sn:= report_type:=normal baud_checkset:=true default_gripper_baud:=2000000 kinematics_suffix:= add_realsense_d435i:=false add_d435i_links:=true use_gazebo_camera:=false add_other_geometry:=false geometry_type:=box geometry_mass:=0.1 geometry_height:=0.1 geometry_radius:=0.1 geometry_length:=0.1 geometry_width:=0.1 geometry_mesh_filename:= geometry_mesh_origin_xyz:=0 0 0 geometry_mesh_origin_rpy:=0 0 0 geometry_mesh_tcp_xyz:=0 0 0 geometry_mesh_tcp_rpy:=0 0 0 
Captured stderr output: Usage: xacro [options] <input>

xacro: error: expected exactly one input file as argument

[vimior]

@Leopold-Klotz

Can you provide a complete launch script so I can try it? Or try the launch script we provide to see if this error is reported.

[Leopold-Klotz]

@vimior Here is a more detailed description of what I'm trying to do. I would like to interact with the "move group interface" as I understand is typical when making use of moveit. To make this error example repeatable on all ends, I'll use the basic moveit2 tutorial which should work with any moveit_config.

Tutorial Example:

• Works If using the full tutorial setup, line 21: auto move_group_interface = MoveGroupInterface(node, "manipulator");
• Works In my case I have a locally built franka panda arm moveit config so line 21: auto move_group_interface = MoveGroupInterface(node, "panda_arm");
• Does not work Or in the case when trying to get it to work with the xarm6 I have used: auto move_group_interface = MoveGroupInterface(node, "xarm6");
• With the full node code being:

  
  #include <memory>

include <rclcpp/rclcpp.hpp>

include <moveit/move_group_interface/move_group_interface.h>

int main(int argc, char * argv[]) { // Initialize ROS and create the Node rclcpp::init(argc, argv); auto const node = std::make_shared( "hello_moveit", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true) );

// Create a ROS logger auto const logger = rclcpp::get_logger("hello_moveit");

// Next step goes here // Create the MoveIt MoveGroup Interface using moveit::planning_interface::MoveGroupInterface; auto move_group_interface = MoveGroupInterface(node, "xarm6");

// Set a target Pose auto const target_pose = []{ geometry_msgs::msg::Pose msg; msg.orientation.w = 1.0; msg.position.x = 0.28; msg.position.y = -0.2; msg.position.z = 0.5; return msg; }(); move_group_interface.setPoseTarget(target_pose);

// Create a plan to that target pose auto const [success, plan] = [&move_group_interface]{ moveit::planning_interface::MoveGroupInterface::Plan msg; auto const ok = static_cast(move_group_interface.plan(msg)); return std::make_pair(ok, msg); }();

// Execute the plan if(success) { move_group_interface.execute(plan); } else { RCLCPP_ERROR(logger, "Planning failed!"); } // Shutdown ROS rclcpp::shutdown(); return 0; }

In terminal one both of these commands have been tried [(tutorial equivalent)](https://moveit.picknik.ai/main/doc/tutorials/quickstart_in_rviz/quickstart_in_rviz_tutorial.html):
- **Ideal, if working** `ros2 launch xarm_moveit_config xarm6_moveit_fake.launch.py`
- **Acceptable, if working** `ros2 launch xarm_planner xarm6_planner_fake.launch.py`
- Which to my understanding the move group is "xarm6"

And in terminal two (run tutorial):
- `ros2 run hello_moveit hello_moveit`
  - Where the move group has been set as said above
  - When using either the tutorial setup or my local franka panda config the arm successfully moves to a point with output:

leo@quadro-ubuntu:~/new_ws$ ros2 run hello_moveit hello_moveit [INFO] [1720725359.193169140] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.326247 seconds [INFO] [1720725359.193287619] [moveit_robot_model.robot_model]: Loading robot model 'panda'... [WARN] [1720725359.207580425] [moveit_ros.robot_model_loader]: No kinematics plugins defined. Fill and load kinematics.yaml! [INFO] [1720725359.221583237] [move_group_interface]: Ready to take commands for planning group panda_arm. [INFO] [1720725359.221707095] [move_group_interface]: MoveGroup action client/server ready [INFO] [1720725359.222208128] [move_group_interface]: Planning request accepted [INFO] [1720725359.277719374] [move_group_interface]: Planning request complete! [INFO] [1720725359.278544144] [move_group_interface]: time taken to generate plan: 0.0118378 seconds [INFO] [1720725359.280469543] [move_group_interface]: Execute request accepted [INFO] [1720725369.716442837] [move_group_interface]: Execute request success!

- When using the provided two xarm launches the error output is:

leo@quadro-ubuntu:~/new_ws$ ros2 run hello_moveit hello_moveit [ERROR] [1720724647.512773402] [hello_moveit]: Could not find parameter robot_description_semantic and did not receive robot_description_semantic via std_msgs::msg::String subscription within 10.000000 seconds. Error: Could not parse the SRDF XML File. Error=XML_ERROR_EMPTY_DOCUMENT ErrorID=13 (0xd) Line number=0 at line 715 in ./src/model.cpp [ERROR] [1720724647.518759074] [moveit_rdf_loader.rdf_loader]: Unable to parse SRDF [FATAL] [1720724647.519070217] [move_group_interface]: Unable to construct robot model. Please make sure all needed information is on the parameter server. terminate called after throwing an instance of 'std::runtime_error' what(): Unable to construct robot model. Please make sure all needed information is on the parameter server.

Which has led me to the belief that the xarm_ros2 package isn't correctly launching all of the necessary items for moveit2 planning (robot_description_semantic is missing). To clarify, using the provided launches *is the approach which I would like to take*, making full use of the UFactory provided moveit config. **If there is a solution to this or I am making use of the UFactory moveit config wrong, please let me know as that would be the quickest/easiest solution!**

In efforts to get around this I have tried to create my own launch using the MoveitConfigsBuilder (from uf_ros_lib) with the necessary descriptions being published which is where this (#81) issue surfaced. I don't think that this custom launch file should be necessary, but if a custom launch file is needed please provide me with an example which would work. I appreciate the help and I hope that there is something obvious which I am missing. 

[penglongxiang]

@Leopold-Klotz , currently the robot_description_semantic is kept as a internal parameter for the node move_group and was not published as global topic. Please make some modification to the file _robot_moveit_common.launch.py inside xarm_moveit_config package, by adding one additional parameter to move_group node:

{'publish_robot_description_semantic': True}

After the modification, the move_group_node part should look like this:

# Start the actual move_group node/action server
    move_group_node = Node(
        package='moveit_ros_move_group',
        executable='move_group',
        output='screen',
        parameters=[
            robot_description_parameters,
            ompl_planning_pipeline_config,
            trajectory_execution,
            plan_execution,
            moveit_controllers,
            planning_scene_monitor_parameters,
            # sensor_manager_parameters,
            {'use_sim_time': use_sim_time},
            {'publish_robot_description_semantic': True},
        ],
    )

In this way, robot_description_semantic would be published as a topic. Please try again running your hello_moveit node while specifying "xarm6" as planning group.

[Leopold-Klotz]

@Leopold-Klotz , currently the robot_description_semantic is kept as a internal parameter for the node move_group and was not published as global topic. Please make some modification to the file _robot_moveit_common.launch.py inside xarm_moveit_config package, by adding one additional parameter to move_group node:

{'publish_robot_description_semantic': True}

After the modification, the move_group_node part should look like this:

# Start the actual move_group node/action server
    move_group_node = Node(
        package='moveit_ros_move_group',
        executable='move_group',
        output='screen',
        parameters=[
            robot_description_parameters,
            ompl_planning_pipeline_config,
            trajectory_execution,
            plan_execution,
            moveit_controllers,
            planning_scene_monitor_parameters,
            # sensor_manager_parameters,
            {'use_sim_time': use_sim_time},
            {'publish_robot_description_semantic': True},
        ],
    )

In this way, robot_description_semantic would be published as a topic. Please try again running your hello_moveit node while specifying "xarm6" as planning group.

Thank you, I can confirm that doing this fixes the interaction with the move group interface and removed my need for MoveItConfigsBuilder. IMO, I would suggest releasing the entire package with this modification so that the xarm_moveit_config can be used as (from what I understand) is the universal way for interacting with MoveIt2. If I can help in this update please let me know.