From 34f26fcb7916dd3dbd2b01b29ae0fbe0b2ae2172 Mon Sep 17 00:00:00 2001 From: NikolaRaicevic Date: Mon, 23 Sep 2024 12:31:06 -0700 Subject: [PATCH 01/30] Robot position --- xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py index 5e94bad1..142e8eb4 100644 --- a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py +++ b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py @@ -109,8 +109,8 @@ def get_per_robot_stack(robot_idx, load_controller): arguments=[ '-topic', f'robot_description', '-allow_renaming', 'false', - '-x', str(-0.4 + robot_idx * 0.4), - '-y', '-0.5', + '-x', str(0.0 + robot_idx * 0.4), + '-y', '-0.3', '-z', '1.021', '-Y', '1.571', '-timeout', '10000', From 5e6ac770a7f6ac2fe513e0ce7480dc67ee5788c9 Mon Sep 17 00:00:00 2001 From: NikolaRaicevic Date: Wed, 25 Sep 2024 12:17:44 -0700 Subject: [PATCH 02/30] _robot_joint_state.launch.py fixed --- .../launch/_robot_joint_state.launch.py | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/xarm_description/launch/_robot_joint_state.launch.py b/xarm_description/launch/_robot_joint_state.launch.py index e9ffff24..767165fd 100644 --- a/xarm_description/launch/_robot_joint_state.launch.py +++ b/xarm_description/launch/_robot_joint_state.launch.py @@ -1,26 +1,18 @@ #!/usr/bin/env python3 -# Software License Agreement (BSD License) -# -# Copyright (c) 2021, UFACTORY, Inc. -# All rights reserved. -# -# Author: Vinman from launch import LaunchDescription -from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument +from launch.actions import OpaqueFunction, IncludeLaunchDescription +from launch_ros.actions import Node from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - -def generate_launch_description(context, *args, **kwargs): +def launch_setup(context, *args, **kwargs): prefix = LaunchConfiguration('prefix', default='') hw_ns = LaunchConfiguration('hw_ns', default='xarm') ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='uf_robot_hardware/UFRobotSystemHardware') # robot description launch - # xarm_description/launch/_robot_description.launch.py robot_description_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_description'), 'launch', '_robot_description.launch.py'])), launch_arguments={ @@ -39,7 +31,15 @@ def generate_launch_description(context, *args, **kwargs): parameters=[{'source_list': ['{}{}/joint_states'.format(prefix.perform(context), hw_ns.perform(context))]}], ) - return LaunchDescription([ + return [ robot_description_launch, - joint_state_publisher_node, - ]) \ No newline at end of file + joint_state_publisher_node + ] + +def generate_launch_description(): + return LaunchDescription([ + OpaqueFunction(function=launch_setup) + ]) + + + From e509cd381bcb219ebbacd875e5c6ab40ebaa25ac Mon Sep 17 00:00:00 2001 From: NikolaRaicevic Date: Thu, 26 Sep 2024 08:56:30 -0700 Subject: [PATCH 03/30] moveit --- xarm_controller/launch/_ros2_control.launch.py | 6 ------ xarm_moveit_config/launch/_robot_moveit_fake.launch.py | 4 +++- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/xarm_controller/launch/_ros2_control.launch.py b/xarm_controller/launch/_ros2_control.launch.py index c3327018..776dcd20 100644 --- a/xarm_controller/launch/_ros2_control.launch.py +++ b/xarm_controller/launch/_ros2_control.launch.py @@ -1,10 +1,4 @@ #!/usr/bin/env python3 -# Software License Agreement (BSD License) -# -# Copyright (c) 2021, UFACTORY, Inc. -# All rights reserved. -# -# Author: Vinman import os from ament_index_python import get_package_share_directory diff --git a/xarm_moveit_config/launch/_robot_moveit_fake.launch.py b/xarm_moveit_config/launch/_robot_moveit_fake.launch.py index e730cb62..ba6de25a 100644 --- a/xarm_moveit_config/launch/_robot_moveit_fake.launch.py +++ b/xarm_moveit_config/launch/_robot_moveit_fake.launch.py @@ -17,7 +17,7 @@ def launch_setup(context, *args, **kwargs): prefix = LaunchConfiguration('prefix', default='') hw_ns = LaunchConfiguration('hw_ns', default='xarm') - + robot_type='xarm' # TODO - fix no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False) ros2_control_plugin = 'uf_robot_hardware/UFRobotFakeSystemHardware' @@ -36,6 +36,7 @@ def launch_setup(context, *args, **kwargs): 'prefix': prefix, 'hw_ns': hw_ns, 'ros2_control_plugin': ros2_control_plugin, + 'robot_type': robot_type, }.items(), ) @@ -87,6 +88,7 @@ def launch_setup(context, *args, **kwargs): 'prefix': prefix, 'hw_ns': hw_ns, 'ros2_control_plugin': ros2_control_plugin, + 'robot_type': robot_type, }.items(), ) From 55636ba65e83a8cadbd2e585e0b441f0e82c00d7 Mon Sep 17 00:00:00 2001 From: Brian Lee Date: Thu, 26 Sep 2024 08:59:06 -0700 Subject: [PATCH 04/30] fix control launch --- .../launch/_ros2_control.launch.py | 7 ++- .../launch/lib/robot_controller_lib.py | 44 +++++++++---------- 2 files changed, 25 insertions(+), 26 deletions(-) diff --git a/xarm_controller/launch/_ros2_control.launch.py b/xarm_controller/launch/_ros2_control.launch.py index c3327018..d584ba92 100644 --- a/xarm_controller/launch/_ros2_control.launch.py +++ b/xarm_controller/launch/_ros2_control.launch.py @@ -23,17 +23,16 @@ def launch_setup(context, *args, **kwargs): ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='uf_robot_hardware/UFRobotSystemHardware') xacro_file = LaunchConfiguration('xacro_file', default=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro'])) + xarm_type = 'xarm6' #TODO - fix + # ros2 control params # xarm_controller/launch/lib/robot_controller_lib.py mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('xarm_controller'), 'launch', 'lib', 'robot_controller_lib.py')) generate_ros2_control_params_temp_file = getattr(mod, 'generate_ros2_control_params_temp_file') ros2_control_params = generate_ros2_control_params_temp_file( - os.path.join(get_package_share_directory('xarm_controller'), 'config', '{}{}_controllers.yaml'.format(robot_type.perform(context), dof.perform(context) if robot_type.perform(context) in ('xarm', 'lite') else '')), + os.path.join(get_package_share_directory('xarm_controller'), 'config', f'{xarm_type}_controllers.yaml'), prefix=prefix.perform(context), - add_gripper=False, - add_bio_gripper=False, ros_namespace=LaunchConfiguration('ros_namespace', default='').perform(context), - robot_type='xarm' ) # robot_description diff --git a/xarm_controller/launch/lib/robot_controller_lib.py b/xarm_controller/launch/lib/robot_controller_lib.py index 16f9505b..e7d5b23e 100644 --- a/xarm_controller/launch/lib/robot_controller_lib.py +++ b/xarm_controller/launch/lib/robot_controller_lib.py @@ -34,32 +34,32 @@ def add_prefix_to_ros2_control_params(prefix, ros2_control_params): controller_manager_ros__parameters[new_name] = controller_manager_ros__parameters.pop(name) -def generate_ros2_control_params_temp_file(ros2_control_params_path, prefix='', add_gripper=False, add_bio_gripper=False, ros_namespace='', update_rate=None, robot_type='xarm'): - if ros_namespace or prefix or add_gripper or add_bio_gripper or update_rate: +def generate_ros2_control_params_temp_file(ros2_control_params_path, prefix='', ros_namespace='', update_rate=None): + if ros_namespace or prefix or update_rate: with open(ros2_control_params_path, 'r') as f: ros2_control_params_yaml = yaml.safe_load(f) if update_rate is not None: ros2_control_params_yaml['controller_manager']['ros__parameters']['update_rate'] = update_rate - if add_gripper: - gripper_control_params_path = os.path.join(get_package_share_directory('xarm_controller'), 'config', '{}_gripper_controllers.yaml'.format(robot_type)) - # check file is exists or not - if os.path.exists(gripper_control_params_path): - with open(gripper_control_params_path, 'r') as f: - gripper_control_params_yaml = yaml.safe_load(f) - for name, value in gripper_control_params_yaml['controller_manager']['ros__parameters'].items(): - ros2_control_params_yaml['controller_manager']['ros__parameters'][name] = value - if name in gripper_control_params_yaml: - ros2_control_params_yaml[name] = gripper_control_params_yaml[name] - elif add_bio_gripper: - gripper_control_params_path = os.path.join(get_package_share_directory('xarm_controller'), 'config', 'bio_gripper_controllers.yaml') - # check file is exists or not - if os.path.exists(gripper_control_params_path): - with open(gripper_control_params_path, 'r') as f: - gripper_control_params_yaml = yaml.safe_load(f) - for name, value in gripper_control_params_yaml['controller_manager']['ros__parameters'].items(): - ros2_control_params_yaml['controller_manager']['ros__parameters'][name] = value - if name in gripper_control_params_yaml: - ros2_control_params_yaml[name] = gripper_control_params_yaml[name] + # if add_gripper: + # gripper_control_params_path = os.path.join(get_package_share_directory('xarm_controller'), 'config', '{}_gripper_controllers.yaml'.format(robot_type)) + # # check file is exists or not + # if os.path.exists(gripper_control_params_path): + # with open(gripper_control_params_path, 'r') as f: + # gripper_control_params_yaml = yaml.safe_load(f) + # for name, value in gripper_control_params_yaml['controller_manager']['ros__parameters'].items(): + # ros2_control_params_yaml['controller_manager']['ros__parameters'][name] = value + # if name in gripper_control_params_yaml: + # ros2_control_params_yaml[name] = gripper_control_params_yaml[name] + # elif add_bio_gripper: + # gripper_control_params_path = os.path.join(get_package_share_directory('xarm_controller'), 'config', 'bio_gripper_controllers.yaml') + # # check file is exists or not + # if os.path.exists(gripper_control_params_path): + # with open(gripper_control_params_path, 'r') as f: + # gripper_control_params_yaml = yaml.safe_load(f) + # for name, value in gripper_control_params_yaml['controller_manager']['ros__parameters'].items(): + # ros2_control_params_yaml['controller_manager']['ros__parameters'][name] = value + # if name in gripper_control_params_yaml: + # ros2_control_params_yaml[name] = gripper_control_params_yaml[name] add_prefix_to_ros2_control_params(prefix, ros2_control_params_yaml) From 52aea05224e9c7028c3abb29fa7298049f48f9ff Mon Sep 17 00:00:00 2001 From: Brian Lee Date: Thu, 26 Sep 2024 09:48:56 -0700 Subject: [PATCH 05/30] propagate changes to gazebo --- xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py index 142e8eb4..3ba68b16 100644 --- a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py +++ b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py @@ -31,11 +31,8 @@ def build_robot_description( 'config', 'xarm6_controllers.yaml' ), prefix=this_robot_prefix, - add_gripper=False, #TODO: fix this later - add_bio_gripper=False, ros_namespace=this_robot_namespace, update_rate=1000, - robot_type='xarm' ) print(f"Generated temporary control params at {ros2_control_params}") From 4f51fc3bf7485838dde62fe103a4602ab3bc7901 Mon Sep 17 00:00:00 2001 From: NikolaRaicevic Date: Thu, 26 Sep 2024 09:50:53 -0700 Subject: [PATCH 06/30] Incorporating moveit in xarm6 --- .../launch/lib/robot_controller_lib.py | 48 +++++----- .../launch/_robot_description.launch.py | 6 -- .../_robot_beside_table_gazebo.launch.py | 90 ++++++++++++++++++- 3 files changed, 109 insertions(+), 35 deletions(-) diff --git a/xarm_controller/launch/lib/robot_controller_lib.py b/xarm_controller/launch/lib/robot_controller_lib.py index e7d5b23e..b5a5bb75 100644 --- a/xarm_controller/launch/lib/robot_controller_lib.py +++ b/xarm_controller/launch/lib/robot_controller_lib.py @@ -1,10 +1,4 @@ #!/usr/bin/env python3 -# Software License Agreement (BSD License) -# -# Copyright (c) 2021, UFACTORY, Inc. -# All rights reserved. -# -# Author: Vinman import os import yaml @@ -34,32 +28,32 @@ def add_prefix_to_ros2_control_params(prefix, ros2_control_params): controller_manager_ros__parameters[new_name] = controller_manager_ros__parameters.pop(name) -def generate_ros2_control_params_temp_file(ros2_control_params_path, prefix='', ros_namespace='', update_rate=None): +def generate_ros2_control_params_temp_file(ros2_control_params_path, prefix='', add_gripper=False, add_bio_gripper=False, ros_namespace='', update_rate=None, robot_type='xarm'): if ros_namespace or prefix or update_rate: with open(ros2_control_params_path, 'r') as f: ros2_control_params_yaml = yaml.safe_load(f) if update_rate is not None: ros2_control_params_yaml['controller_manager']['ros__parameters']['update_rate'] = update_rate - # if add_gripper: - # gripper_control_params_path = os.path.join(get_package_share_directory('xarm_controller'), 'config', '{}_gripper_controllers.yaml'.format(robot_type)) - # # check file is exists or not - # if os.path.exists(gripper_control_params_path): - # with open(gripper_control_params_path, 'r') as f: - # gripper_control_params_yaml = yaml.safe_load(f) - # for name, value in gripper_control_params_yaml['controller_manager']['ros__parameters'].items(): - # ros2_control_params_yaml['controller_manager']['ros__parameters'][name] = value - # if name in gripper_control_params_yaml: - # ros2_control_params_yaml[name] = gripper_control_params_yaml[name] - # elif add_bio_gripper: - # gripper_control_params_path = os.path.join(get_package_share_directory('xarm_controller'), 'config', 'bio_gripper_controllers.yaml') - # # check file is exists or not - # if os.path.exists(gripper_control_params_path): - # with open(gripper_control_params_path, 'r') as f: - # gripper_control_params_yaml = yaml.safe_load(f) - # for name, value in gripper_control_params_yaml['controller_manager']['ros__parameters'].items(): - # ros2_control_params_yaml['controller_manager']['ros__parameters'][name] = value - # if name in gripper_control_params_yaml: - # ros2_control_params_yaml[name] = gripper_control_params_yaml[name] + if add_gripper: + gripper_control_params_path = os.path.join(get_package_share_directory('xarm_controller'), 'config', '{}_gripper_controllers.yaml'.format(robot_type)) + # check file is exists or not + if os.path.exists(gripper_control_params_path): + with open(gripper_control_params_path, 'r') as f: + gripper_control_params_yaml = yaml.safe_load(f) + for name, value in gripper_control_params_yaml['controller_manager']['ros__parameters'].items(): + ros2_control_params_yaml['controller_manager']['ros__parameters'][name] = value + if name in gripper_control_params_yaml: + ros2_control_params_yaml[name] = gripper_control_params_yaml[name] + elif add_bio_gripper: + gripper_control_params_path = os.path.join(get_package_share_directory('xarm_controller'), 'config', 'bio_gripper_controllers.yaml') + # check file is exists or not + if os.path.exists(gripper_control_params_path): + with open(gripper_control_params_path, 'r') as f: + gripper_control_params_yaml = yaml.safe_load(f) + for name, value in gripper_control_params_yaml['controller_manager']['ros__parameters'].items(): + ros2_control_params_yaml['controller_manager']['ros__parameters'][name] = value + if name in gripper_control_params_yaml: + ros2_control_params_yaml[name] = gripper_control_params_yaml[name] add_prefix_to_ros2_control_params(prefix, ros2_control_params_yaml) diff --git a/xarm_description/launch/_robot_description.launch.py b/xarm_description/launch/_robot_description.launch.py index 573794a1..af300b41 100644 --- a/xarm_description/launch/_robot_description.launch.py +++ b/xarm_description/launch/_robot_description.launch.py @@ -1,10 +1,4 @@ #!/usr/bin/env python3 -# Software License Agreement (BSD License) -# -# Copyright (c) 2021, UFACTORY, Inc. -# All rights reserved. -# -# Author: Vinman import os import sys diff --git a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py index 142e8eb4..e997c403 100644 --- a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py +++ b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py @@ -118,8 +118,95 @@ def get_per_robot_stack(robot_idx, load_controller): parameters=[{'use_sim_time': True}], ) + # Planning Configuration + xarm_type='xarm6' #TODO: fix + moveit_config_package_name = 'xarm_moveit_config' + controllers_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'fake_controllers.yaml') + + + mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory(moveit_config_package_name), 'launch', 'lib', 'robot_moveit_config_lib.py')) + load_yaml = getattr(mod, 'load_yaml') + ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'ompl_planning.yaml') + ompl_planning_pipeline_config = { + 'default_planning_pipeline': 'ompl', + 'planning_pipelines': ['ompl'], + } + if os.environ.get('ROS_DISTRO', '') > 'iron': + ompl_planning_pipeline_config['ompl'] = { + 'planning_plugins': ['ompl_interface/OMPLPlanner'], + 'request_adapters': [ + 'default_planning_request_adapters/ResolveConstraintFrames', + 'default_planning_request_adapters/ValidateWorkspaceBounds', + 'default_planning_request_adapters/CheckStartStateBounds', + 'default_planning_request_adapters/CheckStartStateCollision', + ], + 'response_adapters': [ + 'default_planning_response_adapters/AddTimeOptimalParameterization', + 'default_planning_response_adapters/ValidateSolution', + 'default_planning_response_adapters/DisplayMotionPath', + ], + } + else: + ompl_planning_pipeline_config['ompl'] = { + 'planning_plugin': 'ompl_interface/OMPLPlanner', + 'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", + 'start_state_max_bounds_error': 0.1, + } + ompl_planning_pipeline_config['ompl'].update(ompl_planning_yaml) + + # Moveit controllers Configuration + moveit_controllers = { + 'moveit_fake_controller_manager': controllers_yaml, + 'moveit_controller_manager': 'moveit_fake_controller_manager/MoveItFakeControllerManager', + } + + # Trajectory Execution Configuration + trajectory_execution = { + 'moveit_manage_controllers': True, + 'trajectory_execution.allowed_execution_duration_scaling': 1.2, + 'trajectory_execution.allowed_goal_duration_margin': 0.5, + 'trajectory_execution.allowed_start_tolerance': 0.01, + 'trajectory_execution.execution_duration_monitoring': False + } + + plan_execution = { + 'plan_execution.record_trajectory_state_frequency': 10.0, + } + + planning_scene_monitor_parameters = { + 'publish_planning_scene': True, + 'publish_geometry_updates': True, + 'publish_state_updates': True, + 'publish_transforms_updates': True, + # "planning_scene_monitor_options": { + # "name": "planning_scene_monitor", + # "robot_description": "robot_description", + # "joint_state_topic": "/joint_states", + # "attached_collision_object_topic": "/move_group/planning_scene_monitor", + # "publish_planning_scene_topic": "/move_group/publish_planning_scene", + # "monitored_planning_scene_topic": "/move_group/monitored_planning_scene", + # "wait_for_initial_state_timeout": 10.0, + # }, + } + + move_group_node = Node( + package='moveit_ros_move_group', + executable='move_group', + output='screen', + parameters=[ + '-topic', f'robot_description', + ompl_planning_pipeline_config, + trajectory_execution, + plan_execution, + moveit_controllers, + planning_scene_monitor_parameters, + {'use_sim_time': True}, + ], + ) + nodes_to_launch.append( - gazebo_spawn_entity_node + gazebo_spawn_entity_node, + # move_group_node, ) # Load controllers @@ -212,7 +299,6 @@ def generate_launch_description(): nodes_to_launch = [gazebo_launch, clock_parameter_bridge] - camera_robot_description = build_camera_description(this_robot_namespace = camera_namespace) robot_state_publisher_node_camera = Node( From b7ff27074e8fd80b16abaa4b9e21ea991bb2ddee Mon Sep 17 00:00:00 2001 From: NikolaRaicevic Date: Thu, 26 Sep 2024 10:41:33 -0700 Subject: [PATCH 07/30] Moveit in besides_table world --- .../_robot_beside_table_gazebo.launch.py | 109 +++++------------- 1 file changed, 27 insertions(+), 82 deletions(-) diff --git a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py index 22406d15..a162d62d 100644 --- a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py +++ b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py @@ -31,8 +31,11 @@ def build_robot_description( 'config', 'xarm6_controllers.yaml' ), prefix=this_robot_prefix, + add_gripper=False, #TODO: fix this later + add_bio_gripper=False, ros_namespace=this_robot_namespace, update_rate=1000, + robot_type='xarm' ) print(f"Generated temporary control params at {ros2_control_params}") @@ -115,95 +118,37 @@ def get_per_robot_stack(robot_idx, load_controller): parameters=[{'use_sim_time': True}], ) - # Planning Configuration - xarm_type='xarm6' #TODO: fix - moveit_config_package_name = 'xarm_moveit_config' - controllers_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'fake_controllers.yaml') + prefix = LaunchConfiguration('prefix', default='') + hw_ns = LaunchConfiguration('hw_ns', default='xarm') + robot_type='xarm' # TODO - fix + no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False) + ros2_control_plugin = 'uf_robot_hardware/UFRobotFakeSystemHardware' + controllers_name = 'fake_controllers' + moveit_controller_manager_key = 'moveit_simple_controller_manager' + moveit_controller_manager_value = 'moveit_simple_controller_manager/MoveItSimpleControllerManager' - mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory(moveit_config_package_name), 'launch', 'lib', 'robot_moveit_config_lib.py')) - load_yaml = getattr(mod, 'load_yaml') - ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'ompl_planning.yaml') - ompl_planning_pipeline_config = { - 'default_planning_pipeline': 'ompl', - 'planning_pipelines': ['ompl'], - } - if os.environ.get('ROS_DISTRO', '') > 'iron': - ompl_planning_pipeline_config['ompl'] = { - 'planning_plugins': ['ompl_interface/OMPLPlanner'], - 'request_adapters': [ - 'default_planning_request_adapters/ResolveConstraintFrames', - 'default_planning_request_adapters/ValidateWorkspaceBounds', - 'default_planning_request_adapters/CheckStartStateBounds', - 'default_planning_request_adapters/CheckStartStateCollision', - ], - 'response_adapters': [ - 'default_planning_response_adapters/AddTimeOptimalParameterization', - 'default_planning_response_adapters/ValidateSolution', - 'default_planning_response_adapters/DisplayMotionPath', - ], - } - else: - ompl_planning_pipeline_config['ompl'] = { - 'planning_plugin': 'ompl_interface/OMPLPlanner', - 'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", - 'start_state_max_bounds_error': 0.1, - } - ompl_planning_pipeline_config['ompl'].update(ompl_planning_yaml) - - # Moveit controllers Configuration - moveit_controllers = { - 'moveit_fake_controller_manager': controllers_yaml, - 'moveit_controller_manager': 'moveit_fake_controller_manager/MoveItFakeControllerManager', - } - # Trajectory Execution Configuration - trajectory_execution = { - 'moveit_manage_controllers': True, - 'trajectory_execution.allowed_execution_duration_scaling': 1.2, - 'trajectory_execution.allowed_goal_duration_margin': 0.5, - 'trajectory_execution.allowed_start_tolerance': 0.01, - 'trajectory_execution.execution_duration_monitoring': False - } - - plan_execution = { - 'plan_execution.record_trajectory_state_frequency': 10.0, - } - - planning_scene_monitor_parameters = { - 'publish_planning_scene': True, - 'publish_geometry_updates': True, - 'publish_state_updates': True, - 'publish_transforms_updates': True, - # "planning_scene_monitor_options": { - # "name": "planning_scene_monitor", - # "robot_description": "robot_description", - # "joint_state_topic": "/joint_states", - # "attached_collision_object_topic": "/move_group/planning_scene_monitor", - # "publish_planning_scene_topic": "/move_group/publish_planning_scene", - # "monitored_planning_scene_topic": "/move_group/monitored_planning_scene", - # "wait_for_initial_state_timeout": 10.0, - # }, - } - - move_group_node = Node( - package='moveit_ros_move_group', - executable='move_group', - output='screen', - parameters=[ - '-topic', f'robot_description', - ompl_planning_pipeline_config, - trajectory_execution, - plan_execution, - moveit_controllers, - planning_scene_monitor_parameters, - {'use_sim_time': True}, - ], + robot_moveit_common_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_robot_moveit_common.launch.py'])), + launch_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns, + # 'add_gripper': add_gripper if robot_type.perform(context) == 'xarm' else 'false', + 'no_gui_ctrl': no_gui_ctrl, + 'ros2_control_plugin': ros2_control_plugin, + 'controllers_name': controllers_name, + 'moveit_controller_manager_key': moveit_controller_manager_key, + 'moveit_controller_manager_value': moveit_controller_manager_value, + }.items(), ) nodes_to_launch.append( gazebo_spawn_entity_node, - # move_group_node, + ) + + nodes_to_launch.append( + robot_moveit_common_launch, ) # Load controllers From ffd0489d6bd0763c6132e1b8eb82182160ffef6f Mon Sep 17 00:00:00 2001 From: NikolaRaicevic Date: Thu, 26 Sep 2024 11:28:24 -0700 Subject: [PATCH 08/30] Incorporating moveit --- .../launch/_ros2_control.launch.py | 3 +- .../_robot_beside_table_gazebo.launch.py | 134 +++++++++++++++--- 2 files changed, 115 insertions(+), 22 deletions(-) diff --git a/xarm_controller/launch/_ros2_control.launch.py b/xarm_controller/launch/_ros2_control.launch.py index f03fa160..f109fbbf 100644 --- a/xarm_controller/launch/_ros2_control.launch.py +++ b/xarm_controller/launch/_ros2_control.launch.py @@ -67,8 +67,9 @@ def launch_setup(context, *args, **kwargs): ros2_control_node ] - def generate_launch_description(): return LaunchDescription([ OpaqueFunction(function=launch_setup) ]) + + diff --git a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py index a162d62d..b1fdfcbd 100644 --- a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py +++ b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py @@ -118,29 +118,118 @@ def get_per_robot_stack(robot_idx, load_controller): parameters=[{'use_sim_time': True}], ) - prefix = LaunchConfiguration('prefix', default='') - hw_ns = LaunchConfiguration('hw_ns', default='xarm') - robot_type='xarm' # TODO - fix - no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False) + moveit_config_package_name = 'xarm_moveit_config' + xarm_type='xarm6' #TODO: fix + + # robot_description_parameters + mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory(moveit_config_package_name), 'launch', 'lib', 'robot_moveit_config_lib.py')) - ros2_control_plugin = 'uf_robot_hardware/UFRobotFakeSystemHardware' - controllers_name = 'fake_controllers' - moveit_controller_manager_key = 'moveit_simple_controller_manager' - moveit_controller_manager_value = 'moveit_simple_controller_manager/MoveItSimpleControllerManager' + load_yaml = getattr(mod, 'load_yaml') + controllers_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'fake_controllers.yaml') + ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'ompl_planning.yaml') + # Planning Configuration + ompl_planning_pipeline_config = { + 'default_planning_pipeline': 'ompl', + 'planning_pipelines': ['ompl'], + } + if os.environ.get('ROS_DISTRO', '') > 'iron': + ompl_planning_pipeline_config['ompl'] = { + 'planning_plugins': ['ompl_interface/OMPLPlanner'], + 'request_adapters': [ + 'default_planning_request_adapters/ResolveConstraintFrames', + 'default_planning_request_adapters/ValidateWorkspaceBounds', + 'default_planning_request_adapters/CheckStartStateBounds', + 'default_planning_request_adapters/CheckStartStateCollision', + ], + 'response_adapters': [ + 'default_planning_response_adapters/AddTimeOptimalParameterization', + 'default_planning_response_adapters/ValidateSolution', + 'default_planning_response_adapters/DisplayMotionPath', + ], + } + else: + ompl_planning_pipeline_config['ompl'] = { + 'planning_plugin': 'ompl_interface/OMPLPlanner', + 'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", + 'start_state_max_bounds_error': 0.1, + } + ompl_planning_pipeline_config['ompl'].update(ompl_planning_yaml) - robot_moveit_common_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_robot_moveit_common.launch.py'])), - launch_arguments={ - 'prefix': prefix, - 'hw_ns': hw_ns, - # 'add_gripper': add_gripper if robot_type.perform(context) == 'xarm' else 'false', - 'no_gui_ctrl': no_gui_ctrl, - 'ros2_control_plugin': ros2_control_plugin, - 'controllers_name': controllers_name, - 'moveit_controller_manager_key': moveit_controller_manager_key, - 'moveit_controller_manager_value': moveit_controller_manager_value, - }.items(), + # Moveit controllers Configuration + moveit_controllers = { + 'moveit_fake_controller_manager': controllers_yaml, + 'moveit_controller_manager': 'moveit_fake_controller_manager/MoveItFakeControllerManager', + } + + # Trajectory Execution Configuration + trajectory_execution = { + 'moveit_manage_controllers': True, + 'trajectory_execution.allowed_execution_duration_scaling': 1.2, + 'trajectory_execution.allowed_goal_duration_margin': 0.5, + 'trajectory_execution.allowed_start_tolerance': 0.01, + 'trajectory_execution.execution_duration_monitoring': False + } + + plan_execution = { + 'plan_execution.record_trajectory_state_frequency': 10.0, + } + + planning_scene_monitor_parameters = { + 'publish_planning_scene': True, + 'publish_geometry_updates': True, + 'publish_state_updates': True, + 'publish_transforms_updates': True, + # "planning_scene_monitor_options": { + # "name": "planning_scene_monitor", + # "robot_description": "robot_description", + # "joint_state_topic": "/joint_states", + # "attached_collision_object_topic": "/move_group/planning_scene_monitor", + # "publish_planning_scene_topic": "/move_group/publish_planning_scene", + # "monitored_planning_scene_topic": "/move_group/monitored_planning_scene", + # "wait_for_initial_state_timeout": 10.0, + # }, + } + + # Start the actual move_group node/action server + move_group_node = Node( + package='moveit_ros_move_group', + executable='move_group', + namespace=this_robot_namespace, + output='screen', + parameters=[ + '-topic', f'robot_description', + ompl_planning_pipeline_config, + trajectory_execution, + plan_execution, + moveit_controllers, + planning_scene_monitor_parameters, + {'use_sim_time': True}, + ], + remappings=[ + ('/move_group/monitored_planning_scene', f'/{this_robot_namespace}/move_group/monitored_planning_scene'), + ('/move_group/display_planned_path', f'/{this_robot_namespace}/move_group/display_planned_path'), + # Add more remappings as needed + ] + ) + + # rviz with moveit configuration + rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'rviz', 'planner.rviz' if False == 'true' else 'moveit.rviz']) + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2_unique_name', + output='screen', + arguments=['-d', rviz_config_file], + parameters=[ + '-topic', f'robot_description', + ompl_planning_pipeline_config, + {'use_sim_time': True}, + ], + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ] ) nodes_to_launch.append( @@ -148,9 +237,12 @@ def get_per_robot_stack(robot_idx, load_controller): ) nodes_to_launch.append( - robot_moveit_common_launch, + rviz2_node, ) + nodes_to_launch.append( + move_group_node, + ) # Load controllers controllers = [ 'joint_state_broadcaster', From 11536080acc755e085ce688012fcb7bf7951588e Mon Sep 17 00:00:00 2001 From: NikolaRaicevic Date: Wed, 2 Oct 2024 13:43:39 -0700 Subject: [PATCH 09/30] trying to revert namespacing --- .../_robot_beside_table_gazebo.launch.py | 238 +++++++++--------- .../config/xarm6/controllers.yaml | 4 +- .../launch/_robot_moveit_common.launch.py | 11 +- .../launch/lib/robot_moveit_config_lib.py | 8 +- 4 files changed, 134 insertions(+), 127 deletions(-) diff --git a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py index b1fdfcbd..f8a0bb3f 100644 --- a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py +++ b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py @@ -71,8 +71,8 @@ def get_per_robot_stack(robot_idx, load_controller): """ The following happens for each robot """ - this_robot_namespace = f"xarm{robot_idx}" - this_robot_prefix = f"{this_robot_namespace}_" + this_robot_namespace = f"" + this_robot_prefix = f"xarm6_" nodes_to_launch = [] robot_description = build_robot_description( @@ -118,132 +118,132 @@ def get_per_robot_stack(robot_idx, load_controller): parameters=[{'use_sim_time': True}], ) - moveit_config_package_name = 'xarm_moveit_config' - xarm_type='xarm6' #TODO: fix + # moveit_config_package_name = 'xarm_moveit_config' + # xarm_type='xarm6' #TODO: fix - # robot_description_parameters - mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory(moveit_config_package_name), 'launch', 'lib', 'robot_moveit_config_lib.py')) - - load_yaml = getattr(mod, 'load_yaml') - controllers_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'fake_controllers.yaml') - ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'ompl_planning.yaml') - - # Planning Configuration - ompl_planning_pipeline_config = { - 'default_planning_pipeline': 'ompl', - 'planning_pipelines': ['ompl'], - } - if os.environ.get('ROS_DISTRO', '') > 'iron': - ompl_planning_pipeline_config['ompl'] = { - 'planning_plugins': ['ompl_interface/OMPLPlanner'], - 'request_adapters': [ - 'default_planning_request_adapters/ResolveConstraintFrames', - 'default_planning_request_adapters/ValidateWorkspaceBounds', - 'default_planning_request_adapters/CheckStartStateBounds', - 'default_planning_request_adapters/CheckStartStateCollision', - ], - 'response_adapters': [ - 'default_planning_response_adapters/AddTimeOptimalParameterization', - 'default_planning_response_adapters/ValidateSolution', - 'default_planning_response_adapters/DisplayMotionPath', - ], - } - else: - ompl_planning_pipeline_config['ompl'] = { - 'planning_plugin': 'ompl_interface/OMPLPlanner', - 'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", - 'start_state_max_bounds_error': 0.1, - } - ompl_planning_pipeline_config['ompl'].update(ompl_planning_yaml) - - # Moveit controllers Configuration - moveit_controllers = { - 'moveit_fake_controller_manager': controllers_yaml, - 'moveit_controller_manager': 'moveit_fake_controller_manager/MoveItFakeControllerManager', - } - - # Trajectory Execution Configuration - trajectory_execution = { - 'moveit_manage_controllers': True, - 'trajectory_execution.allowed_execution_duration_scaling': 1.2, - 'trajectory_execution.allowed_goal_duration_margin': 0.5, - 'trajectory_execution.allowed_start_tolerance': 0.01, - 'trajectory_execution.execution_duration_monitoring': False - } - - plan_execution = { - 'plan_execution.record_trajectory_state_frequency': 10.0, - } - - planning_scene_monitor_parameters = { - 'publish_planning_scene': True, - 'publish_geometry_updates': True, - 'publish_state_updates': True, - 'publish_transforms_updates': True, - # "planning_scene_monitor_options": { - # "name": "planning_scene_monitor", - # "robot_description": "robot_description", - # "joint_state_topic": "/joint_states", - # "attached_collision_object_topic": "/move_group/planning_scene_monitor", - # "publish_planning_scene_topic": "/move_group/publish_planning_scene", - # "monitored_planning_scene_topic": "/move_group/monitored_planning_scene", - # "wait_for_initial_state_timeout": 10.0, - # }, - } + # # robot_description_parameters + # mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory(moveit_config_package_name), 'launch', 'lib', 'robot_moveit_config_lib.py')) + + # load_yaml = getattr(mod, 'load_yaml') + # controllers_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'fake_controllers.yaml') + # ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'ompl_planning.yaml') + + # # Planning Configuration + # ompl_planning_pipeline_config = { + # 'default_planning_pipeline': 'ompl', + # 'planning_pipelines': ['ompl'], + # } + # if os.environ.get('ROS_DISTRO', '') > 'iron': + # ompl_planning_pipeline_config['ompl'] = { + # 'planning_plugins': ['ompl_interface/OMPLPlanner'], + # 'request_adapters': [ + # 'default_planning_request_adapters/ResolveConstraintFrames', + # 'default_planning_request_adapters/ValidateWorkspaceBounds', + # 'default_planning_request_adapters/CheckStartStateBounds', + # 'default_planning_request_adapters/CheckStartStateCollision', + # ], + # 'response_adapters': [ + # 'default_planning_response_adapters/AddTimeOptimalParameterization', + # 'default_planning_response_adapters/ValidateSolution', + # 'default_planning_response_adapters/DisplayMotionPath', + # ], + # } + # else: + # ompl_planning_pipeline_config['ompl'] = { + # 'planning_plugin': 'ompl_interface/OMPLPlanner', + # 'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", + # 'start_state_max_bounds_error': 0.1, + # } + # ompl_planning_pipeline_config['ompl'].update(ompl_planning_yaml) + + # # Moveit controllers Configuration + # moveit_controllers = { + # 'moveit_fake_controller_manager': controllers_yaml, + # 'moveit_controller_manager': 'moveit_fake_controller_manager/MoveItFakeControllerManager', + # } + + # # Trajectory Execution Configuration + # trajectory_execution = { + # 'moveit_manage_controllers': True, + # 'trajectory_execution.allowed_execution_duration_scaling': 1.2, + # 'trajectory_execution.allowed_goal_duration_margin': 0.5, + # 'trajectory_execution.allowed_start_tolerance': 0.01, + # 'trajectory_execution.execution_duration_monitoring': False + # } + + # plan_execution = { + # 'plan_execution.record_trajectory_state_frequency': 10.0, + # } + + # planning_scene_monitor_parameters = { + # 'publish_planning_scene': True, + # 'publish_geometry_updates': True, + # 'publish_state_updates': True, + # 'publish_transforms_updates': True, + # # "planning_scene_monitor_options": { + # # "name": "planning_scene_monitor", + # # "robot_description": "robot_description", + # # "joint_state_topic": "/joint_states", + # # "attached_collision_object_topic": "/move_group/planning_scene_monitor", + # # "publish_planning_scene_topic": "/move_group/publish_planning_scene", + # # "monitored_planning_scene_topic": "/move_group/monitored_planning_scene", + # # "wait_for_initial_state_timeout": 10.0, + # # }, + # } # Start the actual move_group node/action server - move_group_node = Node( - package='moveit_ros_move_group', - executable='move_group', - namespace=this_robot_namespace, - output='screen', - parameters=[ - '-topic', f'robot_description', - ompl_planning_pipeline_config, - trajectory_execution, - plan_execution, - moveit_controllers, - planning_scene_monitor_parameters, - {'use_sim_time': True}, - ], - remappings=[ - ('/move_group/monitored_planning_scene', f'/{this_robot_namespace}/move_group/monitored_planning_scene'), - ('/move_group/display_planned_path', f'/{this_robot_namespace}/move_group/display_planned_path'), - # Add more remappings as needed - ] - ) - - # rviz with moveit configuration - rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'rviz', 'planner.rviz' if False == 'true' else 'moveit.rviz']) - rviz2_node = Node( - package='rviz2', - executable='rviz2', - name='rviz2_unique_name', - output='screen', - arguments=['-d', rviz_config_file], - parameters=[ - '-topic', f'robot_description', - ompl_planning_pipeline_config, - {'use_sim_time': True}, - ], - remappings=[ - ('/tf', 'tf'), - ('/tf_static', 'tf_static'), - ] - ) + # move_group_node = Node( + # package='moveit_ros_move_group', + # executable='move_group', + # namespace=this_robot_namespace, + # output='screen', + # parameters=[ + # '-topic', f'robot_description', + # ompl_planning_pipeline_config, + # trajectory_execution, + # plan_execution, + # moveit_controllers, + # planning_scene_monitor_parameters, + # {'use_sim_time': True}, + # ], + # remappings=[ + # ('/move_group/monitored_planning_scene', f'/{this_robot_namespace}/move_group/monitored_planning_scene'), + # ('/move_group/display_planned_path', f'/{this_robot_namespace}/move_group/display_planned_path'), + # # Add more remappings as needed + # ] + # ) + + # # rviz with moveit configuration + # rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'rviz', 'planner.rviz' if False == 'true' else 'moveit.rviz']) + # rviz2_node = Node( + # package='rviz2', + # executable='rviz2', + # name='rviz2_unique_name', + # output='screen', + # arguments=['-d', rviz_config_file], + # parameters=[ + # '-topic', f'robot_description', + # ompl_planning_pipeline_config, + # {'use_sim_time': True}, + # ], + # remappings=[ + # ('/tf', 'tf'), + # ('/tf_static', 'tf_static'), + # ] + # ) nodes_to_launch.append( gazebo_spawn_entity_node, ) - nodes_to_launch.append( - rviz2_node, - ) + # nodes_to_launch.append( + # rviz2_node, + # ) - nodes_to_launch.append( - move_group_node, - ) - # Load controllers + # nodes_to_launch.append( + # move_group_node, + # ) + # # Load controllers controllers = [ 'joint_state_broadcaster', # the below becomes something like xarm0_xarm6_traj_controller. diff --git a/xarm_moveit_config/config/xarm6/controllers.yaml b/xarm_moveit_config/config/xarm6/controllers.yaml index 6dbb9241..4c5dcbb1 100644 --- a/xarm_moveit_config/config/xarm6/controllers.yaml +++ b/xarm_moveit_config/config/xarm6/controllers.yaml @@ -1,7 +1,7 @@ controller_names: - - xarm6_traj_controller + - traj_controller -xarm6_traj_controller: +traj_controller: action_ns: follow_joint_trajectory type: FollowJointTrajectory default: true diff --git a/xarm_moveit_config/launch/_robot_moveit_common.launch.py b/xarm_moveit_config/launch/_robot_moveit_common.launch.py index 5efc6850..0e6eb975 100644 --- a/xarm_moveit_config/launch/_robot_moveit_common.launch.py +++ b/xarm_moveit_config/launch/_robot_moveit_common.launch.py @@ -48,7 +48,7 @@ def launch_setup(context, *args, **kwargs): 'ros2_control_plugin': ros2_control_plugin, }, srdf_arguments={ - 'prefix': prefix, + 'prefix': '', # 'dof': dof, # 'robot_type': robot_type, # 'add_gripper': add_gripper, @@ -194,7 +194,9 @@ def launch_setup(context, *args, **kwargs): planning_scene_monitor_parameters, # sensor_manager_parameters, {'use_sim_time': use_sim_time}, + {'publish_robot_description_semantic': True}, ], + namespace="xarm0" ) # rviz with moveit configuration @@ -214,7 +216,12 @@ def launch_setup(context, *args, **kwargs): remappings=[ ('/tf', 'tf'), ('/tf_static', 'tf_static'), - ] + ('/robot_description', '/xarm0/robot_description'), + ('/robot_description_semantic', '/xarm0/robot_description_semantic'), + ('/monitored_planning_scene', '/xarm0/monitored_planning_scene'), + ('/get_planning_scene', '/xarm0/get_planning_scene'), + + ], ) # xyz = attach_xyz.perform(context)[1:-1].split(' ') diff --git a/xarm_moveit_config/launch/lib/robot_moveit_config_lib.py b/xarm_moveit_config/launch/lib/robot_moveit_config_lib.py index 5c1b795a..c2e571a8 100644 --- a/xarm_moveit_config/launch/lib/robot_moveit_config_lib.py +++ b/xarm_moveit_config/launch/lib/robot_moveit_config_lib.py @@ -76,10 +76,10 @@ def get_xarm_robot_description_parameters( # xarm_description/launch/lib/robot_description_lib.py return { - 'robot_description': get_xacro_command( - xacro_file=xacro_urdf_file, - mappings=urdf_arguments - ), + # 'robot_description': get_xacro_command( + # xacro_file=xacro_urdf_file, + # mappings=urdf_arguments + # ), 'robot_description_semantic': get_xacro_command( xacro_file=xacro_srdf_file, mappings=srdf_arguments From 95d612b99deb6ebef288bc9df521fcc4d42daa54 Mon Sep 17 00:00:00 2001 From: brian Date: Wed, 2 Oct 2024 14:29:16 -0700 Subject: [PATCH 10/30] works without namespace --- .../launch/lib/robot_controller_lib.py | 2 +- xarm_description/urdf/xarm_device.urdf.xacro | 2 +- xarm_description/urdf/xarm_device_macro.xacro | 2 +- .../_robot_beside_table_gazebo.launch.py | 78 +++++++++---------- 4 files changed, 42 insertions(+), 42 deletions(-) diff --git a/xarm_controller/launch/lib/robot_controller_lib.py b/xarm_controller/launch/lib/robot_controller_lib.py index b5a5bb75..826a47c3 100644 --- a/xarm_controller/launch/lib/robot_controller_lib.py +++ b/xarm_controller/launch/lib/robot_controller_lib.py @@ -57,7 +57,7 @@ def generate_ros2_control_params_temp_file(ros2_control_params_path, prefix='', add_prefix_to_ros2_control_params(prefix, ros2_control_params_yaml) - if not ros_namespace.startswith('/'): + if ros_namespace and not ros_namespace.startswith('/'): ros_namespace = f'/{ros_namespace}' # add namespace, possibly just slash to all nodes diff --git a/xarm_description/urdf/xarm_device.urdf.xacro b/xarm_description/urdf/xarm_device.urdf.xacro index 918d8b6e..3b573346 100755 --- a/xarm_description/urdf/xarm_device.urdf.xacro +++ b/xarm_description/urdf/xarm_device.urdf.xacro @@ -8,7 +8,7 @@ - + diff --git a/xarm_description/urdf/xarm_device_macro.xacro b/xarm_description/urdf/xarm_device_macro.xacro index c74658c2..266aaefc 100644 --- a/xarm_description/urdf/xarm_device_macro.xacro +++ b/xarm_description/urdf/xarm_device_macro.xacro @@ -2,7 +2,7 @@ Date: Wed, 2 Oct 2024 14:57:27 -0700 Subject: [PATCH 11/30] make single robot planning work --- xarm_moveit_config/config/xarm6/controllers.yaml | 16 ++++++++-------- .../config/xarm6/joint_limits.yaml | 12 ++++++------ xarm_moveit_config/config/xarm6/kinematics.yaml | 2 +- .../config/xarm6/ompl_planning.yaml | 2 +- .../launch/_robot_moveit_common.launch.py | 8 +------- .../launch/_robot_moveit_gazebo.launch.py | 4 ++-- 6 files changed, 19 insertions(+), 25 deletions(-) diff --git a/xarm_moveit_config/config/xarm6/controllers.yaml b/xarm_moveit_config/config/xarm6/controllers.yaml index 4c5dcbb1..05fd5726 100644 --- a/xarm_moveit_config/config/xarm6/controllers.yaml +++ b/xarm_moveit_config/config/xarm6/controllers.yaml @@ -1,14 +1,14 @@ controller_names: - - traj_controller + - xarm6_traj_controller -traj_controller: +xarm6_traj_controller: action_ns: follow_joint_trajectory type: FollowJointTrajectory default: true joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 + - xarm6_joint1 + - xarm6_joint2 + - xarm6_joint3 + - xarm6_joint4 + - xarm6_joint5 + - xarm6_joint6 diff --git a/xarm_moveit_config/config/xarm6/joint_limits.yaml b/xarm_moveit_config/config/xarm6/joint_limits.yaml index e029e822..3b66c454 100755 --- a/xarm_moveit_config/config/xarm6/joint_limits.yaml +++ b/xarm_moveit_config/config/xarm6/joint_limits.yaml @@ -2,32 +2,32 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint1: + xarm6_joint1: has_velocity_limits: true max_velocity: 2.14 has_acceleration_limits: true max_acceleration: 10.0 - joint2: + xarm6_joint2: has_velocity_limits: true max_velocity: 2.14 has_acceleration_limits: true max_acceleration: 10.0 - joint3: + xarm6_joint3: has_velocity_limits: true max_velocity: 2.14 has_acceleration_limits: true max_acceleration: 10.0 - joint4: + xarm6_joint4: has_velocity_limits: true max_velocity: 2.14 has_acceleration_limits: true max_acceleration: 10.0 - joint5: + xarm6_joint5: has_velocity_limits: true max_velocity: 2.14 has_acceleration_limits: true max_acceleration: 10.0 - joint6: + xarm6_joint6: has_velocity_limits: true max_velocity: 2.14 has_acceleration_limits: true diff --git a/xarm_moveit_config/config/xarm6/kinematics.yaml b/xarm_moveit_config/config/xarm6/kinematics.yaml index 01206c4c..1babc566 100755 --- a/xarm_moveit_config/config/xarm6/kinematics.yaml +++ b/xarm_moveit_config/config/xarm6/kinematics.yaml @@ -1,4 +1,4 @@ -xarm6: +xarm6_xarm6: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin # kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin kinematics_solver_search_resolution: 0.005 diff --git a/xarm_moveit_config/config/xarm6/ompl_planning.yaml b/xarm_moveit_config/config/xarm6/ompl_planning.yaml index d859b572..b77a64d0 100755 --- a/xarm_moveit_config/config/xarm6/ompl_planning.yaml +++ b/xarm_moveit_config/config/xarm6/ompl_planning.yaml @@ -121,7 +121,7 @@ planner_configs: dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 max_failures: 5000 # maximum consecutive failure limit. default: 5000 -xarm6: +xarm6_xarm6: default_planner_config: RRTConnect planner_configs: - SBL diff --git a/xarm_moveit_config/launch/_robot_moveit_common.launch.py b/xarm_moveit_config/launch/_robot_moveit_common.launch.py index 0e6eb975..e20a7ba6 100644 --- a/xarm_moveit_config/launch/_robot_moveit_common.launch.py +++ b/xarm_moveit_config/launch/_robot_moveit_common.launch.py @@ -48,7 +48,7 @@ def launch_setup(context, *args, **kwargs): 'ros2_control_plugin': ros2_control_plugin, }, srdf_arguments={ - 'prefix': '', + 'prefix': 'xarm6_', # 'dof': dof, # 'robot_type': robot_type, # 'add_gripper': add_gripper, @@ -196,7 +196,6 @@ def launch_setup(context, *args, **kwargs): {'use_sim_time': use_sim_time}, {'publish_robot_description_semantic': True}, ], - namespace="xarm0" ) # rviz with moveit configuration @@ -216,11 +215,6 @@ def launch_setup(context, *args, **kwargs): remappings=[ ('/tf', 'tf'), ('/tf_static', 'tf_static'), - ('/robot_description', '/xarm0/robot_description'), - ('/robot_description_semantic', '/xarm0/robot_description_semantic'), - ('/monitored_planning_scene', '/xarm0/monitored_planning_scene'), - ('/get_planning_scene', '/xarm0/get_planning_scene'), - ], ) diff --git a/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py b/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py index a8bf235b..b89bce66 100644 --- a/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py +++ b/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py @@ -15,11 +15,11 @@ def generate_launch_description(): prefix = LaunchConfiguration('prefix', default='') - hw_ns = LaunchConfiguration('hw_ns', default='xarm') + hw_ns = LaunchConfiguration('hw_ns', default='') no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False) ros2_control_plugin = 'ign_ros2_control/IgnitionSystem' - controllers_name = 'fake_controllers' + controllers_name = 'controllers' moveit_controller_manager_key = 'moveit_simple_controller_manager' moveit_controller_manager_value = 'moveit_simple_controller_manager/MoveItSimpleControllerManager' From b66c85e1b83b1c798962cdc42915f546e2fc42e1 Mon Sep 17 00:00:00 2001 From: NikolaRaicevic Date: Thu, 3 Oct 2024 12:01:23 -0700 Subject: [PATCH 12/30] Added camera and ros_gz_bridge node --- .../launch/lib/robot_controller_lib.py | 2 - .../_robot_beside_table_gazebo.launch.py | 276 +++++------------- .../launch/_robot_moveit_common.launch.py | 6 - .../launch/_robot_moveit_gazebo.launch.py | 8 - 4 files changed, 69 insertions(+), 223 deletions(-) diff --git a/xarm_controller/launch/lib/robot_controller_lib.py b/xarm_controller/launch/lib/robot_controller_lib.py index 826a47c3..a744a557 100644 --- a/xarm_controller/launch/lib/robot_controller_lib.py +++ b/xarm_controller/launch/lib/robot_controller_lib.py @@ -5,7 +5,6 @@ from tempfile import NamedTemporaryFile from ament_index_python import get_package_share_directory - def add_prefix_to_ros2_control_params(prefix, ros2_control_params): if not prefix: return @@ -27,7 +26,6 @@ def add_prefix_to_ros2_control_params(prefix, ros2_control_params): if name in controller_manager_ros__parameters: controller_manager_ros__parameters[new_name] = controller_manager_ros__parameters.pop(name) - def generate_ros2_control_params_temp_file(ros2_control_params_path, prefix='', add_gripper=False, add_bio_gripper=False, ros_namespace='', update_rate=None, robot_type='xarm'): if ros_namespace or prefix or update_rate: with open(ros2_control_params_path, 'r') as f: diff --git a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py index 53599c95..c21672e9 100644 --- a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py +++ b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py @@ -14,34 +14,23 @@ from launch.event_handlers import OnProcessExit from uf_ros_lib.uf_robot_utils import get_xacro_command -def build_robot_description( - this_robot_prefix="", - this_robot_namespace="" -): - # which ros2 control plugin to use. could be ign control in the future if using ign gazebo - ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='ign_ros2_control/IgnitionSystem') - +def build_robot_description(this_robot_prefix="", this_robot_namespace="", add_gripper = False): # ros2 control params - # xarm_controller/launch/lib/robot_controller_lib.py + ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='ign_ros2_control/IgnitionSystem') mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('xarm_controller'), 'launch', 'lib', 'robot_controller_lib.py')) generate_ros2_control_params_temp_file = getattr(mod, 'generate_ros2_control_params_temp_file') ros2_control_params = generate_ros2_control_params_temp_file( - os.path.join( - get_package_share_directory('xarm_controller'), - 'config', 'xarm6_controllers.yaml' - ), + os.path.join(get_package_share_directory('xarm_controller'),'config', 'xarm6_controllers.yaml'), prefix=this_robot_prefix, - add_gripper=False, #TODO: fix this later + add_gripper= add_gripper, #TODO: fix this later add_bio_gripper=False, ros_namespace=this_robot_namespace, update_rate=1000, robot_type='xarm' ) - print(f"Generated temporary control params at {ros2_control_params}") # robot_description - # xarm_description/launch/lib/robot_description_lib.py robot_description = { 'robot_description': get_xacro_command( xacro_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']), @@ -53,7 +42,6 @@ def build_robot_description( } ), } - return robot_description def build_camera_description(this_robot_namespace=""): @@ -73,12 +61,10 @@ def get_per_robot_stack(robot_idx, load_controller): """ this_robot_namespace = f"" this_robot_prefix = f"xarm6_" + add_gripper = False nodes_to_launch = [] - robot_description = build_robot_description( - this_robot_prefix=this_robot_prefix, - this_robot_namespace=this_robot_namespace - ) + robot_description = build_robot_description(this_robot_prefix=this_robot_prefix, this_robot_namespace=this_robot_namespace,add_gripper = add_gripper) # robot state publisher node robot_state_publisher_node = Node( @@ -118,131 +104,10 @@ def get_per_robot_stack(robot_idx, load_controller): parameters=[{'use_sim_time': True}], ) - # moveit_config_package_name = 'xarm_moveit_config' - # xarm_type='xarm6' #TODO: fix - - # # robot_description_parameters - # mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory(moveit_config_package_name), 'launch', 'lib', 'robot_moveit_config_lib.py')) - - # load_yaml = getattr(mod, 'load_yaml') - # controllers_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'fake_controllers.yaml') - # ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'ompl_planning.yaml') - - # # Planning Configuration - # ompl_planning_pipeline_config = { - # 'default_planning_pipeline': 'ompl', - # 'planning_pipelines': ['ompl'], - # } - # if os.environ.get('ROS_DISTRO', '') > 'iron': - # ompl_planning_pipeline_config['ompl'] = { - # 'planning_plugins': ['ompl_interface/OMPLPlanner'], - # 'request_adapters': [ - # 'default_planning_request_adapters/ResolveConstraintFrames', - # 'default_planning_request_adapters/ValidateWorkspaceBounds', - # 'default_planning_request_adapters/CheckStartStateBounds', - # 'default_planning_request_adapters/CheckStartStateCollision', - # ], - # 'response_adapters': [ - # 'default_planning_response_adapters/AddTimeOptimalParameterization', - # 'default_planning_response_adapters/ValidateSolution', - # 'default_planning_response_adapters/DisplayMotionPath', - # ], - # } - # else: - # ompl_planning_pipeline_config['ompl'] = { - # 'planning_plugin': 'ompl_interface/OMPLPlanner', - # 'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", - # 'start_state_max_bounds_error': 0.1, - # } - # ompl_planning_pipeline_config['ompl'].update(ompl_planning_yaml) - - # # Moveit controllers Configuration - # moveit_controllers = { - # 'moveit_fake_controller_manager': controllers_yaml, - # 'moveit_controller_manager': 'moveit_fake_controller_manager/MoveItFakeControllerManager', - # } - - # # Trajectory Execution Configuration - # trajectory_execution = { - # 'moveit_manage_controllers': True, - # 'trajectory_execution.allowed_execution_duration_scaling': 1.2, - # 'trajectory_execution.allowed_goal_duration_margin': 0.5, - # 'trajectory_execution.allowed_start_tolerance': 0.01, - # 'trajectory_execution.execution_duration_monitoring': False - # } - - # plan_execution = { - # 'plan_execution.record_trajectory_state_frequency': 10.0, - # } - - # planning_scene_monitor_parameters = { - # 'publish_planning_scene': True, - # 'publish_geometry_updates': True, - # 'publish_state_updates': True, - # 'publish_transforms_updates': True, - # # "planning_scene_monitor_options": { - # # "name": "planning_scene_monitor", - # # "robot_description": "robot_description", - # # "joint_state_topic": "/joint_states", - # # "attached_collision_object_topic": "/move_group/planning_scene_monitor", - # # "publish_planning_scene_topic": "/move_group/publish_planning_scene", - # # "monitored_planning_scene_topic": "/move_group/monitored_planning_scene", - # # "wait_for_initial_state_timeout": 10.0, - # # }, - # } - - # Start the actual move_group node/action server - # move_group_node = Node( - # package='moveit_ros_move_group', - # executable='move_group', - # namespace=this_robot_namespace, - # output='screen', - # parameters=[ - # '-topic', f'robot_description', - # ompl_planning_pipeline_config, - # trajectory_execution, - # plan_execution, - # moveit_controllers, - # planning_scene_monitor_parameters, - # {'use_sim_time': True}, - # ], - # remappings=[ - # ('/move_group/monitored_planning_scene', f'/{this_robot_namespace}/move_group/monitored_planning_scene'), - # ('/move_group/display_planned_path', f'/{this_robot_namespace}/move_group/display_planned_path'), - # # Add more remappings as needed - # ] - # ) - - # # rviz with moveit configuration - # rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'rviz', 'planner.rviz' if False == 'true' else 'moveit.rviz']) - # rviz2_node = Node( - # package='rviz2', - # executable='rviz2', - # name='rviz2_unique_name', - # output='screen', - # arguments=['-d', rviz_config_file], - # parameters=[ - # '-topic', f'robot_description', - # ompl_planning_pipeline_config, - # {'use_sim_time': True}, - # ], - # remappings=[ - # ('/tf', 'tf'), - # ('/tf_static', 'tf_static'), - # ] - # ) - nodes_to_launch.append( gazebo_spawn_entity_node, ) - # nodes_to_launch.append( - # rviz2_node, - # ) - - # nodes_to_launch.append( - # move_group_node, - # ) # # Load controllers controllers = [ 'joint_state_broadcaster', @@ -293,90 +158,87 @@ def get_per_robot_stack(robot_idx, load_controller): return nodes_to_launch def generate_launch_description(): - - num_robots_config = LaunchConfiguration("num_robots", default=1) - num_robots_arg = DeclareLaunchArgument("num_robots", default_value="1", description="number of robots to launch") - - load_controller_config = LaunchConfiguration("load_controller", default=True) - load_controller_arg = DeclareLaunchArgument("load_controller", default_value="True", description="whether to load joint controllers") - - # gazebo launch - # gazebo_ros/launch/gazebo.launch.py - # xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('xarm_gazebo'), 'worlds', 'table.world']) - # gazebo_launch = IncludeLaunchDescription( - # PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 'launch', 'gz_sim.launch.py'])), - # launch_arguments={ - # 'gz_args': '-r' - # } - # ) - - world_sdf_path = os.path.join(get_package_share_directory('main'),'world','world.sdf') # Path to your world file - camera_namespace = LaunchConfiguration('camera_namespace', default="camera_01") + camera_namespace = LaunchConfiguration('camera_namespace', default='camera_01') + camera_robot_description = build_camera_description(this_robot_namespace=camera_namespace) + load_controller_config = LaunchConfiguration('load_controller', default=True) + num_robots_config = LaunchConfiguration('num_robots', default=1) + world_sdf_path = os.path.join(get_package_share_directory('main'), 'world', 'world.sdf') # Gazebo launch gazebo_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 'launch', 'gz_sim.launch.py'])), launch_arguments={ - 'gz_args': f'-v4 -r {world_sdf_path}', # Load the world file with Gazebo directly + 'gz_args': f'-v4 -r {world_sdf_path}', }.items(), ) - # ros2 run ros_gz_bridge parameter_bridge /clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock - clock_parameter_bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - output="screen", + # Node for creating bridge between gazebo and ros2 (ros2 run ros_gz_bridge parameter_bridge) + parameters_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + output='screen', arguments=[ - "/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock" + [camera_namespace, '/camera_color@sensor_msgs/msg/Image@ignition.msgs.Image'], + [camera_namespace, '/camera_depth@sensor_msgs/msg/Image@ignition.msgs.Image'], + [camera_namespace, '/camera_depth/points@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked'], + [camera_namespace, '/camera_info@sensor_msgs/msg/CameraInfo@ignition.msgs.CameraInfo'], + [camera_namespace, '/camera_ired1@sensor_msgs/msg/Image@ignition.msgs.Image'], + [camera_namespace, '/camera_ired2@sensor_msgs/msg/Image@ignition.msgs.Image'], + '/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock', + '/model/sensor_d455/pose@tf2_msgs/msg/TFMessage[ignition.msgs.Pose_V', ] ) - nodes_to_launch = [gazebo_launch, clock_parameter_bridge] + nodes_to_launch = [ + gazebo_launch, + parameters_bridge, + ] - # camera_robot_description = build_camera_description(this_robot_namespace = camera_namespace) + # Node for launching camera robot state publisher + robot_state_publisher_node_camera = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + namespace=camera_namespace, + parameters=[camera_robot_description] + ) - # robot_state_publisher_node_camera = Node( - # package='robot_state_publisher', - # executable='robot_state_publisher', - # output='screen', - # namespace=camera_namespace, - # parameters=[camera_robot_description] - # ) - # nodes_to_launch.append(robot_state_publisher_node_camera) - - # gazebo_spawn_camera_node = Node( - # package="ros_gz_sim", - # executable="create", - # namespace=camera_namespace, - # output='screen', - # arguments=[ - # '-topic', 'robot_description', - # '-allow_renaming', 'false', - # '-x', '0.2', # Corrected: Added missing commas - # '-y', '0.7', - # '-z', '1.2', - # '-P', '0', - # '-Y', '-1.8', - # '-timeout', '10000' - # ], - # parameters=[{'use_sim_time': True}], - # ) + nodes_to_launch.append(robot_state_publisher_node_camera) + + # Node for spawning the camera in gazebo environment + gazebo_spawn_camera_node = Node( + package='ros_gz_sim', + executable='create', + namespace=camera_namespace, + output='screen', + arguments=[ + '-topic', 'robot_description', + '-allow_renaming', 'false', + '-x', '0.2', + '-y', '0.7', + '-z', '1.2', + '-P', '0', + '-Y', '-1.8', + '-timeout', '10000', + ], + parameters=[{'use_sim_time': True}], + ) - # delayed_spawn = TimerAction( - # period=2.0, - # actions=[gazebo_spawn_camera_node] - # ) + delayed_spawn = TimerAction( + period=2.0, + actions=[gazebo_spawn_camera_node] + ) - # nodes_to_launch.append(delayed_spawn) + nodes_to_launch.append(delayed_spawn) def _launch_all_robots(context): return sum( [ - get_per_robot_stack( - robot_idx, - bool(load_controller_config.perform(context)) - ) for robot_idx in range(int(num_robots_config.perform(context))) - ], []) - + get_per_robot_stack(robot_idx, bool(load_controller_config.perform(context))) + for robot_idx in range(int(num_robots_config.perform(context))) + ], + [] + ) + launch_all_robots = OpaqueFunction(function=_launch_all_robots) - return LaunchDescription(nodes_to_launch + [launch_all_robots] + [load_controller_arg, num_robots_arg]) + return LaunchDescription(nodes_to_launch + [launch_all_robots]) diff --git a/xarm_moveit_config/launch/_robot_moveit_common.launch.py b/xarm_moveit_config/launch/_robot_moveit_common.launch.py index e20a7ba6..c33bb20a 100644 --- a/xarm_moveit_config/launch/_robot_moveit_common.launch.py +++ b/xarm_moveit_config/launch/_robot_moveit_common.launch.py @@ -1,10 +1,4 @@ #!/usr/bin/env python3 -# Software License Agreement (BSD License) -# -# Copyright (c) 2021, UFACTORY, Inc. -# All rights reserved. -# -# Author: Vinman import os from ament_index_python import get_package_share_directory diff --git a/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py b/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py index b89bce66..f68cd542 100644 --- a/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py +++ b/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py @@ -1,10 +1,4 @@ #!/usr/bin/env python3 -# Software License Agreement (BSD License) -# -# Copyright (c) 2021, UFACTORY, Inc. -# All rights reserved. -# -# Author: Vinman from launch import LaunchDescription from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument @@ -24,7 +18,6 @@ def generate_launch_description(): moveit_controller_manager_value = 'moveit_simple_controller_manager/MoveItSimpleControllerManager' # robot moveit common launch - # xarm_moveit_config/launch/_robot_moveit_common.launch.py robot_moveit_common_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_robot_moveit_common.launch.py'])), launch_arguments={ @@ -40,7 +33,6 @@ def generate_launch_description(): ) # robot gazebo launch - # xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py robot_gazebo_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_gazebo'), 'launch', '_robot_beside_table_gazebo.launch.py'])), launch_arguments={ From 80e3660d7e4693acdaaa2c9be3d832bc73be9b69 Mon Sep 17 00:00:00 2001 From: NikolaRaicevic Date: Thu, 3 Oct 2024 15:22:53 -0700 Subject: [PATCH 13/30] minor --- .../launch/_robot_moveit_common.launch.py | 22 +++++------- .../launch/_robot_moveit_gazebo.launch.py | 35 +++++++++++-------- 2 files changed, 30 insertions(+), 27 deletions(-) diff --git a/xarm_moveit_config/launch/_robot_moveit_common.launch.py b/xarm_moveit_config/launch/_robot_moveit_common.launch.py index c33bb20a..b8ec79b9 100644 --- a/xarm_moveit_config/launch/_robot_moveit_common.launch.py +++ b/xarm_moveit_config/launch/_robot_moveit_common.launch.py @@ -14,20 +14,16 @@ def launch_setup(context, *args, **kwargs): - prefix = LaunchConfiguration('prefix', default='') - hw_ns = LaunchConfiguration('hw_ns', default='xarm') - ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='uf_robot_hardware/UFRobotFakeSystemHardware') - no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False) - controllers_name = LaunchConfiguration('controllers_name', default='fake_controllers') + hw_ns = LaunchConfiguration('hw_ns', default='xarm') moveit_controller_manager_key = LaunchConfiguration('moveit_controller_manager_key', default='moveit_fake_controller_manager') moveit_controller_manager_value = LaunchConfiguration('moveit_controller_manager_value', default='moveit_fake_controller_manager/MoveItFakeControllerManager') - - use_sim_time = LaunchConfiguration('use_sim_time', 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 '') - xarm_type='xarm6' #TODO: fix + no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False) + prefix = LaunchConfiguration('prefix', default='') + robot_type = LaunchConfiguration('robot_type', default='') + ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='uf_robot_hardware/UFRobotFakeSystemHardware') + use_sim_time = LaunchConfiguration('use_sim_time', default=False) # robot_description_parameters # xarm_moveit_config/launch/lib/robot_moveit_config_lib.py @@ -51,13 +47,13 @@ def launch_setup(context, *args, **kwargs): }, arguments={ 'context': context, - 'xarm_type': xarm_type, + 'xarm_type': robot_type.perform(context), } ) load_yaml = getattr(mod, 'load_yaml') - controllers_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, '{}.yaml'.format(controllers_name.perform(context))) - ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'ompl_planning.yaml') + controllers_yaml = load_yaml(moveit_config_package_name, 'config', robot_type.perform(context), '{}.yaml'.format(controllers_name.perform(context))) + ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', robot_type.perform(context), 'ompl_planning.yaml') kinematics_yaml = robot_description_parameters['robot_description_kinematics'] joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None) diff --git a/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py b/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py index f68cd542..4af4b427 100644 --- a/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py +++ b/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py @@ -8,27 +8,31 @@ def generate_launch_description(): - prefix = LaunchConfiguration('prefix', default='') + camera_namespace = LaunchConfiguration('camera_namespace', default='camera_01') + controllers_name = LaunchConfiguration('controllers_name', default='controllers') hw_ns = LaunchConfiguration('hw_ns', default='') + load_controller = LaunchConfiguration('load_controller', default='true') + moveit_controller_manager_key = LaunchConfiguration('moveit_controller_manager_key', default='moveit_simple_controller_manager') + moveit_controller_manager_value = LaunchConfiguration('moveit_controller_manager_value', default='moveit_simple_controller_manager/MoveItSimpleControllerManager') no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False) - - ros2_control_plugin = 'ign_ros2_control/IgnitionSystem' - controllers_name = 'controllers' - moveit_controller_manager_key = 'moveit_simple_controller_manager' - moveit_controller_manager_value = 'moveit_simple_controller_manager/MoveItSimpleControllerManager' + prefix = LaunchConfiguration('prefix', default='') + robot_type= LaunchConfiguration('robot_type', default='xarm6') + ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='ign_ros2_control/IgnitionSystem') + use_sim_time = LaunchConfiguration('use_sim_time', default=True) # robot moveit common launch robot_moveit_common_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_robot_moveit_common.launch.py'])), launch_arguments={ - 'prefix': prefix, - 'hw_ns': hw_ns, - 'no_gui_ctrl': no_gui_ctrl, - 'ros2_control_plugin': ros2_control_plugin, 'controllers_name': controllers_name, + 'hw_ns': hw_ns, 'moveit_controller_manager_key': moveit_controller_manager_key, 'moveit_controller_manager_value': moveit_controller_manager_value, - 'use_sim_time': 'true' + 'no_gui_ctrl': no_gui_ctrl, + 'prefix': prefix, + 'robot_type': robot_type, + 'ros2_control_plugin': ros2_control_plugin, + 'use_sim_time': use_sim_time, }.items(), ) @@ -36,14 +40,17 @@ def generate_launch_description(): robot_gazebo_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_gazebo'), 'launch', '_robot_beside_table_gazebo.launch.py'])), launch_arguments={ + 'camera_namespace': camera_namespace, + 'load_controller': load_controller, + 'no_gui_ctrl': no_gui_ctrl, 'prefix': prefix, - 'hw_ns': hw_ns, 'ros2_control_plugin': ros2_control_plugin, - 'load_controller': 'true', }.items(), ) return LaunchDescription([ robot_gazebo_launch, robot_moveit_common_launch, - ]) \ No newline at end of file + ]) + + From 1be6be99e6c5d22d80680557e4747b6c3ecdeab5 Mon Sep 17 00:00:00 2001 From: NikolaRaicevic Date: Thu, 3 Oct 2024 17:27:37 -0700 Subject: [PATCH 14/30] minor --- xarm_moveit_config/launch/_robot_moveit_common.launch.py | 1 - xarm_moveit_config/launch/_robot_moveit_fake.launch.py | 6 ------ xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py | 1 - xarm_moveit_config/launch/lib/robot_moveit_config_lib.py | 6 ------ 4 files changed, 14 deletions(-) diff --git a/xarm_moveit_config/launch/_robot_moveit_common.launch.py b/xarm_moveit_config/launch/_robot_moveit_common.launch.py index b8ec79b9..9745a8cb 100644 --- a/xarm_moveit_config/launch/_robot_moveit_common.launch.py +++ b/xarm_moveit_config/launch/_robot_moveit_common.launch.py @@ -12,7 +12,6 @@ from launch.event_handlers import OnProcessExit from launch.events import Shutdown - def launch_setup(context, *args, **kwargs): controllers_name = LaunchConfiguration('controllers_name', default='fake_controllers') hw_ns = LaunchConfiguration('hw_ns', default='xarm') diff --git a/xarm_moveit_config/launch/_robot_moveit_fake.launch.py b/xarm_moveit_config/launch/_robot_moveit_fake.launch.py index ba6de25a..4d766d60 100644 --- a/xarm_moveit_config/launch/_robot_moveit_fake.launch.py +++ b/xarm_moveit_config/launch/_robot_moveit_fake.launch.py @@ -1,10 +1,4 @@ #!/usr/bin/env python3 -# Software License Agreement (BSD License) -# -# Copyright (c) 2021, UFACTORY, Inc. -# All rights reserved. -# -# Author: Vinman from launch import LaunchDescription from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument diff --git a/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py b/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py index 4af4b427..a3c0a8ff 100644 --- a/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py +++ b/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py @@ -6,7 +6,6 @@ from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.substitutions import FindPackageShare - def generate_launch_description(): camera_namespace = LaunchConfiguration('camera_namespace', default='camera_01') controllers_name = LaunchConfiguration('controllers_name', default='controllers') diff --git a/xarm_moveit_config/launch/lib/robot_moveit_config_lib.py b/xarm_moveit_config/launch/lib/robot_moveit_config_lib.py index c2e571a8..548a7f67 100644 --- a/xarm_moveit_config/launch/lib/robot_moveit_config_lib.py +++ b/xarm_moveit_config/launch/lib/robot_moveit_config_lib.py @@ -1,10 +1,4 @@ #!/usr/bin/env python3 -# Software License Agreement (BSD License) -# -# Copyright (c) 2021, UFACTORY, Inc. -# All rights reserved. -# -# Author: Vinman import os import yaml From bb109f25d9725f4b7dc28a814780f6a864a96f40 Mon Sep 17 00:00:00 2001 From: NikolaRaicevic Date: Thu, 3 Oct 2024 18:54:16 -0700 Subject: [PATCH 15/30] correct camera orientation --- xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py index c21672e9..19357ef2 100644 --- a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py +++ b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py @@ -217,8 +217,9 @@ def generate_launch_description(): '-x', '0.2', '-y', '0.7', '-z', '1.2', - '-P', '0', - '-Y', '-1.8', + '-R', '0.0', + '-P', '-1.57', + '-Y', '-0.35', '-timeout', '10000', ], parameters=[{'use_sim_time': True}], From c58fd82c302b3bc1c1bf5063658ba8b2fbb8d5ce Mon Sep 17 00:00:00 2001 From: NikolaRaicevic Date: Fri, 4 Oct 2024 16:00:10 -0700 Subject: [PATCH 16/30] Adding gripper --- .../config/default_urdf_arguments/end_effector.yaml | 2 +- xarm_description/urdf/xarm6/xarm6.urdf.xacro | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/xarm_description/config/default_urdf_arguments/end_effector.yaml b/xarm_description/config/default_urdf_arguments/end_effector.yaml index c8499692..d4dd3024 100644 --- a/xarm_description/config/default_urdf_arguments/end_effector.yaml +++ b/xarm_description/config/default_urdf_arguments/end_effector.yaml @@ -1,5 +1,5 @@ # end-effector -gripper: false +gripper: true vacuum_gripper: false bio_gripper: false realsense_d435i: false diff --git a/xarm_description/urdf/xarm6/xarm6.urdf.xacro b/xarm_description/urdf/xarm6/xarm6.urdf.xacro index d95df9a5..f8e9ce62 100755 --- a/xarm_description/urdf/xarm6/xarm6.urdf.xacro +++ b/xarm_description/urdf/xarm6/xarm6.urdf.xacro @@ -105,7 +105,7 @@ - + - + + Date: Mon, 14 Oct 2024 11:47:47 -0700 Subject: [PATCH 17/30] minor --- .../config/default_urdf_arguments/end_effector.yaml | 2 +- xarm_sdk/cxx | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/xarm_description/config/default_urdf_arguments/end_effector.yaml b/xarm_description/config/default_urdf_arguments/end_effector.yaml index d4dd3024..c8499692 100644 --- a/xarm_description/config/default_urdf_arguments/end_effector.yaml +++ b/xarm_description/config/default_urdf_arguments/end_effector.yaml @@ -1,5 +1,5 @@ # end-effector -gripper: true +gripper: false vacuum_gripper: false bio_gripper: false realsense_d435i: false diff --git a/xarm_sdk/cxx b/xarm_sdk/cxx index 87f47358..fa91a437 160000 --- a/xarm_sdk/cxx +++ b/xarm_sdk/cxx @@ -1 +1 @@ -Subproject commit 87f4735890a6e8e9daf7489db47a1bb085e85d81 +Subproject commit fa91a43794c12b84821058635ad65b955775892f From 11389584724f7f603dac00ce22c60203c512c2a7 Mon Sep 17 00:00:00 2001 From: NikolaRaicevic Date: Wed, 16 Oct 2024 16:31:32 -0700 Subject: [PATCH 18/30] octomap --- .../default_urdf_arguments/end_effector.yaml | 2 +- .../_robot_beside_table_gazebo.launch.py | 25 ++++++------- xarm_moveit_config/config/sensors.yaml | 11 ++++++ .../launch/_robot_moveit_common.launch.py | 35 +++++++++++-------- .../launch/_robot_moveit_gazebo.launch.py | 5 +++ 5 files changed, 51 insertions(+), 27 deletions(-) create mode 100644 xarm_moveit_config/config/sensors.yaml diff --git a/xarm_description/config/default_urdf_arguments/end_effector.yaml b/xarm_description/config/default_urdf_arguments/end_effector.yaml index c8499692..d4dd3024 100644 --- a/xarm_description/config/default_urdf_arguments/end_effector.yaml +++ b/xarm_description/config/default_urdf_arguments/end_effector.yaml @@ -1,5 +1,5 @@ # end-effector -gripper: false +gripper: true vacuum_gripper: false bio_gripper: false realsense_d435i: false diff --git a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py index 19357ef2..5778f101 100644 --- a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py +++ b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py @@ -22,7 +22,7 @@ def build_robot_description(this_robot_prefix="", this_robot_namespace="", add_g ros2_control_params = generate_ros2_control_params_temp_file( os.path.join(get_package_share_directory('xarm_controller'),'config', 'xarm6_controllers.yaml'), prefix=this_robot_prefix, - add_gripper= add_gripper, #TODO: fix this later + add_gripper= add_gripper,#TODO: fix this later add_bio_gripper=False, ros_namespace=this_robot_namespace, update_rate=1000, @@ -44,16 +44,17 @@ def build_robot_description(this_robot_prefix="", this_robot_namespace="", add_g } return robot_description -def build_camera_description(this_robot_namespace=""): - robot_description = { +def build_camera_description(camera_namespace=""): + camera_robot_description = { 'robot_description': get_xacro_command( - xacro_file=PathJoinSubstitution([FindPackageShare('main'), 'urdf','camera', 'sensor_d455.urdf.xacro']), + # xacro_file=PathJoinSubstitution([FindPackageShare('main'), 'urdf', 'camera', 'sensor_d455.urdf.xacro']), + xacro_file=PathJoinSubstitution([FindPackageShare('realsense2_description'), 'urdf', 'test_d455_camera.urdf.xacro']), mappings={ - 'prefix': this_robot_namespace, + 'prefix': camera_namespace, } ), } - return robot_description + return camera_robot_description def get_per_robot_stack(robot_idx, load_controller): """ @@ -159,7 +160,7 @@ def get_per_robot_stack(robot_idx, load_controller): def generate_launch_description(): camera_namespace = LaunchConfiguration('camera_namespace', default='camera_01') - camera_robot_description = build_camera_description(this_robot_namespace=camera_namespace) + camera_robot_description = build_camera_description(camera_namespace=camera_namespace) load_controller_config = LaunchConfiguration('load_controller', default=True) num_robots_config = LaunchConfiguration('num_robots', default=1) world_sdf_path = os.path.join(get_package_share_directory('main'), 'world', 'world.sdf') @@ -214,12 +215,12 @@ def generate_launch_description(): arguments=[ '-topic', 'robot_description', '-allow_renaming', 'false', - '-x', '0.2', - '-y', '0.7', - '-z', '1.2', + '-x', '0.5', + '-y', '0.9', + '-z', '1.15', '-R', '0.0', - '-P', '-1.57', - '-Y', '-0.35', + '-P', '0.0', + '-Y', '-1.9', '-timeout', '10000', ], parameters=[{'use_sim_time': True}], diff --git a/xarm_moveit_config/config/sensors.yaml b/xarm_moveit_config/config/sensors.yaml new file mode 100644 index 00000000..2bc7047e --- /dev/null +++ b/xarm_moveit_config/config/sensors.yaml @@ -0,0 +1,11 @@ +sensors: ["realsense_points"] + +realsense_points: + sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater + point_cloud_topic: /camera/camera/depth/color/points + max_range: 5.0 + point_subsample: 1 + padding_offset: 0.1 + padding_scale: 1.0 + max_update_rate: 1.0 + filtered_cloud_topic: filtered_cloud \ No newline at end of file diff --git a/xarm_moveit_config/launch/_robot_moveit_common.launch.py b/xarm_moveit_config/launch/_robot_moveit_common.launch.py index 9745a8cb..2f3a0165 100644 --- a/xarm_moveit_config/launch/_robot_moveit_common.launch.py +++ b/xarm_moveit_config/launch/_robot_moveit_common.launch.py @@ -12,6 +12,9 @@ from launch.event_handlers import OnProcessExit from launch.events import Shutdown +import yaml +from uf_ros_lib.moveit_configs_builder import MoveItConfigsBuilder + def launch_setup(context, *args, **kwargs): controllers_name = LaunchConfiguration('controllers_name', default='fake_controllers') hw_ns = LaunchConfiguration('hw_ns', default='xarm') @@ -156,18 +159,19 @@ def launch_setup(context, *args, **kwargs): # }, } - # sensor_manager_parameters = { - # 'sensors': ['ros'], - # 'octomap_resolution': 0.02, - # 'ros.sensor_plugin': 'occupancy_map_monitor/PointCloudOctomapUpdater', - # 'ros.point_cloud_topic': '/camera/depth/color/points', - # 'ros.max_range': 2.0, - # 'ros.point_subsample': 1, - # 'ros.padding_offset': 0.1, - # 'ros.padding_scale': 1.0, - # 'ros.max_update_rate': 1.0, - # 'ros.filtered_cloud_topic': 'filtered_cloud', - # } + sensor_manager_parameters = { + 'sensors': ['realsense_points'], + 'realsense_points': { + 'sensor_plugin': 'occupancy_map_monitor/PointCloudOctomapUpdater', + 'point_cloud_topic': '/camera_01/camera_depth/points', + 'max_range': 2.0, + 'point_subsample': 1, + 'padding_offset': 0.1, + 'padding_scale': 1.0, + 'max_update_rate': 1.0, + 'filtered_cloud_topic': 'filtered_cloud', + } + } # Start the actual move_group node/action server move_group_node = Node( @@ -181,8 +185,11 @@ def launch_setup(context, *args, **kwargs): plan_execution, moveit_controllers, planning_scene_monitor_parameters, - # sensor_manager_parameters, - {'use_sim_time': use_sim_time}, + sensor_manager_parameters, + {'use_sim_time': use_sim_time}, # Simulation time flag + {'octomap_topic': '/octomap_full'}, # Octomap topic name + {'octomap_resolution': 0.05}, # Resolution of the Octomap + {'octomap_queue_size': 100}, # Queue size for the Octomap topic {'publish_robot_description_semantic': True}, ], ) diff --git a/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py b/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py index a3c0a8ff..0dbd3c2a 100644 --- a/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py +++ b/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py @@ -52,4 +52,9 @@ def generate_launch_description(): robot_moveit_common_launch, ]) +# ros2 run tf2_ros static_transform_publisher --x 0.5 --y 0.9 --z 1.15 --roll 0 --pitch -1.57 --yaw -0.33 --frame-id world --child-frame-id camera_01_bottom_screw_frame +# ros2 run tf2_ros static_transform_publisher --x 0 --y -0.3 --z 1.021 --roll 0 --pitch 0 --yaw 1.571 --frame-id world --child-frame-id xarm6_link_base + + + From 28a0b68056b3d33fb5c949998543a5f06eefd2bb Mon Sep 17 00:00:00 2001 From: Brian Lee Date: Wed, 30 Oct 2024 15:04:03 -0700 Subject: [PATCH 19/30] Fix TF tree (#10) * TF tree now connected, but robot falls over * robot doesn't fall down now, but sim is slow * use keti_gz_utils create instead * remove the 'heavy' link --------- Co-authored-by: brian --- .../config/default_urdf_arguments/xarm6.yaml | 2 +- xarm_description/urdf/xarm_device.urdf.xacro | 3 +- xarm_description/urdf/xarm_device_macro.xacro | 33 ++++++--- .../_robot_beside_table_gazebo.launch.py | 72 +++++++++++++------ xarm_moveit_config/rviz/moveit.rviz | 2 +- xarm_moveit_config/rviz/planner.rviz | 2 +- xarm_moveit_config/srdf/xarm.srdf.xacro | 2 +- xarm_moveit_config/srdf/xarm_macro.srdf.xacro | 2 +- 8 files changed, 84 insertions(+), 34 deletions(-) diff --git a/xarm_description/config/default_urdf_arguments/xarm6.yaml b/xarm_description/config/default_urdf_arguments/xarm6.yaml index 7712f508..e57a4c6c 100644 --- a/xarm_description/config/default_urdf_arguments/xarm6.yaml +++ b/xarm_description/config/default_urdf_arguments/xarm6.yaml @@ -15,7 +15,7 @@ limited: false effort_control: false velocity_control: false # urdf-related -attach_to: 'world' +attach_to: 'xarm_device' #TODO: this currently has to match the robot name in the URDF. Fix in gazebo to change frame id. attach_xyz: '0 0 0' attach_rpy: '0 0 0' create_attach_link: true diff --git a/xarm_description/urdf/xarm_device.urdf.xacro b/xarm_description/urdf/xarm_device.urdf.xacro index 3b573346..4c0793a4 100755 --- a/xarm_description/urdf/xarm_device.urdf.xacro +++ b/xarm_description/urdf/xarm_device.urdf.xacro @@ -1,6 +1,6 @@ - + @@ -80,6 +80,7 @@ attach_to="${robot_spec['attach_to']}" attach_xyz="${robot_spec['attach_xyz']}" attach_rpy="${robot_spec['attach_rpy']}" + create_attach_link="${robot_spec['create_attach_link']}" mesh_suffix="${robot_spec['mesh_suffix']}" kinematics_suffix="${robot_spec['kinematics_suffix']}" diff --git a/xarm_description/urdf/xarm_device_macro.xacro b/xarm_description/urdf/xarm_device_macro.xacro index 266aaefc..d03bf56f 100644 --- a/xarm_description/urdf/xarm_device_macro.xacro +++ b/xarm_description/urdf/xarm_device_macro.xacro @@ -68,15 +68,18 @@ - - - + + + + - - - - - + + + + + + + @@ -303,6 +306,20 @@ + + + false + false + false + false + true + true + true + 10 + false + 10 + + \ No newline at end of file diff --git a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py index 5778f101..72b4483c 100644 --- a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py +++ b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py @@ -88,26 +88,46 @@ def get_per_robot_stack(robot_idx, load_controller): ) # gazebo spawn entity node - gazebo_spawn_entity_node = Node( - package="ros_gz_sim", - executable="create", + # gazebo_spawn_entity_node = Node( + # package="ros_gz_sim", + # executable="create", + # namespace=this_robot_namespace, + # output='screen', + # arguments=[ + # '-topic', f'robot_description', + # '-allow_renaming', 'false', + # '-x', str(0.0 + robot_idx * 0.4), + # '-y', '-0.3', + # '-z', '1.021', + # '-Y', '1.571', + # '-timeout', '10000', + # ], + # parameters=[{'use_sim_time': True}], + # ) + + # nodes_to_launch.append( + # gazebo_spawn_entity_node, + # ) + + spawn_entity_test_node = Node( + package="keti_gz_utils", + executable="create_on_table", namespace=this_robot_namespace, output='screen', - arguments=[ - '-topic', f'robot_description', - '-allow_renaming', 'false', - '-x', str(0.0 + robot_idx * 0.4), - '-y', '-0.3', - '-z', '1.021', - '-Y', '1.571', - '-timeout', '10000', - ], - parameters=[{'use_sim_time': True}], + # NOTE: this version uses parameters instead of CLI arguments. + # This leads to cleaner dependencies. + parameters=[{ + 'use_sim_time': True, + 'topic': 'robot_description', + 'allow_renaming': False, + 'x': 0.0 + robot_idx * 0.4, + 'y': -0.3, + 'z': 1.021, + 'Y': 1.571 + }], ) - nodes_to_launch.append( - gazebo_spawn_entity_node, - ) + nodes_to_launch.append(spawn_entity_test_node) # # Load controllers controllers = [ @@ -151,7 +171,7 @@ def get_per_robot_stack(robot_idx, load_controller): nodes_to_launch.append( RegisterEventHandler( event_handler=OnProcessExit( - target_action=gazebo_spawn_entity_node, + target_action=spawn_entity_test_node, on_exit=load_controllers ) ) @@ -169,7 +189,7 @@ def generate_launch_description(): gazebo_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 'launch', 'gz_sim.launch.py'])), launch_arguments={ - 'gz_args': f'-v4 -r {world_sdf_path}', + 'gz_args': f' -r {world_sdf_path}', }.items(), ) @@ -186,14 +206,26 @@ def generate_launch_description(): [camera_namespace, '/camera_ired1@sensor_msgs/msg/Image@ignition.msgs.Image'], [camera_namespace, '/camera_ired2@sensor_msgs/msg/Image@ignition.msgs.Image'], '/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock', - '/model/sensor_d455/pose@tf2_msgs/msg/TFMessage[ignition.msgs.Pose_V', + '/model/realsense2_camera/pose@tf2_msgs/msg/TFMessage[ignition.msgs.Pose_V', + '/model/xarm_device/pose@tf2_msgs/msg/TFMessage[ignition.msgs.Pose_V', ] ) + relay_nodes = [ + Node( + package="topic_tools", + executable="relay", + arguments=[ + f"/model/{source}/pose", + "/tf" + ] + ) for source in ["realsense2_camera", "xarm_device"] + ] + nodes_to_launch = [ gazebo_launch, parameters_bridge, - ] + ] + relay_nodes # Node for launching camera robot state publisher robot_state_publisher_node_camera = Node( diff --git a/xarm_moveit_config/rviz/moveit.rviz b/xarm_moveit_config/rviz/moveit.rviz index c92f9d4e..436118db 100644 --- a/xarm_moveit_config/rviz/moveit.rviz +++ b/xarm_moveit_config/rviz/moveit.rviz @@ -325,7 +325,7 @@ Visualization Manager: Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: world + Fixed Frame: world_table Frame Rate: 30 Name: root Tools: diff --git a/xarm_moveit_config/rviz/planner.rviz b/xarm_moveit_config/rviz/planner.rviz index adcab548..08568c5d 100644 --- a/xarm_moveit_config/rviz/planner.rviz +++ b/xarm_moveit_config/rviz/planner.rviz @@ -310,7 +310,7 @@ Visualization Manager: Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: world + Fixed Frame: world_table Frame Rate: 30 Name: root Tools: diff --git a/xarm_moveit_config/srdf/xarm.srdf.xacro b/xarm_moveit_config/srdf/xarm.srdf.xacro index c1b7614d..fca4cef7 100644 --- a/xarm_moveit_config/srdf/xarm.srdf.xacro +++ b/xarm_moveit_config/srdf/xarm.srdf.xacro @@ -1,5 +1,5 @@ - + diff --git a/xarm_moveit_config/srdf/xarm_macro.srdf.xacro b/xarm_moveit_config/srdf/xarm_macro.srdf.xacro index 1789a6d9..8c4d66da 100644 --- a/xarm_moveit_config/srdf/xarm_macro.srdf.xacro +++ b/xarm_moveit_config/srdf/xarm_macro.srdf.xacro @@ -1,5 +1,5 @@ - + From 3361a3deb460ee3a2c0382ef48cbf2ccc8ee6aeb Mon Sep 17 00:00:00 2001 From: NikolaRaicevic Date: Thu, 31 Oct 2024 16:43:10 -0700 Subject: [PATCH 20/30] world_table.sdf changed --- xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py index 72b4483c..4ce8a7c9 100644 --- a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py +++ b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py @@ -183,7 +183,7 @@ def generate_launch_description(): camera_robot_description = build_camera_description(camera_namespace=camera_namespace) load_controller_config = LaunchConfiguration('load_controller', default=True) num_robots_config = LaunchConfiguration('num_robots', default=1) - world_sdf_path = os.path.join(get_package_share_directory('main'), 'world', 'world.sdf') + world_sdf_path = os.path.join(get_package_share_directory('main'), 'world', 'world_table.sdf') # Gazebo launch gazebo_launch = IncludeLaunchDescription( From b2307cbe3f15ea109276015df2fc4aa031773a6b Mon Sep 17 00:00:00 2001 From: NikolaRaicevic Date: Thu, 31 Oct 2024 17:02:43 -0700 Subject: [PATCH 21/30] =?UTF-8?q?Added=20=1B[200~#=20Software=20License=20?= =?UTF-8?q?Agreement=20(BSD=20License)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- xarm_controller/launch/_ros2_control.launch.py | 1 + xarm_controller/launch/lib/robot_controller_lib.py | 1 + xarm_description/launch/_robot_description.launch.py | 1 + xarm_description/launch/_robot_joint_state.launch.py | 1 + xarm_moveit_config/launch/_robot_moveit_common.launch.py | 1 + xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py | 1 + xarm_moveit_config/launch/lib/robot_moveit_config_lib.py | 1 + 7 files changed, 7 insertions(+) diff --git a/xarm_controller/launch/_ros2_control.launch.py b/xarm_controller/launch/_ros2_control.launch.py index f109fbbf..28af3a51 100644 --- a/xarm_controller/launch/_ros2_control.launch.py +++ b/xarm_controller/launch/_ros2_control.launch.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +# Software License Agreement (BSD License) import os from ament_index_python import get_package_share_directory diff --git a/xarm_controller/launch/lib/robot_controller_lib.py b/xarm_controller/launch/lib/robot_controller_lib.py index a744a557..3810e8eb 100644 --- a/xarm_controller/launch/lib/robot_controller_lib.py +++ b/xarm_controller/launch/lib/robot_controller_lib.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +# Software License Agreement (BSD License) import os import yaml diff --git a/xarm_description/launch/_robot_description.launch.py b/xarm_description/launch/_robot_description.launch.py index af300b41..fb252351 100644 --- a/xarm_description/launch/_robot_description.launch.py +++ b/xarm_description/launch/_robot_description.launch.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +# Software License Agreement (BSD License) import os import sys diff --git a/xarm_description/launch/_robot_joint_state.launch.py b/xarm_description/launch/_robot_joint_state.launch.py index 767165fd..f35033d0 100644 --- a/xarm_description/launch/_robot_joint_state.launch.py +++ b/xarm_description/launch/_robot_joint_state.launch.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +# Software License Agreement (BSD License) from launch import LaunchDescription from launch.actions import OpaqueFunction, IncludeLaunchDescription diff --git a/xarm_moveit_config/launch/_robot_moveit_common.launch.py b/xarm_moveit_config/launch/_robot_moveit_common.launch.py index 2f3a0165..634c68d5 100644 --- a/xarm_moveit_config/launch/_robot_moveit_common.launch.py +++ b/xarm_moveit_config/launch/_robot_moveit_common.launch.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +# Software License Agreement (BSD License) import os from ament_index_python import get_package_share_directory diff --git a/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py b/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py index 0dbd3c2a..bfef489a 100644 --- a/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py +++ b/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +# Software License Agreement (BSD License) from launch import LaunchDescription from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument diff --git a/xarm_moveit_config/launch/lib/robot_moveit_config_lib.py b/xarm_moveit_config/launch/lib/robot_moveit_config_lib.py index 548a7f67..7f1a3b5d 100644 --- a/xarm_moveit_config/launch/lib/robot_moveit_config_lib.py +++ b/xarm_moveit_config/launch/lib/robot_moveit_config_lib.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +# Software License Agreement (BSD License) import os import yaml From 98462cb99b3c0fb70eb11cc435f6c25367e75d1f Mon Sep 17 00:00:00 2001 From: brian Date: Fri, 1 Nov 2024 11:26:42 -0700 Subject: [PATCH 22/30] use a simple timer for the moment --- xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py b/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py index bfef489a..c7a5dba2 100644 --- a/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py +++ b/xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py @@ -2,7 +2,7 @@ # Software License Agreement (BSD License) from launch import LaunchDescription -from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument +from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument, TimerAction from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.substitutions import FindPackageShare @@ -50,7 +50,10 @@ def generate_launch_description(): return LaunchDescription([ robot_gazebo_launch, - robot_moveit_common_launch, + TimerAction( + period=5.0, + actions=[robot_moveit_common_launch] + ) ]) # ros2 run tf2_ros static_transform_publisher --x 0.5 --y 0.9 --z 1.15 --roll 0 --pitch -1.57 --yaw -0.33 --frame-id world --child-frame-id camera_01_bottom_screw_frame From 83236c00409212021e4e120f2ebdb632e13ac2b6 Mon Sep 17 00:00:00 2001 From: NikolaRaicevic2001 Date: Sun, 17 Nov 2024 21:50:15 -0800 Subject: [PATCH 23/30] minor --- xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py index 4ce8a7c9..391c39de 100644 --- a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py +++ b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py @@ -14,6 +14,8 @@ from launch.event_handlers import OnProcessExit from uf_ros_lib.uf_robot_utils import get_xacro_command +import yaml + def build_robot_description(this_robot_prefix="", this_robot_namespace="", add_gripper = False): # ros2 control params ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='ign_ros2_control/IgnitionSystem') @@ -28,6 +30,10 @@ def build_robot_description(this_robot_prefix="", this_robot_namespace="", add_g update_rate=1000, robot_type='xarm' ) + + # Export the generated ros2 control params + with open(f'ros2_control_params.yaml', 'w') as file: + yaml.dump(ros2_control_params, file, sort_keys=False) print(f"Generated temporary control params at {ros2_control_params}") # robot_description @@ -129,7 +135,7 @@ def get_per_robot_stack(robot_idx, load_controller): nodes_to_launch.append(spawn_entity_test_node) - # # Load controllers + # Load controllers controllers = [ 'joint_state_broadcaster', # the below becomes something like xarm0_xarm6_traj_controller. From 0766c78cccfdf376ef4eae2260e0d28f70c01701 Mon Sep 17 00:00:00 2001 From: Niyas-A Date: Fri, 3 Jan 2025 13:14:07 -0800 Subject: [PATCH 24/30] updated real robot_ip --- xarm_description/config/default_urdf_arguments/xarm6.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/xarm_description/config/default_urdf_arguments/xarm6.yaml b/xarm_description/config/default_urdf_arguments/xarm6.yaml index e57a4c6c..b0f0ced3 100644 --- a/xarm_description/config/default_urdf_arguments/xarm6.yaml +++ b/xarm_description/config/default_urdf_arguments/xarm6.yaml @@ -2,7 +2,7 @@ # hw_ns: 'xarm' # changes at runtime # ros2_control_plugin: 'uf_robot_hardware/UFRobotSystemHardware' # changes at runtime # ros2_control_params: '' # changes at runtime -robot_ip: '' +robot_ip: '192.168.1.242' dof: 6 robot_type: 'xarm' report_type: 'normal' From 9dea4a8ecd9f616ee87859ee040cf68f63f711bb Mon Sep 17 00:00:00 2001 From: Dwait Bhatt Date: Tue, 14 Jan 2025 19:55:22 -0800 Subject: [PATCH 25/30] Gazebo velocity controller --- xarm_controller/config/xarm6_controllers.yaml | 46 +++++++++++++++++++ .../urdf/xarm6/xarm6.ros2_control.xacro | 24 ++++++++++ .../_robot_beside_table_gazebo.launch.py | 4 +- 3 files changed, 73 insertions(+), 1 deletion(-) diff --git a/xarm_controller/config/xarm6_controllers.yaml b/xarm_controller/config/xarm6_controllers.yaml index 35c0d476..7717c5d4 100644 --- a/xarm_controller/config/xarm6_controllers.yaml +++ b/xarm_controller/config/xarm6_controllers.yaml @@ -4,8 +4,18 @@ controller_manager: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster + position_controller: + type: position_controllers/JointGroupPositionController + + velocity_controller: + type: velocity_controllers/JointGroupVelocityController + + effort_controller: + type: effort_controllers/JointGroupEffortController + traj_controller: type: joint_trajectory_controller/JointTrajectoryController + # Original comment: only for fake control (?) gripper_traj_controller: type: joint_trajectory_controller/JointTrajectoryController @@ -14,6 +24,42 @@ controller_manager: bio_gripper_traj_controller: type: joint_trajectory_controller/JointTrajectoryController +position_controller: + ros__parameters: + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + +velocity_controller: + ros__parameters: + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + interface_name: velocity + command_interfaces: + - velocity + state_interfaces: + - position + - velocity + +effort_controller: + ros__parameters: + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + traj_controller: ros__parameters: command_interfaces: diff --git a/xarm_description/urdf/xarm6/xarm6.ros2_control.xacro b/xarm_description/urdf/xarm6/xarm6.ros2_control.xacro index 0d41e443..cddf9edc 100644 --- a/xarm_description/urdf/xarm6/xarm6.ros2_control.xacro +++ b/xarm_description/urdf/xarm6/xarm6.ros2_control.xacro @@ -45,6 +45,10 @@ -3.14 3.14 + @@ -58,6 +62,10 @@ -3.14 3.14 + @@ -71,6 +79,10 @@ -3.14 3.14 + @@ -84,6 +96,10 @@ -3.14 3.14 + @@ -97,6 +113,10 @@ -3.14 3.14 + @@ -110,6 +130,10 @@ -3.14 3.14 + diff --git a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py index 391c39de..6fbea146 100644 --- a/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py +++ b/xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py @@ -141,7 +141,9 @@ def get_per_robot_stack(robot_idx, load_controller): # the below becomes something like xarm0_xarm6_traj_controller. # The first "xarm0" is from prefix. # The second needs to match what's in xarm_control/config/*.yaml - f'{this_robot_prefix}traj_controller', + # f'{this_robot_prefix}traj_controller', + f'{this_robot_prefix}velocity_controller', + # f'{this_robot_prefix}effort_controller', ] # TODO: fix gripper loading for controllers From bb8783def351797f1797a3c93fa3fd074de1522d Mon Sep 17 00:00:00 2001 From: Dwait Bhatt Date: Wed, 15 Jan 2025 11:44:39 -0800 Subject: [PATCH 26/30] Velocity controller --- xarm_description/config/default_urdf_arguments/xarm6.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/xarm_description/config/default_urdf_arguments/xarm6.yaml b/xarm_description/config/default_urdf_arguments/xarm6.yaml index b0f0ced3..67feb21e 100644 --- a/xarm_description/config/default_urdf_arguments/xarm6.yaml +++ b/xarm_description/config/default_urdf_arguments/xarm6.yaml @@ -13,7 +13,7 @@ robot_sn: '' # control-related limited: false effort_control: false -velocity_control: false +velocity_control: true # urdf-related attach_to: 'xarm_device' #TODO: this currently has to match the robot name in the URDF. Fix in gazebo to change frame id. attach_xyz: '0 0 0' From 5f8fcd96df52b4c6a53e29621cf3a06bd9ae3685 Mon Sep 17 00:00:00 2001 From: NikolaRaicevic2001 Date: Tue, 21 Jan 2025 17:33:31 -0800 Subject: [PATCH 27/30] Controller specifing robot description --- .../launch/_ros2_control.launch.py | 23 +++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) diff --git a/xarm_controller/launch/_ros2_control.launch.py b/xarm_controller/launch/_ros2_control.launch.py index 28af3a51..4cebc97a 100644 --- a/xarm_controller/launch/_ros2_control.launch.py +++ b/xarm_controller/launch/_ros2_control.launch.py @@ -44,6 +44,22 @@ def launch_setup(context, *args, **kwargs): ) } + robot_state_publisher_node = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[robot_description], + # BL: don't remap /tf to custom topics. + # I.e., multiple robots publish to same /tf topic + # Instead, use prefix to distinguish different robots + # remappings=[ + # ('/tf', 'tf'), + # ('/tf_static', 'tf_static'), + # ] + # namespace=this_robot_namespace + ) + + mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('xarm_api'), 'launch', 'lib', 'robot_api_lib.py')) generate_robot_api_params = getattr(mod, 'generate_robot_api_params') robot_params = generate_robot_api_params( @@ -57,15 +73,18 @@ def launch_setup(context, *args, **kwargs): package='controller_manager', executable='ros2_control_node', parameters=[ - robot_description, ros2_control_params, robot_params, ], + remappings=[ + ('~/robot_description', '/robot_description'), + ], output='screen', ) return [ - ros2_control_node + ros2_control_node, + robot_state_publisher_node ] def generate_launch_description(): From 2394a45fe99e34a024e13747c583fb8932e8801a Mon Sep 17 00:00:00 2001 From: NikolaRaicevic2001 Date: Wed, 22 Jan 2025 17:28:02 -0800 Subject: [PATCH 28/30] use different kinematics yaml between sim and real --- .../default/xarm6_default_kinematics_sim.yaml | 43 +++++++++++++++++++ xarm_description/urdf/xarm6/xarm6.urdf.xacro | 6 +-- xarm_description/urdf/xarm_device.urdf.xacro | 3 +- .../_robot_beside_table_gazebo.launch.py | 1 + 4 files changed, 49 insertions(+), 4 deletions(-) create mode 100644 xarm_description/config/kinematics/default/xarm6_default_kinematics_sim.yaml diff --git a/xarm_description/config/kinematics/default/xarm6_default_kinematics_sim.yaml b/xarm_description/config/kinematics/default/xarm6_default_kinematics_sim.yaml new file mode 100644 index 00000000..278b3641 --- /dev/null +++ b/xarm_description/config/kinematics/default/xarm6_default_kinematics_sim.yaml @@ -0,0 +1,43 @@ +kinematics: + joint1: + x: 0 + y: 0 + z: 0.267 + roll: 0 + pitch: 0 + yaw: 0 + joint2: + x: 0 + y: 0 + z: 0 + roll: -1.5708 + pitch: -0.5 + yaw: 0 + joint3: + x: 0.0535 + y: -0.2845 + z: 0 + roll: 0 + pitch: 0 + yaw: -0.5 + joint4: + x: 0.0775 + y: 0.3425 + z: 0 + roll: -1.5708 + pitch: 0 + yaw: 0 + joint5: + x: 0 + y: 0 + z: 0 + roll: 1.5708 + pitch: -0.95 + yaw: 0 + joint6: + x: 0.076 + y: 0.097 + z: 0 + roll: -1.5708 + pitch: 0 + yaw: 0 \ No newline at end of file diff --git a/xarm_description/urdf/xarm6/xarm6.urdf.xacro b/xarm_description/urdf/xarm6/xarm6.urdf.xacro index f8e9ce62..d9c43871 100755 --- a/xarm_description/urdf/xarm6/xarm6.urdf.xacro +++ b/xarm_description/urdf/xarm6/xarm6.urdf.xacro @@ -105,7 +105,7 @@ - + - + - + + Date: Wed, 22 Jan 2025 19:09:56 -0800 Subject: [PATCH 29/30] Offset handling --- xarm_controller/launch/_ros2_control.launch.py | 2 ++ .../config/kinematics/default/xarm6_default_kinematics.yaml | 4 ++-- ..._default_kinematics_sim.yaml => xarm6_kinematics_sim.yaml} | 0 3 files changed, 4 insertions(+), 2 deletions(-) rename xarm_description/config/kinematics/default/{xarm6_default_kinematics_sim.yaml => xarm6_kinematics_sim.yaml} (100%) diff --git a/xarm_controller/launch/_ros2_control.launch.py b/xarm_controller/launch/_ros2_control.launch.py index 4cebc97a..bd548cba 100644 --- a/xarm_controller/launch/_ros2_control.launch.py +++ b/xarm_controller/launch/_ros2_control.launch.py @@ -14,6 +14,7 @@ def launch_setup(context, *args, **kwargs): prefix = LaunchConfiguration('prefix', default='') hw_ns = LaunchConfiguration('hw_ns', default='xarm') + kinematics_suffix = LaunchConfiguration('hw_ns', default='') ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='uf_robot_hardware/UFRobotSystemHardware') xacro_file = LaunchConfiguration('xacro_file', default=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro'])) @@ -40,6 +41,7 @@ def launch_setup(context, *args, **kwargs): 'hw_ns': hw_ns.perform(context).strip('/'), 'ros2_control_plugin': ros2_control_plugin, 'ros2_control_params': ros2_control_params, + 'kinematics_suffix': kinematics_suffix, } ) } diff --git a/xarm_description/config/kinematics/default/xarm6_default_kinematics.yaml b/xarm_description/config/kinematics/default/xarm6_default_kinematics.yaml index 4047e93b..68a7fa38 100644 --- a/xarm_description/config/kinematics/default/xarm6_default_kinematics.yaml +++ b/xarm_description/config/kinematics/default/xarm6_default_kinematics.yaml @@ -11,7 +11,7 @@ kinematics: y: 0 z: 0 roll: -1.5708 - pitch: 0 + pitch: 0.1 yaw: 0 joint3: x: 0.0535 @@ -19,7 +19,7 @@ kinematics: z: 0 roll: 0 pitch: 0 - yaw: 0 + yaw: -0.1 joint4: x: 0.0775 y: 0.3425 diff --git a/xarm_description/config/kinematics/default/xarm6_default_kinematics_sim.yaml b/xarm_description/config/kinematics/default/xarm6_kinematics_sim.yaml similarity index 100% rename from xarm_description/config/kinematics/default/xarm6_default_kinematics_sim.yaml rename to xarm_description/config/kinematics/default/xarm6_kinematics_sim.yaml From 1c7f8df42ecf56220f3f0d4ddb67f8a13ce72857 Mon Sep 17 00:00:00 2001 From: brian Date: Tue, 14 Oct 2025 08:22:14 -0700 Subject: [PATCH 30/30] add user/xarm6_kinematics_sim.yaml --- .../kinematics/user/xarm6_kinematics_sim.yaml | 43 +++++++++++++++++++ 1 file changed, 43 insertions(+) create mode 100644 xarm_description/config/kinematics/user/xarm6_kinematics_sim.yaml diff --git a/xarm_description/config/kinematics/user/xarm6_kinematics_sim.yaml b/xarm_description/config/kinematics/user/xarm6_kinematics_sim.yaml new file mode 100644 index 00000000..278b3641 --- /dev/null +++ b/xarm_description/config/kinematics/user/xarm6_kinematics_sim.yaml @@ -0,0 +1,43 @@ +kinematics: + joint1: + x: 0 + y: 0 + z: 0.267 + roll: 0 + pitch: 0 + yaw: 0 + joint2: + x: 0 + y: 0 + z: 0 + roll: -1.5708 + pitch: -0.5 + yaw: 0 + joint3: + x: 0.0535 + y: -0.2845 + z: 0 + roll: 0 + pitch: 0 + yaw: -0.5 + joint4: + x: 0.0775 + y: 0.3425 + z: 0 + roll: -1.5708 + pitch: 0 + yaw: 0 + joint5: + x: 0 + y: 0 + z: 0 + roll: 1.5708 + pitch: -0.95 + yaw: 0 + joint6: + x: 0.076 + y: 0.097 + z: 0 + roll: -1.5708 + pitch: 0 + yaw: 0 \ No newline at end of file