From 0f54e1cd37aa325ae491d76afebc5e11d93b44fd Mon Sep 17 00:00:00 2001 From: GerardoFJ Date: Fri, 24 Jan 2025 00:55:10 -0700 Subject: [PATCH 1/3] test_urdf --- .../launch/test_xarm_launch.launch.py | 283 ++++++++++++++++++ .../frida_description/urdf/FRIDA.urdf.xacro | 6 +- .../frida_description/urdf/gripper.xacro | 2 +- .../frida_description/urdf/robot.xacro | 2 +- .../worlds/empty_classic.world | 20 ++ 5 files changed, 308 insertions(+), 5 deletions(-) create mode 100644 robot_description/frida_description/launch/test_xarm_launch.launch.py create mode 100755 robot_description/frida_description/worlds/empty_classic.world diff --git a/robot_description/frida_description/launch/test_xarm_launch.launch.py b/robot_description/frida_description/launch/test_xarm_launch.launch.py new file mode 100644 index 0000000..667902c --- /dev/null +++ b/robot_description/frida_description/launch/test_xarm_launch.launch.py @@ -0,0 +1,283 @@ +#!/usr/bin/env python3 +# Software License Agreement (BSD License) +# +# Copyright (c) 2021, UFACTORY, Inc. +# All rights reserved. +# +# Author: Vinman + +import os +import yaml +from ament_index_python import get_package_share_directory +from launch.launch_description_sources import load_python_launch_file_as_module +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, RegisterEventHandler +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch.conditions import IfCondition +from launch_ros.substitutions import FindPackageShare +from launch.event_handlers import OnProcessExit, OnProcessStart +from launch.actions import OpaqueFunction + + +def launch_setup(context, *args, **kwargs): + prefix = LaunchConfiguration('prefix', default='') + hw_ns = LaunchConfiguration('hw_ns', default='xarm') + limited = LaunchConfiguration('limited', default=False) + effort_control = LaunchConfiguration('effort_control', default=False) + velocity_control = LaunchConfiguration('velocity_control', default=False) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + add_bio_gripper = LaunchConfiguration('add_bio_gripper', default=False) + dof = LaunchConfiguration('dof', default=6) + robot_type = LaunchConfiguration('robot_type', default='xarm') + ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='gazebo_ros2_control/GazeboSystem') + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + add_d435i_links = LaunchConfiguration('add_d435i_links', default=True) + model1300 = LaunchConfiguration('model1300', default=False) + robot_sn = LaunchConfiguration('robot_sn', default='') + attach_to = LaunchConfiguration('attach_to', default='base_link') + attach_xyz = LaunchConfiguration('attach_xyz', default='"0.0049756 0 0.34"') + attach_rpy = LaunchConfiguration('attach_rpy', default='"0 0 1.57"') + + add_other_geometry = LaunchConfiguration('add_other_geometry', default=False) + geometry_type = LaunchConfiguration('geometry_type', default='box') + geometry_mass = LaunchConfiguration('geometry_mass', default=0.1) + geometry_height = LaunchConfiguration('geometry_height', default=0.1) + geometry_radius = LaunchConfiguration('geometry_radius', default=0.1) + geometry_length = LaunchConfiguration('geometry_length', default=0.1) + geometry_width = LaunchConfiguration('geometry_width', default=0.1) + geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='') + geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"') + geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"') + geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"') + geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"') + kinematics_suffix = LaunchConfiguration('kinematics_suffix', default='') + + load_controller = LaunchConfiguration('load_controller', default=False) + show_rviz = LaunchConfiguration('show_rviz', default=False) + no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False) + + ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context) + moveit_config_dump = LaunchConfiguration('moveit_config_dump', default='') + + moveit_config_dump = moveit_config_dump.perform(context) + moveit_config_dict = yaml.load(moveit_config_dump, Loader=yaml.FullLoader) if moveit_config_dump else {} + 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 '') + + if not moveit_config_dict: + # 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(xarm_type)), + prefix=prefix.perform(context), + add_gripper=add_gripper.perform(context) in ('True', 'true'), + add_bio_gripper=add_bio_gripper.perform(context) in ('True', 'true'), + ros_namespace=ros_namespace, + update_rate=1000, + robot_type=robot_type.perform(context) + ) + # robot_description + # xarm_description/launch/lib/robot_description_lib.py + mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('xarm_description'), 'launch', 'lib', 'robot_description_lib.py')) + get_xacro_file_content = getattr(mod, 'get_xacro_file_content') + robot_description = { + 'robot_description': get_xacro_file_content( + xacro_file=PathJoinSubstitution([FindPackageShare('frida_description'), 'urdf', 'FRIDA.urdf.xacro']), + arguments={ + 'prefix': prefix, + 'dof': dof, + 'robot_type': robot_type, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'add_bio_gripper': add_bio_gripper, + 'hw_ns': hw_ns.perform(context).strip('/'), + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'ros2_control_plugin': ros2_control_plugin, + 'ros2_control_params': ros2_control_params, + 'add_realsense_d435i': add_realsense_d435i, + 'add_d435i_links': add_d435i_links, + 'model1300': model1300, + 'robot_sn': robot_sn, + 'attach_to': attach_to, + 'attach_xyz': attach_xyz, + 'attach_rpy': attach_rpy, + 'add_other_geometry': add_other_geometry, + 'geometry_type': geometry_type, + 'geometry_mass': geometry_mass, + 'geometry_height': geometry_height, + 'geometry_radius': geometry_radius, + 'geometry_length': geometry_length, + 'geometry_width': geometry_width, + 'geometry_mesh_filename': geometry_mesh_filename, + 'geometry_mesh_origin_xyz': geometry_mesh_origin_xyz, + 'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy, + 'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz, + 'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy, + 'kinematics_suffix': kinematics_suffix, + } + ), + } + moveit_config_dict = robot_description + else: + robot_description = {'robot_description': moveit_config_dict['robot_description']} + + # robot state publisher node + robot_state_publisher_node = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[{'use_sim_time': True}, robot_description], + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ] + ) + + # gazebo launch + # gazebo_ros/launch/gazebo.launch.py + xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('frida_description'), 'worlds', 'empty_classic.world']) + gazebo_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('gazebo_ros'), 'launch', 'gazebo.launch.py'])), + launch_arguments={ + 'world': xarm_gazebo_world, + 'server_required': 'true', + 'gui_required': 'true', + }.items(), + ) + + # gazebo spawn entity node + gazebo_spawn_entity_node = Node( + package="gazebo_ros", + executable="spawn_entity.py", + output='screen', + arguments=[ + '-topic', 'robot_description', + # '-entity', '{}'.format(xarm_type), + '-entity', 'UF_ROBOT', + '-x', '-0.2', + '-y', '-0.54' if robot_type.perform(context) == 'uf850' else '-0.5', + '-z', '0.2', + '-Y', '1.571', + ], + parameters=[{'use_sim_time': True}], + ) + + # rviz with moveit configuration + rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'rviz', 'planner.rviz' if no_gui_ctrl.perform(context) == 'true' else 'moveit.rviz']) + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file], + parameters=[ + { + 'robot_description': moveit_config_dict.get('robot_description', ''), + 'robot_description_semantic': moveit_config_dict.get('robot_description_semantic', ''), + 'robot_description_kinematics': moveit_config_dict.get('robot_description_kinematics', {}), + 'robot_description_planning': moveit_config_dict.get('robot_description_planning', {}), + 'planning_pipelines': moveit_config_dict.get('planning_pipelines', {}), + 'use_sim_time': True + } + ], + # condition=IfCondition(show_rviz), + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ] + ) + + # Load controllers + controllers = [ + 'joint_state_broadcaster', + '{}{}_traj_controller'.format(prefix.perform(context), xarm_type), + ] + if robot_type.perform(context) != 'lite' and add_gripper.perform(context) in ('True', 'true'): + controllers.append('{}{}_gripper_traj_controller'.format(prefix.perform(context), robot_type.perform(context))) + elif robot_type.perform(context) != 'lite' and add_bio_gripper.perform(context) in ('True', 'true'): + controllers.append('{}bio_gripper_traj_controller'.format(prefix.perform(context))) + + controller_nodes = [] + if load_controller.perform(context) in ('True', 'true'): + for controller in controllers: + controller_nodes.append(Node( + package='controller_manager', + executable='spawner', + output='screen', + arguments=[ + controller, + '--controller-manager', '{}/controller_manager'.format(ros_namespace) + ], + parameters=[{'use_sim_time': True}], + )) + + if len(controller_nodes) > 0: + return [ + RegisterEventHandler( + event_handler=OnProcessStart( + target_action=robot_state_publisher_node, + on_start=gazebo_launch, + ) + ), + RegisterEventHandler( + event_handler=OnProcessStart( + target_action=robot_state_publisher_node, + on_start=gazebo_spawn_entity_node, + ) + ), + RegisterEventHandler( + condition=IfCondition(show_rviz), + event_handler=OnProcessExit( + target_action=gazebo_spawn_entity_node, + on_exit=rviz2_node, + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=gazebo_spawn_entity_node, + on_exit=controller_nodes, + ) + ), + robot_state_publisher_node, + # gazebo_launch, + # gazebo_spawn_entity_node, + ] + else: + return [ + RegisterEventHandler( + event_handler=OnProcessStart( + target_action=robot_state_publisher_node, + on_start=gazebo_launch, + ) + ), + RegisterEventHandler( + event_handler=OnProcessStart( + target_action=robot_state_publisher_node, + on_start=gazebo_spawn_entity_node, + ) + ), + RegisterEventHandler( + condition=IfCondition(show_rviz), + event_handler=OnProcessExit( + target_action=gazebo_spawn_entity_node, + on_exit=rviz2_node, + ) + ), + robot_state_publisher_node, + # gazebo_launch, + # gazebo_spawn_entity_node, + ] + + +def generate_launch_description(): + return LaunchDescription([ + OpaqueFunction(function=launch_setup) + ]) diff --git a/robot_description/frida_description/urdf/FRIDA.urdf.xacro b/robot_description/frida_description/urdf/FRIDA.urdf.xacro index 80c07b1..3ec2024 100644 --- a/robot_description/frida_description/urdf/FRIDA.urdf.xacro +++ b/robot_description/frida_description/urdf/FRIDA.urdf.xacro @@ -44,7 +44,7 @@ - + @@ -70,10 +70,10 @@ model1300="$(arg model1300)" attach_to="$(arg attach_to)" attach_xyz="$(arg attach_xyz)" attach_rpy="$(arg attach_rpy)" robot_sn="$(arg robot_sn)" use_gazebo_camera="$(arg use_gazebo_camera)" mesh_suffix="$(arg mesh_suffix)" kinematics_suffix="$(arg kinematics_suffix)" /> - + \ No newline at end of file diff --git a/robot_description/frida_description/urdf/gripper.xacro b/robot_description/frida_description/urdf/gripper.xacro index a341562..326125b 100644 --- a/robot_description/frida_description/urdf/gripper.xacro +++ b/robot_description/frida_description/urdf/gripper.xacro @@ -1,7 +1,7 @@ - + diff --git a/robot_description/frida_description/urdf/robot.xacro b/robot_description/frida_description/urdf/robot.xacro index fcd6fd5..17cf5f7 100644 --- a/robot_description/frida_description/urdf/robot.xacro +++ b/robot_description/frida_description/urdf/robot.xacro @@ -1,7 +1,7 @@ - + diff --git a/robot_description/frida_description/worlds/empty_classic.world b/robot_description/frida_description/worlds/empty_classic.world new file mode 100755 index 0000000..b9181ab --- /dev/null +++ b/robot_description/frida_description/worlds/empty_classic.world @@ -0,0 +1,20 @@ + + + + + model://ground_plane + + + model://sun + + + + 0.001 + 1 + 1000 + + 0 0 0 + + + + From f6501b5743b48cc1e4531c23f274ad2847d1a68d Mon Sep 17 00:00:00 2001 From: GerardoFJ Date: Wed, 29 Jan 2025 14:11:11 -0700 Subject: [PATCH 2/3] Fixed sim launch --- .../frida_description/launch/moveit.launch.py | 152 ++++++++++ .../launch/test2_xarm_move.launch.py | 283 ++++++++++++++++++ 2 files changed, 435 insertions(+) create mode 100644 robot_description/frida_description/launch/moveit.launch.py create mode 100644 robot_description/frida_description/launch/test2_xarm_move.launch.py diff --git a/robot_description/frida_description/launch/moveit.launch.py b/robot_description/frida_description/launch/moveit.launch.py new file mode 100644 index 0000000..b44025f --- /dev/null +++ b/robot_description/frida_description/launch/moveit.launch.py @@ -0,0 +1,152 @@ +#!/usr/bin/env python3 +# Software License Agreement (BSD License) +# +# Copyright (c) 2021, UFACTORY, Inc. +# All rights reserved. +# +# Author: Vinman + +import os +import yaml +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from uf_ros_lib.moveit_configs_builder import MoveItConfigsBuilder +from uf_ros_lib.uf_robot_utils import generate_ros2_control_params_temp_file + + +def launch_setup(context, *args, **kwargs): + dof = LaunchConfiguration('dof', default=6) + robot_type = LaunchConfiguration('robot_type', default='xarm') + prefix = LaunchConfiguration('prefix', default='') + hw_ns = LaunchConfiguration('hw_ns', default='xarm') + limited = LaunchConfiguration('limited', default=True) + effort_control = LaunchConfiguration('effort_control', default=False) + velocity_control = LaunchConfiguration('velocity_control', default=False) + model1300 = LaunchConfiguration('model1300', default=False) + robot_sn = LaunchConfiguration('robot_sn', default='') + attach_to = LaunchConfiguration('attach_to', default='base_link') + attach_xyz = LaunchConfiguration('attach_xyz', default='"0.0049756 0 0.34"') + attach_rpy = LaunchConfiguration('attach_rpy', default='"0 0 1.57"') + mesh_suffix = LaunchConfiguration('mesh_suffix', default='stl') + kinematics_suffix = LaunchConfiguration('kinematics_suffix', default='') + + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + add_bio_gripper = LaunchConfiguration('add_bio_gripper', default=False) + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + add_d435i_links = LaunchConfiguration('add_d435i_links', default=True) + add_other_geometry = LaunchConfiguration('add_other_geometry', default=False) + geometry_type = LaunchConfiguration('geometry_type', default='box') + geometry_mass = LaunchConfiguration('geometry_mass', default=0.1) + geometry_height = LaunchConfiguration('geometry_height', default=0.1) + geometry_radius = LaunchConfiguration('geometry_radius', default=0.1) + geometry_length = LaunchConfiguration('geometry_length', default=0.1) + geometry_width = LaunchConfiguration('geometry_width', default=0.1) + geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='') + geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"') + geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"') + geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"') + geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"') + + no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False) + ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context) + + ros2_control_plugin = 'gazebo_ros2_control/GazeboSystem' + controllers_name = 'fake_controllers' + + 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 '')), + prefix=prefix.perform(context), + add_gripper=add_gripper.perform(context) in ('True', 'true'), + add_bio_gripper=add_bio_gripper.perform(context) in ('True', 'true'), + ros_namespace=ros_namespace, + update_rate=1000, + robot_type=robot_type.perform(context) + ) + + moveit_config = MoveItConfigsBuilder( + context=context, + controllers_name=controllers_name, + dof=dof, + robot_type=robot_type, + prefix=prefix, + hw_ns=hw_ns, + limited=limited, + effort_control=effort_control, + velocity_control=velocity_control, + model1300=model1300, + robot_sn=robot_sn, + attach_to=attach_to, + attach_xyz=attach_xyz, + attach_rpy=attach_rpy, + mesh_suffix=mesh_suffix, + kinematics_suffix=kinematics_suffix, + ros2_control_plugin=ros2_control_plugin, + ros2_control_params=ros2_control_params, + add_gripper=add_gripper, + add_vacuum_gripper=add_vacuum_gripper, + add_bio_gripper=add_bio_gripper, + add_realsense_d435i=add_realsense_d435i, + add_d435i_links=add_d435i_links, + add_other_geometry=add_other_geometry, + geometry_type=geometry_type, + geometry_mass=geometry_mass, + geometry_height=geometry_height, + geometry_radius=geometry_radius, + geometry_length=geometry_length, + geometry_width=geometry_width, + geometry_mesh_filename=geometry_mesh_filename, + geometry_mesh_origin_xyz=geometry_mesh_origin_xyz, + geometry_mesh_origin_rpy=geometry_mesh_origin_rpy, + geometry_mesh_tcp_xyz=geometry_mesh_tcp_xyz, + geometry_mesh_tcp_rpy=geometry_mesh_tcp_rpy, + + ).to_moveit_configs() + + moveit_config_dump = yaml.dump(moveit_config.to_dict()) + + # robot moveit common launch + # xarm_moveit_config/launch/_robot_moveit_common2.launch.py + robot_moveit_common_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_robot_moveit_common2.launch.py'])), + launch_arguments={ + 'prefix': prefix, + 'attach_to': attach_to, + 'attach_xyz': attach_xyz, + 'attach_rpy': attach_rpy, + 'no_gui_ctrl': no_gui_ctrl, + 'show_rviz': 'false', + 'use_sim_time': 'true', + 'moveit_config_dump': moveit_config_dump, + }.items(), + ) + + # robot gazebo launch + # xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py + robot_gazebo_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('frida_description'), 'launch', 'test_xarm_launch.launch.py'])), + launch_arguments={ + 'dof': dof, + 'robot_type': robot_type, + 'prefix': prefix, + 'moveit_config_dump': moveit_config_dump, + 'load_controller': 'true', + 'show_rviz': 'true', + 'no_gui_ctrl': no_gui_ctrl, + }.items(), + ) + + return [ + robot_gazebo_launch, + robot_moveit_common_launch, + ] + + +def generate_launch_description(): + return LaunchDescription([ + OpaqueFunction(function=launch_setup) + ]) diff --git a/robot_description/frida_description/launch/test2_xarm_move.launch.py b/robot_description/frida_description/launch/test2_xarm_move.launch.py new file mode 100644 index 0000000..667902c --- /dev/null +++ b/robot_description/frida_description/launch/test2_xarm_move.launch.py @@ -0,0 +1,283 @@ +#!/usr/bin/env python3 +# Software License Agreement (BSD License) +# +# Copyright (c) 2021, UFACTORY, Inc. +# All rights reserved. +# +# Author: Vinman + +import os +import yaml +from ament_index_python import get_package_share_directory +from launch.launch_description_sources import load_python_launch_file_as_module +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, RegisterEventHandler +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch.conditions import IfCondition +from launch_ros.substitutions import FindPackageShare +from launch.event_handlers import OnProcessExit, OnProcessStart +from launch.actions import OpaqueFunction + + +def launch_setup(context, *args, **kwargs): + prefix = LaunchConfiguration('prefix', default='') + hw_ns = LaunchConfiguration('hw_ns', default='xarm') + limited = LaunchConfiguration('limited', default=False) + effort_control = LaunchConfiguration('effort_control', default=False) + velocity_control = LaunchConfiguration('velocity_control', default=False) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + add_bio_gripper = LaunchConfiguration('add_bio_gripper', default=False) + dof = LaunchConfiguration('dof', default=6) + robot_type = LaunchConfiguration('robot_type', default='xarm') + ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='gazebo_ros2_control/GazeboSystem') + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + add_d435i_links = LaunchConfiguration('add_d435i_links', default=True) + model1300 = LaunchConfiguration('model1300', default=False) + robot_sn = LaunchConfiguration('robot_sn', default='') + attach_to = LaunchConfiguration('attach_to', default='base_link') + attach_xyz = LaunchConfiguration('attach_xyz', default='"0.0049756 0 0.34"') + attach_rpy = LaunchConfiguration('attach_rpy', default='"0 0 1.57"') + + add_other_geometry = LaunchConfiguration('add_other_geometry', default=False) + geometry_type = LaunchConfiguration('geometry_type', default='box') + geometry_mass = LaunchConfiguration('geometry_mass', default=0.1) + geometry_height = LaunchConfiguration('geometry_height', default=0.1) + geometry_radius = LaunchConfiguration('geometry_radius', default=0.1) + geometry_length = LaunchConfiguration('geometry_length', default=0.1) + geometry_width = LaunchConfiguration('geometry_width', default=0.1) + geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='') + geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"') + geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"') + geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"') + geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"') + kinematics_suffix = LaunchConfiguration('kinematics_suffix', default='') + + load_controller = LaunchConfiguration('load_controller', default=False) + show_rviz = LaunchConfiguration('show_rviz', default=False) + no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False) + + ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context) + moveit_config_dump = LaunchConfiguration('moveit_config_dump', default='') + + moveit_config_dump = moveit_config_dump.perform(context) + moveit_config_dict = yaml.load(moveit_config_dump, Loader=yaml.FullLoader) if moveit_config_dump else {} + 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 '') + + if not moveit_config_dict: + # 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(xarm_type)), + prefix=prefix.perform(context), + add_gripper=add_gripper.perform(context) in ('True', 'true'), + add_bio_gripper=add_bio_gripper.perform(context) in ('True', 'true'), + ros_namespace=ros_namespace, + update_rate=1000, + robot_type=robot_type.perform(context) + ) + # robot_description + # xarm_description/launch/lib/robot_description_lib.py + mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('xarm_description'), 'launch', 'lib', 'robot_description_lib.py')) + get_xacro_file_content = getattr(mod, 'get_xacro_file_content') + robot_description = { + 'robot_description': get_xacro_file_content( + xacro_file=PathJoinSubstitution([FindPackageShare('frida_description'), 'urdf', 'FRIDA.urdf.xacro']), + arguments={ + 'prefix': prefix, + 'dof': dof, + 'robot_type': robot_type, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'add_bio_gripper': add_bio_gripper, + 'hw_ns': hw_ns.perform(context).strip('/'), + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'ros2_control_plugin': ros2_control_plugin, + 'ros2_control_params': ros2_control_params, + 'add_realsense_d435i': add_realsense_d435i, + 'add_d435i_links': add_d435i_links, + 'model1300': model1300, + 'robot_sn': robot_sn, + 'attach_to': attach_to, + 'attach_xyz': attach_xyz, + 'attach_rpy': attach_rpy, + 'add_other_geometry': add_other_geometry, + 'geometry_type': geometry_type, + 'geometry_mass': geometry_mass, + 'geometry_height': geometry_height, + 'geometry_radius': geometry_radius, + 'geometry_length': geometry_length, + 'geometry_width': geometry_width, + 'geometry_mesh_filename': geometry_mesh_filename, + 'geometry_mesh_origin_xyz': geometry_mesh_origin_xyz, + 'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy, + 'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz, + 'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy, + 'kinematics_suffix': kinematics_suffix, + } + ), + } + moveit_config_dict = robot_description + else: + robot_description = {'robot_description': moveit_config_dict['robot_description']} + + # robot state publisher node + robot_state_publisher_node = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[{'use_sim_time': True}, robot_description], + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ] + ) + + # gazebo launch + # gazebo_ros/launch/gazebo.launch.py + xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('frida_description'), 'worlds', 'empty_classic.world']) + gazebo_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('gazebo_ros'), 'launch', 'gazebo.launch.py'])), + launch_arguments={ + 'world': xarm_gazebo_world, + 'server_required': 'true', + 'gui_required': 'true', + }.items(), + ) + + # gazebo spawn entity node + gazebo_spawn_entity_node = Node( + package="gazebo_ros", + executable="spawn_entity.py", + output='screen', + arguments=[ + '-topic', 'robot_description', + # '-entity', '{}'.format(xarm_type), + '-entity', 'UF_ROBOT', + '-x', '-0.2', + '-y', '-0.54' if robot_type.perform(context) == 'uf850' else '-0.5', + '-z', '0.2', + '-Y', '1.571', + ], + parameters=[{'use_sim_time': True}], + ) + + # rviz with moveit configuration + rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'rviz', 'planner.rviz' if no_gui_ctrl.perform(context) == 'true' else 'moveit.rviz']) + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file], + parameters=[ + { + 'robot_description': moveit_config_dict.get('robot_description', ''), + 'robot_description_semantic': moveit_config_dict.get('robot_description_semantic', ''), + 'robot_description_kinematics': moveit_config_dict.get('robot_description_kinematics', {}), + 'robot_description_planning': moveit_config_dict.get('robot_description_planning', {}), + 'planning_pipelines': moveit_config_dict.get('planning_pipelines', {}), + 'use_sim_time': True + } + ], + # condition=IfCondition(show_rviz), + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ] + ) + + # Load controllers + controllers = [ + 'joint_state_broadcaster', + '{}{}_traj_controller'.format(prefix.perform(context), xarm_type), + ] + if robot_type.perform(context) != 'lite' and add_gripper.perform(context) in ('True', 'true'): + controllers.append('{}{}_gripper_traj_controller'.format(prefix.perform(context), robot_type.perform(context))) + elif robot_type.perform(context) != 'lite' and add_bio_gripper.perform(context) in ('True', 'true'): + controllers.append('{}bio_gripper_traj_controller'.format(prefix.perform(context))) + + controller_nodes = [] + if load_controller.perform(context) in ('True', 'true'): + for controller in controllers: + controller_nodes.append(Node( + package='controller_manager', + executable='spawner', + output='screen', + arguments=[ + controller, + '--controller-manager', '{}/controller_manager'.format(ros_namespace) + ], + parameters=[{'use_sim_time': True}], + )) + + if len(controller_nodes) > 0: + return [ + RegisterEventHandler( + event_handler=OnProcessStart( + target_action=robot_state_publisher_node, + on_start=gazebo_launch, + ) + ), + RegisterEventHandler( + event_handler=OnProcessStart( + target_action=robot_state_publisher_node, + on_start=gazebo_spawn_entity_node, + ) + ), + RegisterEventHandler( + condition=IfCondition(show_rviz), + event_handler=OnProcessExit( + target_action=gazebo_spawn_entity_node, + on_exit=rviz2_node, + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=gazebo_spawn_entity_node, + on_exit=controller_nodes, + ) + ), + robot_state_publisher_node, + # gazebo_launch, + # gazebo_spawn_entity_node, + ] + else: + return [ + RegisterEventHandler( + event_handler=OnProcessStart( + target_action=robot_state_publisher_node, + on_start=gazebo_launch, + ) + ), + RegisterEventHandler( + event_handler=OnProcessStart( + target_action=robot_state_publisher_node, + on_start=gazebo_spawn_entity_node, + ) + ), + RegisterEventHandler( + condition=IfCondition(show_rviz), + event_handler=OnProcessExit( + target_action=gazebo_spawn_entity_node, + on_exit=rviz2_node, + ) + ), + robot_state_publisher_node, + # gazebo_launch, + # gazebo_spawn_entity_node, + ] + + +def generate_launch_description(): + return LaunchDescription([ + OpaqueFunction(function=launch_setup) + ]) From 87254edea94e2a7de1005030292feb6e311c5ac6 Mon Sep 17 00:00:00 2001 From: GerardoFJ Date: Tue, 4 Feb 2025 15:58:23 -0700 Subject: [PATCH 3/3] Added new urdf and simulation moveit --- .../frida_description/launch/moveit.launch.py | 2 +- .../launch/test_xarm_launch.launch.py | 283 ------------------ ...m_move.launch.py => xarm_launch.launch.py} | 0 .../frida_description/urdf/FRIDA.urdf.xacro | 4 +- .../frida_description/urdf/base.xacro | 8 +- .../frida_description/urdf/castwheel.xacro | 15 +- .../urdf/castwheelbraket.xacro | 15 +- .../frida_description/urdf/gripper.xacro | 4 + .../frida_description/urdf/realsense.xacro | 8 +- .../frida_description/urdf/robot.xacro | 2 - .../frida_description/urdf/wheels.xacro | 16 +- .../frida_description/urdf/zed.xacro | 2 +- .../worlds/empty_classic.world | 2 +- 13 files changed, 65 insertions(+), 296 deletions(-) delete mode 100644 robot_description/frida_description/launch/test_xarm_launch.launch.py rename robot_description/frida_description/launch/{test2_xarm_move.launch.py => xarm_launch.launch.py} (100%) diff --git a/robot_description/frida_description/launch/moveit.launch.py b/robot_description/frida_description/launch/moveit.launch.py index b44025f..0047ee2 100644 --- a/robot_description/frida_description/launch/moveit.launch.py +++ b/robot_description/frida_description/launch/moveit.launch.py @@ -128,7 +128,7 @@ def launch_setup(context, *args, **kwargs): # robot gazebo launch # xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py robot_gazebo_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('frida_description'), 'launch', 'test_xarm_launch.launch.py'])), + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('frida_description'), 'launch', 'xarm_launch.launch.py'])), launch_arguments={ 'dof': dof, 'robot_type': robot_type, diff --git a/robot_description/frida_description/launch/test_xarm_launch.launch.py b/robot_description/frida_description/launch/test_xarm_launch.launch.py deleted file mode 100644 index 667902c..0000000 --- a/robot_description/frida_description/launch/test_xarm_launch.launch.py +++ /dev/null @@ -1,283 +0,0 @@ -#!/usr/bin/env python3 -# Software License Agreement (BSD License) -# -# Copyright (c) 2021, UFACTORY, Inc. -# All rights reserved. -# -# Author: Vinman - -import os -import yaml -from ament_index_python import get_package_share_directory -from launch.launch_description_sources import load_python_launch_file_as_module -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, RegisterEventHandler -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node -from launch.conditions import IfCondition -from launch_ros.substitutions import FindPackageShare -from launch.event_handlers import OnProcessExit, OnProcessStart -from launch.actions import OpaqueFunction - - -def launch_setup(context, *args, **kwargs): - prefix = LaunchConfiguration('prefix', default='') - hw_ns = LaunchConfiguration('hw_ns', default='xarm') - limited = LaunchConfiguration('limited', default=False) - effort_control = LaunchConfiguration('effort_control', default=False) - velocity_control = LaunchConfiguration('velocity_control', default=False) - add_gripper = LaunchConfiguration('add_gripper', default=False) - add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) - add_bio_gripper = LaunchConfiguration('add_bio_gripper', default=False) - dof = LaunchConfiguration('dof', default=6) - robot_type = LaunchConfiguration('robot_type', default='xarm') - ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='gazebo_ros2_control/GazeboSystem') - - add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) - add_d435i_links = LaunchConfiguration('add_d435i_links', default=True) - model1300 = LaunchConfiguration('model1300', default=False) - robot_sn = LaunchConfiguration('robot_sn', default='') - attach_to = LaunchConfiguration('attach_to', default='base_link') - attach_xyz = LaunchConfiguration('attach_xyz', default='"0.0049756 0 0.34"') - attach_rpy = LaunchConfiguration('attach_rpy', default='"0 0 1.57"') - - add_other_geometry = LaunchConfiguration('add_other_geometry', default=False) - geometry_type = LaunchConfiguration('geometry_type', default='box') - geometry_mass = LaunchConfiguration('geometry_mass', default=0.1) - geometry_height = LaunchConfiguration('geometry_height', default=0.1) - geometry_radius = LaunchConfiguration('geometry_radius', default=0.1) - geometry_length = LaunchConfiguration('geometry_length', default=0.1) - geometry_width = LaunchConfiguration('geometry_width', default=0.1) - geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='') - geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"') - geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"') - geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"') - geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"') - kinematics_suffix = LaunchConfiguration('kinematics_suffix', default='') - - load_controller = LaunchConfiguration('load_controller', default=False) - show_rviz = LaunchConfiguration('show_rviz', default=False) - no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False) - - ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context) - moveit_config_dump = LaunchConfiguration('moveit_config_dump', default='') - - moveit_config_dump = moveit_config_dump.perform(context) - moveit_config_dict = yaml.load(moveit_config_dump, Loader=yaml.FullLoader) if moveit_config_dump else {} - 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 '') - - if not moveit_config_dict: - # 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(xarm_type)), - prefix=prefix.perform(context), - add_gripper=add_gripper.perform(context) in ('True', 'true'), - add_bio_gripper=add_bio_gripper.perform(context) in ('True', 'true'), - ros_namespace=ros_namespace, - update_rate=1000, - robot_type=robot_type.perform(context) - ) - # robot_description - # xarm_description/launch/lib/robot_description_lib.py - mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('xarm_description'), 'launch', 'lib', 'robot_description_lib.py')) - get_xacro_file_content = getattr(mod, 'get_xacro_file_content') - robot_description = { - 'robot_description': get_xacro_file_content( - xacro_file=PathJoinSubstitution([FindPackageShare('frida_description'), 'urdf', 'FRIDA.urdf.xacro']), - arguments={ - 'prefix': prefix, - 'dof': dof, - 'robot_type': robot_type, - 'add_gripper': add_gripper, - 'add_vacuum_gripper': add_vacuum_gripper, - 'add_bio_gripper': add_bio_gripper, - 'hw_ns': hw_ns.perform(context).strip('/'), - 'limited': limited, - 'effort_control': effort_control, - 'velocity_control': velocity_control, - 'ros2_control_plugin': ros2_control_plugin, - 'ros2_control_params': ros2_control_params, - 'add_realsense_d435i': add_realsense_d435i, - 'add_d435i_links': add_d435i_links, - 'model1300': model1300, - 'robot_sn': robot_sn, - 'attach_to': attach_to, - 'attach_xyz': attach_xyz, - 'attach_rpy': attach_rpy, - 'add_other_geometry': add_other_geometry, - 'geometry_type': geometry_type, - 'geometry_mass': geometry_mass, - 'geometry_height': geometry_height, - 'geometry_radius': geometry_radius, - 'geometry_length': geometry_length, - 'geometry_width': geometry_width, - 'geometry_mesh_filename': geometry_mesh_filename, - 'geometry_mesh_origin_xyz': geometry_mesh_origin_xyz, - 'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy, - 'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz, - 'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy, - 'kinematics_suffix': kinematics_suffix, - } - ), - } - moveit_config_dict = robot_description - else: - robot_description = {'robot_description': moveit_config_dict['robot_description']} - - # robot state publisher node - robot_state_publisher_node = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - output='screen', - parameters=[{'use_sim_time': True}, robot_description], - remappings=[ - ('/tf', 'tf'), - ('/tf_static', 'tf_static'), - ] - ) - - # gazebo launch - # gazebo_ros/launch/gazebo.launch.py - xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('frida_description'), 'worlds', 'empty_classic.world']) - gazebo_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('gazebo_ros'), 'launch', 'gazebo.launch.py'])), - launch_arguments={ - 'world': xarm_gazebo_world, - 'server_required': 'true', - 'gui_required': 'true', - }.items(), - ) - - # gazebo spawn entity node - gazebo_spawn_entity_node = Node( - package="gazebo_ros", - executable="spawn_entity.py", - output='screen', - arguments=[ - '-topic', 'robot_description', - # '-entity', '{}'.format(xarm_type), - '-entity', 'UF_ROBOT', - '-x', '-0.2', - '-y', '-0.54' if robot_type.perform(context) == 'uf850' else '-0.5', - '-z', '0.2', - '-Y', '1.571', - ], - parameters=[{'use_sim_time': True}], - ) - - # rviz with moveit configuration - rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'rviz', 'planner.rviz' if no_gui_ctrl.perform(context) == 'true' else 'moveit.rviz']) - rviz2_node = Node( - package='rviz2', - executable='rviz2', - name='rviz2', - output='screen', - arguments=['-d', rviz_config_file], - parameters=[ - { - 'robot_description': moveit_config_dict.get('robot_description', ''), - 'robot_description_semantic': moveit_config_dict.get('robot_description_semantic', ''), - 'robot_description_kinematics': moveit_config_dict.get('robot_description_kinematics', {}), - 'robot_description_planning': moveit_config_dict.get('robot_description_planning', {}), - 'planning_pipelines': moveit_config_dict.get('planning_pipelines', {}), - 'use_sim_time': True - } - ], - # condition=IfCondition(show_rviz), - remappings=[ - ('/tf', 'tf'), - ('/tf_static', 'tf_static'), - ] - ) - - # Load controllers - controllers = [ - 'joint_state_broadcaster', - '{}{}_traj_controller'.format(prefix.perform(context), xarm_type), - ] - if robot_type.perform(context) != 'lite' and add_gripper.perform(context) in ('True', 'true'): - controllers.append('{}{}_gripper_traj_controller'.format(prefix.perform(context), robot_type.perform(context))) - elif robot_type.perform(context) != 'lite' and add_bio_gripper.perform(context) in ('True', 'true'): - controllers.append('{}bio_gripper_traj_controller'.format(prefix.perform(context))) - - controller_nodes = [] - if load_controller.perform(context) in ('True', 'true'): - for controller in controllers: - controller_nodes.append(Node( - package='controller_manager', - executable='spawner', - output='screen', - arguments=[ - controller, - '--controller-manager', '{}/controller_manager'.format(ros_namespace) - ], - parameters=[{'use_sim_time': True}], - )) - - if len(controller_nodes) > 0: - return [ - RegisterEventHandler( - event_handler=OnProcessStart( - target_action=robot_state_publisher_node, - on_start=gazebo_launch, - ) - ), - RegisterEventHandler( - event_handler=OnProcessStart( - target_action=robot_state_publisher_node, - on_start=gazebo_spawn_entity_node, - ) - ), - RegisterEventHandler( - condition=IfCondition(show_rviz), - event_handler=OnProcessExit( - target_action=gazebo_spawn_entity_node, - on_exit=rviz2_node, - ) - ), - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=gazebo_spawn_entity_node, - on_exit=controller_nodes, - ) - ), - robot_state_publisher_node, - # gazebo_launch, - # gazebo_spawn_entity_node, - ] - else: - return [ - RegisterEventHandler( - event_handler=OnProcessStart( - target_action=robot_state_publisher_node, - on_start=gazebo_launch, - ) - ), - RegisterEventHandler( - event_handler=OnProcessStart( - target_action=robot_state_publisher_node, - on_start=gazebo_spawn_entity_node, - ) - ), - RegisterEventHandler( - condition=IfCondition(show_rviz), - event_handler=OnProcessExit( - target_action=gazebo_spawn_entity_node, - on_exit=rviz2_node, - ) - ), - robot_state_publisher_node, - # gazebo_launch, - # gazebo_spawn_entity_node, - ] - - -def generate_launch_description(): - return LaunchDescription([ - OpaqueFunction(function=launch_setup) - ]) diff --git a/robot_description/frida_description/launch/test2_xarm_move.launch.py b/robot_description/frida_description/launch/xarm_launch.launch.py similarity index 100% rename from robot_description/frida_description/launch/test2_xarm_move.launch.py rename to robot_description/frida_description/launch/xarm_launch.launch.py diff --git a/robot_description/frida_description/urdf/FRIDA.urdf.xacro b/robot_description/frida_description/urdf/FRIDA.urdf.xacro index 3ec2024..2bff17a 100644 --- a/robot_description/frida_description/urdf/FRIDA.urdf.xacro +++ b/robot_description/frida_description/urdf/FRIDA.urdf.xacro @@ -70,10 +70,10 @@ model1300="$(arg model1300)" attach_to="$(arg attach_to)" attach_xyz="$(arg attach_xyz)" attach_rpy="$(arg attach_rpy)" robot_sn="$(arg robot_sn)" use_gazebo_camera="$(arg use_gazebo_camera)" mesh_suffix="$(arg mesh_suffix)" kinematics_suffix="$(arg kinematics_suffix)" /> - + \ No newline at end of file diff --git a/robot_description/frida_description/urdf/base.xacro b/robot_description/frida_description/urdf/base.xacro index baf0de9..cae9670 100644 --- a/robot_description/frida_description/urdf/base.xacro +++ b/robot_description/frida_description/urdf/base.xacro @@ -24,5 +24,11 @@ - + + 200 + 200 + 1000000 + 1 + Gazebo/Grey + \ No newline at end of file diff --git a/robot_description/frida_description/urdf/castwheel.xacro b/robot_description/frida_description/urdf/castwheel.xacro index 0650cc5..77bab1d 100644 --- a/robot_description/frida_description/urdf/castwheel.xacro +++ b/robot_description/frida_description/urdf/castwheel.xacro @@ -35,7 +35,20 @@ - + + 0.1 + 0.1 + 1000000 + 1 + Gazebo/Black + + + 0.1 + 0.1 + 1000000 + 1 + Gazebo/Black + \ No newline at end of file diff --git a/robot_description/frida_description/urdf/castwheelbraket.xacro b/robot_description/frida_description/urdf/castwheelbraket.xacro index 6503833..a8abf73 100644 --- a/robot_description/frida_description/urdf/castwheelbraket.xacro +++ b/robot_description/frida_description/urdf/castwheelbraket.xacro @@ -36,6 +36,19 @@ - + + 0.1 + 0.1 + 1000000 + 1 + Gazebo/White + + + 0.1 + 0.1 + 1000000 + 1 + Gazebo/White + \ No newline at end of file diff --git a/robot_description/frida_description/urdf/gripper.xacro b/robot_description/frida_description/urdf/gripper.xacro index 326125b..f393e01 100644 --- a/robot_description/frida_description/urdf/gripper.xacro +++ b/robot_description/frida_description/urdf/gripper.xacro @@ -88,4 +88,8 @@ + + + Gazebo/DarkGrey + \ No newline at end of file diff --git a/robot_description/frida_description/urdf/realsense.xacro b/robot_description/frida_description/urdf/realsense.xacro index b6994b7..7ba17e7 100644 --- a/robot_description/frida_description/urdf/realsense.xacro +++ b/robot_description/frida_description/urdf/realsense.xacro @@ -31,5 +31,11 @@ - + + 200 + 200 + 1000000 + 1 + Gazebo/Turquoise + \ No newline at end of file diff --git a/robot_description/frida_description/urdf/robot.xacro b/robot_description/frida_description/urdf/robot.xacro index 17cf5f7..8a40108 100644 --- a/robot_description/frida_description/urdf/robot.xacro +++ b/robot_description/frida_description/urdf/robot.xacro @@ -12,7 +12,5 @@ - \ No newline at end of file diff --git a/robot_description/frida_description/urdf/wheels.xacro b/robot_description/frida_description/urdf/wheels.xacro index 727f969..74a2c62 100644 --- a/robot_description/frida_description/urdf/wheels.xacro +++ b/robot_description/frida_description/urdf/wheels.xacro @@ -35,6 +35,18 @@ - - + + 200 + 200 + 1000000 + 1 + Gazebo/Black + + + 200 + 200 + 1000000 + 1 + Gazebo/Black + \ No newline at end of file diff --git a/robot_description/frida_description/urdf/zed.xacro b/robot_description/frida_description/urdf/zed.xacro index 2f06132..3f3295a 100644 --- a/robot_description/frida_description/urdf/zed.xacro +++ b/robot_description/frida_description/urdf/zed.xacro @@ -1,7 +1,7 @@ - + diff --git a/robot_description/frida_description/worlds/empty_classic.world b/robot_description/frida_description/worlds/empty_classic.world index b9181ab..701d3ad 100755 --- a/robot_description/frida_description/worlds/empty_classic.world +++ b/robot_description/frida_description/worlds/empty_classic.world @@ -13,7 +13,7 @@ 1 1000 - 0 0 0 + 0 0 -9.81