From 1c86423c28f9148aaf5f5b9ad7fa23a1e746200d Mon Sep 17 00:00:00 2001 From: gadorneles Date: Sat, 31 Jan 2026 15:14:59 -0300 Subject: [PATCH 1/5] [feat] enable gripper features and add MTC capabilities in launch files --- xarm_api/config/xarm_params.yaml | 10 +++---- .../launch/_robot_moveit_common.launch.py | 26 ++++++++++++------- .../launch/_robot_moveit_common2.launch.py | 14 +++++++--- .../launch/_robot_moveit_fake.launch.py | 26 +++++++++++++++++-- .../launch/_robot_moveit_realmove.launch.py | 13 +++++++--- .../launch/xarm6_moveit_fake.launch.py | 2 ++ xarm_planner/CMakeLists.txt | 3 +++ 7 files changed, 69 insertions(+), 25 deletions(-) diff --git a/xarm_api/config/xarm_params.yaml b/xarm_api/config/xarm_params.yaml index e663933d..aca6ea84 100644 --- a/xarm_api/config/xarm_params.yaml +++ b/xarm_api/config/xarm_params.yaml @@ -55,8 +55,8 @@ ufactory_driver: set_state: true set_collision_sensitivity: false set_teach_sensitivity: false - set_gripper_mode: false - set_gripper_enable: false + set_gripper_mode: true + set_gripper_enable: true set_tgpio_modbus_timeout: false set_bio_gripper_speed: false set_collision_rebound: false @@ -83,7 +83,7 @@ ufactory_driver: set_tgpio_modbus_baudrate: false get_checkset_default_baud: false set_checkset_default_baud: false - get_gripper_position: false + get_gripper_position: true get_position: false get_servo_angle: true get_position_aa: false @@ -93,7 +93,7 @@ ufactory_driver: set_tcp_maxacc: false set_joint_jerk: false set_joint_maxacc: false - set_gripper_speed: false + set_gripper_speed: true set_reduced_max_tcp_speed: false set_reduced_max_joint_speed: false set_gravity_direction: false @@ -123,7 +123,7 @@ ufactory_driver: set_cgpio_analog: false set_cgpio_analog_with_xyz: false set_vacuum_gripper: false - set_gripper_position: false + set_gripper_position: true set_bio_gripper_enable: false open_bio_gripper: false close_bio_gripper: false diff --git a/xarm_moveit_config/launch/_robot_moveit_common.launch.py b/xarm_moveit_config/launch/_robot_moveit_common.launch.py index b4033c8f..116a645c 100644 --- a/xarm_moveit_config/launch/_robot_moveit_common.launch.py +++ b/xarm_moveit_config/launch/_robot_moveit_common.launch.py @@ -60,6 +60,7 @@ def launch_setup(context, *args, **kwargs): kinematics_suffix = LaunchConfiguration('kinematics_suffix', default='') use_sim_time = LaunchConfiguration('use_sim_time', default=False) + add_mtc = LaunchConfiguration('add_mtc', default=False) moveit_config_package_name = 'xarm_moveit_config' xarm_type = '{}{}'.format(robot_type.perform(context), dof.perform(context) if robot_type.perform(context) in ('xarm', 'lite') else '') @@ -237,21 +238,26 @@ def launch_setup(context, *args, **kwargs): # 'ros.filtered_cloud_topic': 'filtered_cloud', # } + # MTC ExecuteTaskSolution capability (requires ros-${ROS_DISTRO}-moveit-task-constructor-capabilities) + move_group_params = [ + 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}, + ] + if add_mtc.perform(context) in ('True', 'true'): + move_group_params.append({'capabilities': 'move_group/ExecuteTaskSolutionCapability'}) + # 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}, - ], + parameters=move_group_params, ) # rviz with moveit configuration diff --git a/xarm_moveit_config/launch/_robot_moveit_common2.launch.py b/xarm_moveit_config/launch/_robot_moveit_common2.launch.py index fdc32553..efae4e91 100644 --- a/xarm_moveit_config/launch/_robot_moveit_common2.launch.py +++ b/xarm_moveit_config/launch/_robot_moveit_common2.launch.py @@ -30,20 +30,26 @@ def launch_setup(context, *args, **kwargs): use_sim_time = LaunchConfiguration('use_sim_time', default=False) moveit_config_dump = LaunchConfiguration('moveit_config_dump') rviz_config = LaunchConfiguration('rviz_config', default='') + add_mtc = LaunchConfiguration('add_mtc', default=False) moveit_config_dump = moveit_config_dump.perform(context) moveit_config_dict = yaml.load(moveit_config_dump, Loader=yaml.FullLoader) moveit_config_package_name = 'xarm_moveit_config' + # MTC ExecuteTaskSolution capability (requires ros-${ROS_DISTRO}-moveit-task-constructor-capabilities) + move_group_params = [ + moveit_config_dict, + {'use_sim_time': use_sim_time}, + ] + if add_mtc.perform(context) in ('True', 'true'): + move_group_params.append({'capabilities': 'move_group/ExecuteTaskSolutionCapability'}) + # Start the actual move_group node/action server move_group_node = Node( package='moveit_ros_move_group', executable='move_group', output='screen', - parameters=[ - moveit_config_dict, - {'use_sim_time': use_sim_time}, - ], + parameters=move_group_params, ) # rviz with moveit configuration diff --git a/xarm_moveit_config/launch/_robot_moveit_fake.launch.py b/xarm_moveit_config/launch/_robot_moveit_fake.launch.py index 8782ff21..fdcc1195 100644 --- a/xarm_moveit_config/launch/_robot_moveit_fake.launch.py +++ b/xarm_moveit_config/launch/_robot_moveit_fake.launch.py @@ -54,6 +54,7 @@ def launch_setup(context, *args, **kwargs): geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"') no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False) + add_mtc = LaunchConfiguration('add_mtc', default=False) ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context) ros2_control_plugin = 'uf_robot_hardware/UFRobotFakeSystemHardware' @@ -69,7 +70,8 @@ def launch_setup(context, *args, **kwargs): robot_type=robot_type.perform(context) ) - moveit_config = MoveItConfigsBuilder( + moveit_config = ( + MoveItConfigsBuilder( context=context, controllers_name=controllers_name, dof=dof, @@ -105,7 +107,12 @@ def launch_setup(context, *args, **kwargs): geometry_mesh_origin_rpy=geometry_mesh_origin_rpy, geometry_mesh_tcp_xyz=geometry_mesh_tcp_xyz, geometry_mesh_tcp_rpy=geometry_mesh_tcp_rpy, - ).to_moveit_configs() + ) + .planning_scene_monitor( + publish_robot_description=True, publish_robot_description_semantic=True + ) + .to_moveit_configs() + ) # robot description launch # xarm_description/launch/_robot_description.launch.py @@ -126,6 +133,7 @@ def launch_setup(context, *args, **kwargs): 'attach_xyz': attach_xyz, 'attach_rpy': attach_rpy, 'no_gui_ctrl': no_gui_ctrl, + 'add_mtc': add_mtc, 'use_sim_time': 'false', 'moveit_config_dump': yaml.dump(moveit_config.to_dict()), }.items(), @@ -170,11 +178,25 @@ def launch_setup(context, *args, **kwargs): ], )) + package_shared_path = get_package_share_directory("moveit_task_constructor_demo") + task_constructor_node = Node( + package="moveit_task_constructor_demo", + executable="pick_place_demo", + name="moveit_task_constructor_demo", + namespace=ros_namespace, + output="screen", + parameters=[ + moveit_config.to_dict(), + os.path.join(package_shared_path, "config", "xarm6_config.yaml"), + ], + ) + return [ robot_description_launch, robot_moveit_common_launch, joint_state_broadcaster, ros2_control_launch, + #task_constructor_node, ] + controller_nodes diff --git a/xarm_moveit_config/launch/_robot_moveit_realmove.launch.py b/xarm_moveit_config/launch/_robot_moveit_realmove.launch.py index 120a3667..2a2b02bb 100644 --- a/xarm_moveit_config/launch/_robot_moveit_realmove.launch.py +++ b/xarm_moveit_config/launch/_robot_moveit_realmove.launch.py @@ -74,7 +74,8 @@ def launch_setup(context, *args, **kwargs): robot_type=robot_type.perform(context) ) - moveit_config = MoveItConfigsBuilder( + moveit_config = ( + MoveItConfigsBuilder( context=context, controllers_name=controllers_name, robot_ip=robot_ip, @@ -113,9 +114,13 @@ def launch_setup(context, *args, **kwargs): geometry_mesh_origin_xyz=geometry_mesh_origin_xyz, geometry_mesh_origin_rpy=geometry_mesh_origin_rpy, geometry_mesh_tcp_xyz=geometry_mesh_tcp_xyz, - geometry_mesh_tcp_rpy=geometry_mesh_tcp_rpy, - - ).to_moveit_configs() + geometry_mesh_tcp_rpy=geometry_mesh_tcp_rpy, + ) + .planning_scene_monitor( + publish_robot_description=True, publish_robot_description_semantic=True + ) + .to_moveit_configs() + ) # robot description launch # xarm_description/launch/_robot_description.launch.py diff --git a/xarm_moveit_config/launch/xarm6_moveit_fake.launch.py b/xarm_moveit_config/launch/xarm6_moveit_fake.launch.py index 61304972..98a40437 100644 --- a/xarm_moveit_config/launch/xarm6_moveit_fake.launch.py +++ b/xarm_moveit_config/launch/xarm6_moveit_fake.launch.py @@ -15,6 +15,7 @@ def generate_launch_description(): hw_ns = LaunchConfiguration('hw_ns', default='xarm') + add_mtc = LaunchConfiguration('add_mtc', default='false') # robot moveit fake launch # xarm_moveit_config/launch/_robot_moveit_fake.launch.py @@ -25,6 +26,7 @@ def generate_launch_description(): 'robot_type': 'xarm', 'hw_ns': hw_ns, 'no_gui_ctrl': 'false', + 'add_mtc': add_mtc, }.items(), ) diff --git a/xarm_planner/CMakeLists.txt b/xarm_planner/CMakeLists.txt index b0967280..a6de93d2 100644 --- a/xarm_planner/CMakeLists.txt +++ b/xarm_planner/CMakeLists.txt @@ -160,4 +160,7 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) +ament_export_include_directories(include) +ament_export_libraries(xarm_planner) + ament_package() From e7e137b6406696a2a003f66cd6df98603d86a663 Mon Sep 17 00:00:00 2001 From: gadorneles Date: Sat, 31 Jan 2026 15:25:48 -0300 Subject: [PATCH 2/5] revert a few changes for the pr --- xarm_api/config/xarm_params.yaml | 8 +++--- .../launch/_robot_moveit_fake.launch.py | 26 +++---------------- .../launch/_robot_moveit_realmove.launch.py | 10 ++----- xarm_planner/CMakeLists.txt | 3 --- 4 files changed, 9 insertions(+), 38 deletions(-) diff --git a/xarm_api/config/xarm_params.yaml b/xarm_api/config/xarm_params.yaml index aca6ea84..3f0511f2 100644 --- a/xarm_api/config/xarm_params.yaml +++ b/xarm_api/config/xarm_params.yaml @@ -55,8 +55,8 @@ ufactory_driver: set_state: true set_collision_sensitivity: false set_teach_sensitivity: false - set_gripper_mode: true - set_gripper_enable: true + set_gripper_mode: false + set_gripper_enable: false set_tgpio_modbus_timeout: false set_bio_gripper_speed: false set_collision_rebound: false @@ -93,7 +93,7 @@ ufactory_driver: set_tcp_maxacc: false set_joint_jerk: false set_joint_maxacc: false - set_gripper_speed: true + set_gripper_speed: false set_reduced_max_tcp_speed: false set_reduced_max_joint_speed: false set_gravity_direction: false @@ -123,7 +123,7 @@ ufactory_driver: set_cgpio_analog: false set_cgpio_analog_with_xyz: false set_vacuum_gripper: false - set_gripper_position: true + set_gripper_position: false set_bio_gripper_enable: false open_bio_gripper: false close_bio_gripper: false diff --git a/xarm_moveit_config/launch/_robot_moveit_fake.launch.py b/xarm_moveit_config/launch/_robot_moveit_fake.launch.py index fdcc1195..46af1bb8 100644 --- a/xarm_moveit_config/launch/_robot_moveit_fake.launch.py +++ b/xarm_moveit_config/launch/_robot_moveit_fake.launch.py @@ -70,8 +70,7 @@ def launch_setup(context, *args, **kwargs): robot_type=robot_type.perform(context) ) - moveit_config = ( - MoveItConfigsBuilder( + moveit_config = MoveItConfigsBuilder( context=context, controllers_name=controllers_name, dof=dof, @@ -107,13 +106,8 @@ def launch_setup(context, *args, **kwargs): geometry_mesh_origin_rpy=geometry_mesh_origin_rpy, geometry_mesh_tcp_xyz=geometry_mesh_tcp_xyz, geometry_mesh_tcp_rpy=geometry_mesh_tcp_rpy, - ) - .planning_scene_monitor( - publish_robot_description=True, publish_robot_description_semantic=True - ) - .to_moveit_configs() - ) - + ).to_moveit_configs() + # robot description launch # xarm_description/launch/_robot_description.launch.py robot_description_launch = IncludeLaunchDescription( @@ -178,25 +172,11 @@ def launch_setup(context, *args, **kwargs): ], )) - package_shared_path = get_package_share_directory("moveit_task_constructor_demo") - task_constructor_node = Node( - package="moveit_task_constructor_demo", - executable="pick_place_demo", - name="moveit_task_constructor_demo", - namespace=ros_namespace, - output="screen", - parameters=[ - moveit_config.to_dict(), - os.path.join(package_shared_path, "config", "xarm6_config.yaml"), - ], - ) - return [ robot_description_launch, robot_moveit_common_launch, joint_state_broadcaster, ros2_control_launch, - #task_constructor_node, ] + controller_nodes diff --git a/xarm_moveit_config/launch/_robot_moveit_realmove.launch.py b/xarm_moveit_config/launch/_robot_moveit_realmove.launch.py index 2a2b02bb..53e85c26 100644 --- a/xarm_moveit_config/launch/_robot_moveit_realmove.launch.py +++ b/xarm_moveit_config/launch/_robot_moveit_realmove.launch.py @@ -74,8 +74,7 @@ def launch_setup(context, *args, **kwargs): robot_type=robot_type.perform(context) ) - moveit_config = ( - MoveItConfigsBuilder( + moveit_config = MoveItConfigsBuilder( context=context, controllers_name=controllers_name, robot_ip=robot_ip, @@ -115,12 +114,7 @@ def launch_setup(context, *args, **kwargs): geometry_mesh_origin_rpy=geometry_mesh_origin_rpy, geometry_mesh_tcp_xyz=geometry_mesh_tcp_xyz, geometry_mesh_tcp_rpy=geometry_mesh_tcp_rpy, - ) - .planning_scene_monitor( - publish_robot_description=True, publish_robot_description_semantic=True - ) - .to_moveit_configs() - ) + ).to_moveit_configs() # robot description launch # xarm_description/launch/_robot_description.launch.py diff --git a/xarm_planner/CMakeLists.txt b/xarm_planner/CMakeLists.txt index a6de93d2..b0967280 100644 --- a/xarm_planner/CMakeLists.txt +++ b/xarm_planner/CMakeLists.txt @@ -160,7 +160,4 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) -ament_export_include_directories(include) -ament_export_libraries(xarm_planner) - ament_package() From e742c9ef59c467c6309d103e4130fafec15279f8 Mon Sep 17 00:00:00 2001 From: gadorneles Date: Sat, 31 Jan 2026 15:27:49 -0300 Subject: [PATCH 3/5] [fix] clean up launch file formatting --- xarm_api/config/xarm_params.yaml | 2 +- xarm_moveit_config/launch/_robot_moveit_realmove.launch.py | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/xarm_api/config/xarm_params.yaml b/xarm_api/config/xarm_params.yaml index 3f0511f2..e663933d 100644 --- a/xarm_api/config/xarm_params.yaml +++ b/xarm_api/config/xarm_params.yaml @@ -83,7 +83,7 @@ ufactory_driver: set_tgpio_modbus_baudrate: false get_checkset_default_baud: false set_checkset_default_baud: false - get_gripper_position: true + get_gripper_position: false get_position: false get_servo_angle: true get_position_aa: false diff --git a/xarm_moveit_config/launch/_robot_moveit_realmove.launch.py b/xarm_moveit_config/launch/_robot_moveit_realmove.launch.py index 53e85c26..69fcae90 100644 --- a/xarm_moveit_config/launch/_robot_moveit_realmove.launch.py +++ b/xarm_moveit_config/launch/_robot_moveit_realmove.launch.py @@ -113,7 +113,8 @@ def launch_setup(context, *args, **kwargs): geometry_mesh_origin_xyz=geometry_mesh_origin_xyz, geometry_mesh_origin_rpy=geometry_mesh_origin_rpy, geometry_mesh_tcp_xyz=geometry_mesh_tcp_xyz, - geometry_mesh_tcp_rpy=geometry_mesh_tcp_rpy, + geometry_mesh_tcp_rpy=geometry_mesh_tcp_rpy, + ).to_moveit_configs() # robot description launch From 2ada57584a69a2079cce1d8958915640787253ab Mon Sep 17 00:00:00 2001 From: gadorneles Date: Sat, 31 Jan 2026 15:28:29 -0300 Subject: [PATCH 4/5] fix: remove unnecessary whitespace in launch file --- xarm_moveit_config/launch/_robot_moveit_realmove.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/xarm_moveit_config/launch/_robot_moveit_realmove.launch.py b/xarm_moveit_config/launch/_robot_moveit_realmove.launch.py index 69fcae90..18648d5e 100644 --- a/xarm_moveit_config/launch/_robot_moveit_realmove.launch.py +++ b/xarm_moveit_config/launch/_robot_moveit_realmove.launch.py @@ -114,7 +114,7 @@ def launch_setup(context, *args, **kwargs): geometry_mesh_origin_rpy=geometry_mesh_origin_rpy, geometry_mesh_tcp_xyz=geometry_mesh_tcp_xyz, geometry_mesh_tcp_rpy=geometry_mesh_tcp_rpy, - + ).to_moveit_configs() # robot description launch From 594ab35479218c7da0687f484584857db2d99677 Mon Sep 17 00:00:00 2001 From: gadorneles Date: Sat, 31 Jan 2026 15:29:19 -0300 Subject: [PATCH 5/5] fix: remove unnecessary whitespace in launch_setup function --- xarm_moveit_config/launch/_robot_moveit_realmove.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/xarm_moveit_config/launch/_robot_moveit_realmove.launch.py b/xarm_moveit_config/launch/_robot_moveit_realmove.launch.py index 18648d5e..120a3667 100644 --- a/xarm_moveit_config/launch/_robot_moveit_realmove.launch.py +++ b/xarm_moveit_config/launch/_robot_moveit_realmove.launch.py @@ -114,7 +114,7 @@ def launch_setup(context, *args, **kwargs): geometry_mesh_origin_rpy=geometry_mesh_origin_rpy, geometry_mesh_tcp_xyz=geometry_mesh_tcp_xyz, geometry_mesh_tcp_rpy=geometry_mesh_tcp_rpy, - + ).to_moveit_configs() # robot description launch