diff --git a/Dockerfile b/Dockerfile index 6af8d6a..3db93c2 100644 --- a/Dockerfile +++ b/Dockerfile @@ -64,12 +64,15 @@ RUN source "/opt/ros/${ROS_DISTRO}/setup.bash" && \ # Build lite6 and xarm packages COPY ./src/lite6_controller ${WS_SRC_DIR}/lite6_controller +COPY ./src/custom_xarm_description ${WS_SRC_DIR}/custom_xarm_description +COPY ./src/custom_xarm_moveit_config ${WS_SRC_DIR}/custom_xarm_moveit_config +COPY ./src/custom_xarm_gazebo ${WS_SRC_DIR}/custom_xarm_gazebo RUN apt-get update RUN source "/opt/ros/${ROS_DISTRO}/setup.bash" && \ vcs import --recursive --shallow ${WS_SRC_DIR} < ${WS_SRC_DIR}/lite6_controller/lite6_controller.repos && \ mv ${WS_SRC_DIR}/xarm_ros2/xarm* ${WS_SRC_DIR} && \ rosdep install -y -r -i --rosdistro "${ROS_DISTRO}" --from-paths ${WS_SRC_DIR}/xarm_* && \ - colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" --paths ${WS_SRC_DIR}/xarm_* ${WS_SRC_DIR}/lite6_controller && \ + colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" --paths ${WS_SRC_DIR}/xarm_* ${WS_SRC_DIR}/lite6_controller ${WS_SRC_DIR}/custom_xarm_description ${WS_SRC_DIR}/custom_xarm_moveit_config ${WS_SRC_DIR}/custom_xarm_gazebo && \ rm -rf ${WS_LOG_DIR} # Copy example svg images diff --git a/src/custom_xarm_description/CMakeLists.txt b/src/custom_xarm_description/CMakeLists.txt new file mode 100644 index 0000000..7a8799f --- /dev/null +++ b/src/custom_xarm_description/CMakeLists.txt @@ -0,0 +1,43 @@ +cmake_minimum_required(VERSION 3.5) +project(custom_xarm_description) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +install(DIRECTORY + rviz + urdf + meshes + launch + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/src/custom_xarm_description/launch/_robot_description.launch.py b/src/custom_xarm_description/launch/_robot_description.launch.py new file mode 100644 index 0000000..01e5b5b --- /dev/null +++ b/src/custom_xarm_description/launch/_robot_description.launch.py @@ -0,0 +1,104 @@ +#!/usr/bin/env python3 +# Software License Agreement (BSD License) +# +# Copyright (c) 2021, UFACTORY, Inc. +# All rights reserved. +# +# Author: Vinman + +import os +import sys +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 OpaqueFunction, DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +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) + dof = LaunchConfiguration('dof', default=7) + robot_type = LaunchConfiguration('robot_type', default='xarm') + ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='uf_robot_hardware/UFRobotSystemHardware') + joint_states_remapping = LaunchConfiguration('joint_states_remapping', default='joint_states') + xacro_file = LaunchConfiguration('xacro_file', default=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro'])) + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # 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=xacro_file, + arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns.perform(context).strip('/'), + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': dof, + 'robot_type': robot_type, + 'ros2_control_plugin': ros2_control_plugin, + 'add_realsense_d435i': add_realsense_d435i, + '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, + } + ) + } + + # robot state publisher node + robot_state_publisher_node = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[robot_description], + remappings=[ + # ('joint_states', joint_states_remapping), + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ] + ) + return [ + robot_state_publisher_node + ] + + +def generate_launch_description(): + return LaunchDescription([ + OpaqueFunction(function=launch_setup) + ]) diff --git a/src/custom_xarm_description/launch/_robot_joint_state.launch.py b/src/custom_xarm_description/launch/_robot_joint_state.launch.py new file mode 100644 index 0000000..0b36f48 --- /dev/null +++ b/src/custom_xarm_description/launch/_robot_joint_state.launch.py @@ -0,0 +1,95 @@ +#!/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.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +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) + dof = LaunchConfiguration('dof', default=7) + robot_type = LaunchConfiguration('robot_type', default='xarm') + ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='uf_robot_hardware/UFRobotSystemHardware') + joint_states_remapping = LaunchConfiguration('joint_states_remapping', default='joint_states') + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # 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={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': dof, + 'robot_type': robot_type, + 'ros2_control_plugin': ros2_control_plugin, + 'joint_states_remapping': joint_states_remapping, + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + # joint state publisher node + joint_state_publisher_node = Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + output='screen', + parameters=[{'source_list': ['{}{}/joint_states'.format(prefix.perform(context), hw_ns.perform(context))]}], + ) + + return [ + robot_description_launch, + joint_state_publisher_node, + ] + + +def generate_launch_description(): + return LaunchDescription([ + OpaqueFunction(function=launch_setup) + ]) diff --git a/src/custom_xarm_description/launch/_robot_rviz_display.launch.py b/src/custom_xarm_description/launch/_robot_rviz_display.launch.py new file mode 100644 index 0000000..92d5c1d --- /dev/null +++ b/src/custom_xarm_description/launch/_robot_rviz_display.launch.py @@ -0,0 +1,81 @@ +#!/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 IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +def generate_launch_description(): + 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) + dof = LaunchConfiguration('dof', default=7) + robot_type = LaunchConfiguration('robot_type', default='xarm') + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # robot joint state launch + # xarm_description/launch/_robot_joint_state.launch.py + robot_joint_state_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/_robot_joint_state.launch.py']), + launch_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': dof, + 'robot_type': robot_type, + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + # rviz2 display launch + # xarm_description/launch/_rviz_display.launch.py + rviz2_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/_rviz_display.launch.py']), + ) + + return LaunchDescription([ + robot_joint_state_launch, + rviz2_launch + ]) diff --git a/src/custom_xarm_description/launch/_rviz_display.launch.py b/src/custom_xarm_description/launch/_rviz_display.launch.py new file mode 100644 index 0000000..424010d --- /dev/null +++ b/src/custom_xarm_description/launch/_rviz_display.launch.py @@ -0,0 +1,39 @@ +#!/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.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.actions import RegisterEventHandler, EmitEvent +from launch.event_handlers import OnProcessExit +from launch.events import Shutdown + + +def generate_launch_description(): + # rviz2 node + rviz2_params = PathJoinSubstitution([FindPackageShare('xarm_description'), 'rviz', 'display.rviz']) + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz2_params], + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ] + ) + + return LaunchDescription([ + RegisterEventHandler(event_handler=OnProcessExit( + target_action=rviz2_node, + on_exit=[EmitEvent(event=Shutdown())] + )), + rviz2_node, + ]) diff --git a/src/custom_xarm_description/launch/lib/robot_description_lib.py b/src/custom_xarm_description/launch/lib/robot_description_lib.py new file mode 100644 index 0000000..e6163f0 --- /dev/null +++ b/src/custom_xarm_description/launch/lib/robot_description_lib.py @@ -0,0 +1,29 @@ +#!/usr/bin/env python3 +# Software License Agreement (BSD License) +# +# Copyright (c) 2021, UFACTORY, Inc. +# All rights reserved. +# +# Author: Vinman + +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def get_xacro_file_content( + xacro_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']), + arguments={}): + command = [ + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', + xacro_file, + ' ' + ] + if arguments and isinstance(arguments, dict): + for key, val in arguments.items(): + command.extend([ + '{}:='.format(key), + val, + ' ' + ]) + return Command(command) diff --git a/src/custom_xarm_description/launch/lite6_rviz_display.launch.py b/src/custom_xarm_description/launch/lite6_rviz_display.launch.py new file mode 100644 index 0000000..07d839f --- /dev/null +++ b/src/custom_xarm_description/launch/lite6_rviz_display.launch.py @@ -0,0 +1,42 @@ +#!/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 IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir + +def generate_launch_description(): + prefix = LaunchConfiguration('prefix', default='') + hw_ns = LaunchConfiguration('hw_ns', default='ufactory') + 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) + + # robot rviz launch + # xarm_description/launch/_robot_rviz_display.launch.py + robot_rviz_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/_robot_rviz_display.launch.py']), + launch_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '6', + 'robot_type': 'lite', + }.items(), + ) + + return LaunchDescription([ + robot_rviz_launch + ]) diff --git a/src/custom_xarm_description/launch/xarm5_rviz_display.launch.py b/src/custom_xarm_description/launch/xarm5_rviz_display.launch.py new file mode 100644 index 0000000..5465d16 --- /dev/null +++ b/src/custom_xarm_description/launch/xarm5_rviz_display.launch.py @@ -0,0 +1,42 @@ +#!/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 IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir + +def generate_launch_description(): + 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) + + # robot rviz launch + # xarm_description/launch/_robot_rviz_display.launch.py + robot_rviz_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/_robot_rviz_display.launch.py']), + launch_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '5', + 'robot_type': 'xarm', + }.items(), + ) + + return LaunchDescription([ + robot_rviz_launch + ]) diff --git a/src/custom_xarm_description/launch/xarm6_rviz_display.launch.py b/src/custom_xarm_description/launch/xarm6_rviz_display.launch.py new file mode 100644 index 0000000..1db6cf7 --- /dev/null +++ b/src/custom_xarm_description/launch/xarm6_rviz_display.launch.py @@ -0,0 +1,42 @@ +#!/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 IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir + +def generate_launch_description(): + 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) + + # robot rviz launch + # xarm_description/launch/_robot_rviz_display.launch.py + robot_rviz_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/_robot_rviz_display.launch.py']), + launch_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '6', + 'robot_type': 'xarm', + }.items(), + ) + + return LaunchDescription([ + robot_rviz_launch + ]) diff --git a/src/custom_xarm_description/launch/xarm7_rviz_display.launch.py b/src/custom_xarm_description/launch/xarm7_rviz_display.launch.py new file mode 100644 index 0000000..5f04a7e --- /dev/null +++ b/src/custom_xarm_description/launch/xarm7_rviz_display.launch.py @@ -0,0 +1,42 @@ +#!/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 IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir + +def generate_launch_description(): + 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) + + # robot rviz launch + # xarm_description/launch/_robot_rviz_display.launch.py + robot_rviz_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/_robot_rviz_display.launch.py']), + launch_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '7', + 'robot_type': 'xarm', + }.items(), + ) + + return LaunchDescription([ + robot_rviz_launch + ]) diff --git a/src/custom_xarm_description/meshes/camera/realsense/collision/d435_with_cam_stand.STL b/src/custom_xarm_description/meshes/camera/realsense/collision/d435_with_cam_stand.STL new file mode 100755 index 0000000..16b3894 Binary files /dev/null and b/src/custom_xarm_description/meshes/camera/realsense/collision/d435_with_cam_stand.STL differ diff --git a/src/custom_xarm_description/meshes/camera/realsense/visual/d435_with_cam_stand.STL b/src/custom_xarm_description/meshes/camera/realsense/visual/d435_with_cam_stand.STL new file mode 100755 index 0000000..4707cae Binary files /dev/null and b/src/custom_xarm_description/meshes/camera/realsense/visual/d435_with_cam_stand.STL differ diff --git a/src/custom_xarm_description/meshes/gripper/base_link.STL b/src/custom_xarm_description/meshes/gripper/base_link.STL new file mode 100755 index 0000000..6a3263b Binary files /dev/null and b/src/custom_xarm_description/meshes/gripper/base_link.STL differ diff --git a/src/custom_xarm_description/meshes/gripper/left_finger.STL b/src/custom_xarm_description/meshes/gripper/left_finger.STL new file mode 100755 index 0000000..41f2c0d Binary files /dev/null and b/src/custom_xarm_description/meshes/gripper/left_finger.STL differ diff --git a/src/custom_xarm_description/meshes/gripper/left_inner_knuckle.STL b/src/custom_xarm_description/meshes/gripper/left_inner_knuckle.STL new file mode 100755 index 0000000..abeff0c Binary files /dev/null and b/src/custom_xarm_description/meshes/gripper/left_inner_knuckle.STL differ diff --git a/src/custom_xarm_description/meshes/gripper/left_outer_knuckle.STL b/src/custom_xarm_description/meshes/gripper/left_outer_knuckle.STL new file mode 100755 index 0000000..639add9 Binary files /dev/null and b/src/custom_xarm_description/meshes/gripper/left_outer_knuckle.STL differ diff --git a/src/custom_xarm_description/meshes/gripper/right_finger.STL b/src/custom_xarm_description/meshes/gripper/right_finger.STL new file mode 100755 index 0000000..53f0eec Binary files /dev/null and b/src/custom_xarm_description/meshes/gripper/right_finger.STL differ diff --git a/src/custom_xarm_description/meshes/gripper/right_inner_knuckle.STL b/src/custom_xarm_description/meshes/gripper/right_inner_knuckle.STL new file mode 100755 index 0000000..f1a486d Binary files /dev/null and b/src/custom_xarm_description/meshes/gripper/right_inner_knuckle.STL differ diff --git a/src/custom_xarm_description/meshes/gripper/right_outer_knuckle.STL b/src/custom_xarm_description/meshes/gripper/right_outer_knuckle.STL new file mode 100755 index 0000000..3099dfd Binary files /dev/null and b/src/custom_xarm_description/meshes/gripper/right_outer_knuckle.STL differ diff --git a/src/custom_xarm_description/meshes/lite6/collision/base.stl b/src/custom_xarm_description/meshes/lite6/collision/base.stl new file mode 100644 index 0000000..57802b3 Binary files /dev/null and b/src/custom_xarm_description/meshes/lite6/collision/base.stl differ diff --git a/src/custom_xarm_description/meshes/lite6/collision/gripper_lite.stl b/src/custom_xarm_description/meshes/lite6/collision/gripper_lite.stl new file mode 100644 index 0000000..9b27197 Binary files /dev/null and b/src/custom_xarm_description/meshes/lite6/collision/gripper_lite.stl differ diff --git a/src/custom_xarm_description/meshes/lite6/collision/link1.stl b/src/custom_xarm_description/meshes/lite6/collision/link1.stl new file mode 100644 index 0000000..7f1a1b4 Binary files /dev/null and b/src/custom_xarm_description/meshes/lite6/collision/link1.stl differ diff --git a/src/custom_xarm_description/meshes/lite6/collision/link2.stl b/src/custom_xarm_description/meshes/lite6/collision/link2.stl new file mode 100644 index 0000000..d3451b7 Binary files /dev/null and b/src/custom_xarm_description/meshes/lite6/collision/link2.stl differ diff --git a/src/custom_xarm_description/meshes/lite6/collision/link3.stl b/src/custom_xarm_description/meshes/lite6/collision/link3.stl new file mode 100644 index 0000000..2d9bf5a Binary files /dev/null and b/src/custom_xarm_description/meshes/lite6/collision/link3.stl differ diff --git a/src/custom_xarm_description/meshes/lite6/collision/link4.stl b/src/custom_xarm_description/meshes/lite6/collision/link4.stl new file mode 100644 index 0000000..9a93c31 Binary files /dev/null and b/src/custom_xarm_description/meshes/lite6/collision/link4.stl differ diff --git a/src/custom_xarm_description/meshes/lite6/collision/link5.stl b/src/custom_xarm_description/meshes/lite6/collision/link5.stl new file mode 100644 index 0000000..ef7c2db Binary files /dev/null and b/src/custom_xarm_description/meshes/lite6/collision/link5.stl differ diff --git a/src/custom_xarm_description/meshes/lite6/collision/link6.stl b/src/custom_xarm_description/meshes/lite6/collision/link6.stl new file mode 100644 index 0000000..d2edc78 Binary files /dev/null and b/src/custom_xarm_description/meshes/lite6/collision/link6.stl differ diff --git a/src/custom_xarm_description/meshes/lite6/collision/vacuum_gripper_lite.stl b/src/custom_xarm_description/meshes/lite6/collision/vacuum_gripper_lite.stl new file mode 100644 index 0000000..5b39f58 Binary files /dev/null and b/src/custom_xarm_description/meshes/lite6/collision/vacuum_gripper_lite.stl differ diff --git a/src/custom_xarm_description/meshes/lite6/visual/base.stl b/src/custom_xarm_description/meshes/lite6/visual/base.stl new file mode 100755 index 0000000..56cf92d Binary files /dev/null and b/src/custom_xarm_description/meshes/lite6/visual/base.stl differ diff --git a/src/custom_xarm_description/meshes/lite6/visual/gripper_lite.stl b/src/custom_xarm_description/meshes/lite6/visual/gripper_lite.stl new file mode 100644 index 0000000..84d3e5d Binary files /dev/null and b/src/custom_xarm_description/meshes/lite6/visual/gripper_lite.stl differ diff --git a/src/custom_xarm_description/meshes/lite6/visual/link1.stl b/src/custom_xarm_description/meshes/lite6/visual/link1.stl new file mode 100755 index 0000000..c846dbb Binary files /dev/null and b/src/custom_xarm_description/meshes/lite6/visual/link1.stl differ diff --git a/src/custom_xarm_description/meshes/lite6/visual/link2.stl b/src/custom_xarm_description/meshes/lite6/visual/link2.stl new file mode 100755 index 0000000..d1b1f83 Binary files /dev/null and b/src/custom_xarm_description/meshes/lite6/visual/link2.stl differ diff --git a/src/custom_xarm_description/meshes/lite6/visual/link3.stl b/src/custom_xarm_description/meshes/lite6/visual/link3.stl new file mode 100755 index 0000000..e2833e7 Binary files /dev/null and b/src/custom_xarm_description/meshes/lite6/visual/link3.stl differ diff --git a/src/custom_xarm_description/meshes/lite6/visual/link4.stl b/src/custom_xarm_description/meshes/lite6/visual/link4.stl new file mode 100755 index 0000000..8eec2c4 Binary files /dev/null and b/src/custom_xarm_description/meshes/lite6/visual/link4.stl differ diff --git a/src/custom_xarm_description/meshes/lite6/visual/link5.stl b/src/custom_xarm_description/meshes/lite6/visual/link5.stl new file mode 100755 index 0000000..2c46563 Binary files /dev/null and b/src/custom_xarm_description/meshes/lite6/visual/link5.stl differ diff --git a/src/custom_xarm_description/meshes/lite6/visual/link6.stl b/src/custom_xarm_description/meshes/lite6/visual/link6.stl new file mode 100755 index 0000000..60a915e Binary files /dev/null and b/src/custom_xarm_description/meshes/lite6/visual/link6.stl differ diff --git a/src/custom_xarm_description/meshes/lite6/visual/vacuum_gripper_lite.stl b/src/custom_xarm_description/meshes/lite6/visual/vacuum_gripper_lite.stl new file mode 100644 index 0000000..6b55e2c Binary files /dev/null and b/src/custom_xarm_description/meshes/lite6/visual/vacuum_gripper_lite.stl differ diff --git a/src/custom_xarm_description/meshes/vacuum_gripper/collision/vacuum_gripper.STL b/src/custom_xarm_description/meshes/vacuum_gripper/collision/vacuum_gripper.STL new file mode 100644 index 0000000..05e9a46 Binary files /dev/null and b/src/custom_xarm_description/meshes/vacuum_gripper/collision/vacuum_gripper.STL differ diff --git a/src/custom_xarm_description/meshes/vacuum_gripper/visual/vacuum_gripper.STL b/src/custom_xarm_description/meshes/vacuum_gripper/visual/vacuum_gripper.STL new file mode 100644 index 0000000..ae84102 Binary files /dev/null and b/src/custom_xarm_description/meshes/vacuum_gripper/visual/vacuum_gripper.STL differ diff --git a/src/custom_xarm_description/meshes/xarm5/visual/base_link.STL b/src/custom_xarm_description/meshes/xarm5/visual/base_link.STL new file mode 100755 index 0000000..6cb3f5f Binary files /dev/null and b/src/custom_xarm_description/meshes/xarm5/visual/base_link.STL differ diff --git a/src/custom_xarm_description/meshes/xarm5/visual/link1.STL b/src/custom_xarm_description/meshes/xarm5/visual/link1.STL new file mode 100755 index 0000000..6d56e2e Binary files /dev/null and b/src/custom_xarm_description/meshes/xarm5/visual/link1.STL differ diff --git a/src/custom_xarm_description/meshes/xarm5/visual/link2.STL b/src/custom_xarm_description/meshes/xarm5/visual/link2.STL new file mode 100755 index 0000000..04c851e Binary files /dev/null and b/src/custom_xarm_description/meshes/xarm5/visual/link2.STL differ diff --git a/src/custom_xarm_description/meshes/xarm5/visual/link3.STL b/src/custom_xarm_description/meshes/xarm5/visual/link3.STL new file mode 100755 index 0000000..9256f09 Binary files /dev/null and b/src/custom_xarm_description/meshes/xarm5/visual/link3.STL differ diff --git a/src/custom_xarm_description/meshes/xarm5/visual/link4.STL b/src/custom_xarm_description/meshes/xarm5/visual/link4.STL new file mode 100755 index 0000000..9951b53 Binary files /dev/null and b/src/custom_xarm_description/meshes/xarm5/visual/link4.STL differ diff --git a/src/custom_xarm_description/meshes/xarm5/visual/link5.STL b/src/custom_xarm_description/meshes/xarm5/visual/link5.STL new file mode 100755 index 0000000..80f0fdc Binary files /dev/null and b/src/custom_xarm_description/meshes/xarm5/visual/link5.STL differ diff --git a/src/custom_xarm_description/meshes/xarm6/visual/base.stl b/src/custom_xarm_description/meshes/xarm6/visual/base.stl new file mode 100755 index 0000000..1b2c31c Binary files /dev/null and b/src/custom_xarm_description/meshes/xarm6/visual/base.stl differ diff --git a/src/custom_xarm_description/meshes/xarm6/visual/link1.stl b/src/custom_xarm_description/meshes/xarm6/visual/link1.stl new file mode 100755 index 0000000..431d81a Binary files /dev/null and b/src/custom_xarm_description/meshes/xarm6/visual/link1.stl differ diff --git a/src/custom_xarm_description/meshes/xarm6/visual/link2.stl b/src/custom_xarm_description/meshes/xarm6/visual/link2.stl new file mode 100755 index 0000000..f040cfd Binary files /dev/null and b/src/custom_xarm_description/meshes/xarm6/visual/link2.stl differ diff --git a/src/custom_xarm_description/meshes/xarm6/visual/link3.stl b/src/custom_xarm_description/meshes/xarm6/visual/link3.stl new file mode 100755 index 0000000..1e53f10 Binary files /dev/null and b/src/custom_xarm_description/meshes/xarm6/visual/link3.stl differ diff --git a/src/custom_xarm_description/meshes/xarm6/visual/link4.stl b/src/custom_xarm_description/meshes/xarm6/visual/link4.stl new file mode 100755 index 0000000..a1e6017 Binary files /dev/null and b/src/custom_xarm_description/meshes/xarm6/visual/link4.stl differ diff --git a/src/custom_xarm_description/meshes/xarm6/visual/link5.stl b/src/custom_xarm_description/meshes/xarm6/visual/link5.stl new file mode 100755 index 0000000..510b410 Binary files /dev/null and b/src/custom_xarm_description/meshes/xarm6/visual/link5.stl differ diff --git a/src/custom_xarm_description/meshes/xarm6/visual/link6.stl b/src/custom_xarm_description/meshes/xarm6/visual/link6.stl new file mode 100755 index 0000000..80f0fdc Binary files /dev/null and b/src/custom_xarm_description/meshes/xarm6/visual/link6.stl differ diff --git a/src/custom_xarm_description/meshes/xarm7/visual/link1.STL b/src/custom_xarm_description/meshes/xarm7/visual/link1.STL new file mode 100755 index 0000000..a5ca309 Binary files /dev/null and b/src/custom_xarm_description/meshes/xarm7/visual/link1.STL differ diff --git a/src/custom_xarm_description/meshes/xarm7/visual/link2.STL b/src/custom_xarm_description/meshes/xarm7/visual/link2.STL new file mode 100755 index 0000000..1539adb Binary files /dev/null and b/src/custom_xarm_description/meshes/xarm7/visual/link2.STL differ diff --git a/src/custom_xarm_description/meshes/xarm7/visual/link3.STL b/src/custom_xarm_description/meshes/xarm7/visual/link3.STL new file mode 100755 index 0000000..2392635 Binary files /dev/null and b/src/custom_xarm_description/meshes/xarm7/visual/link3.STL differ diff --git a/src/custom_xarm_description/meshes/xarm7/visual/link4.STL b/src/custom_xarm_description/meshes/xarm7/visual/link4.STL new file mode 100755 index 0000000..a24cc91 Binary files /dev/null and b/src/custom_xarm_description/meshes/xarm7/visual/link4.STL differ diff --git a/src/custom_xarm_description/meshes/xarm7/visual/link5.STL b/src/custom_xarm_description/meshes/xarm7/visual/link5.STL new file mode 100755 index 0000000..7f91e37 Binary files /dev/null and b/src/custom_xarm_description/meshes/xarm7/visual/link5.STL differ diff --git a/src/custom_xarm_description/meshes/xarm7/visual/link6.STL b/src/custom_xarm_description/meshes/xarm7/visual/link6.STL new file mode 100755 index 0000000..04fc480 Binary files /dev/null and b/src/custom_xarm_description/meshes/xarm7/visual/link6.STL differ diff --git a/src/custom_xarm_description/meshes/xarm7/visual/link7.STL b/src/custom_xarm_description/meshes/xarm7/visual/link7.STL new file mode 100755 index 0000000..0e9963c Binary files /dev/null and b/src/custom_xarm_description/meshes/xarm7/visual/link7.STL differ diff --git a/src/custom_xarm_description/meshes/xarm7/visual/link_base.STL b/src/custom_xarm_description/meshes/xarm7/visual/link_base.STL new file mode 100755 index 0000000..555d6f2 Binary files /dev/null and b/src/custom_xarm_description/meshes/xarm7/visual/link_base.STL differ diff --git a/src/custom_xarm_description/package.xml b/src/custom_xarm_description/package.xml new file mode 100644 index 0000000..5c21b33 --- /dev/null +++ b/src/custom_xarm_description/package.xml @@ -0,0 +1,26 @@ + + + + custom_xarm_description + 0.0.0 + TODO: Package description + + TODO: License declaration + + ament_cmake + + joint_state_publisher + robot_state_publisher + rviz2 + xacro + urdf + launch + launch_ros + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/custom_xarm_description/rviz/display.rviz b/src/custom_xarm_description/rviz/display.rviz new file mode 100644 index 0000000..7a7c733 --- /dev/null +++ b/src/custom_xarm_description/rviz/display.rviz @@ -0,0 +1,195 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + Splitter Ratio: 0.5 + Tree Height: 617 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_eef: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 3.1222100257873535 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6253979802131653 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.18356990814209 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 60 + Y: 60 diff --git a/src/custom_xarm_description/urdf/camera/realsense.gazebo.xacro b/src/custom_xarm_description/urdf/camera/realsense.gazebo.xacro new file mode 100644 index 0000000..0829f52 --- /dev/null +++ b/src/custom_xarm_description/urdf/camera/realsense.gazebo.xacro @@ -0,0 +1,127 @@ + + + + + + + + 1.57 + + 1280 + 720 + + + 0.1 + 100 + + + gaussian + 0.0 + 0.100 + + + 1 + 30 + 0 + + + + + + 1.57 + + 1280 + 720 + RGB_INT8 + + + 0.1 + 100 + + + gaussian + 0.0 + 0.007 + + + 1 + 30 + 1 + + + + + + 1.57 + + 1280 + 720 + L_INT8 + + + 0.1 + 100 + + + gaussian + 0.0 + 0.05 + + + 1 + 1 + 0 + + + + + + 1.57 + + 1280 + 720 + L_INT8 + + + 0.1 + 100 + + + gaussian + 0.0 + 0.05 + + + 1 + 1 + 0 + + + + + camera + 30.0 + 30.0 + 1.0 + aligned_depth_to_color/image_raw + depth/camera_info + color/image_raw + color/camera_info + infra1/image_raw + infra1/camera_info + infra2/image_raw + infra2/camera_info + camera_color_optical_frame + camera_depth_optical_frame + camera_left_ir_optical_frame + camera_right_ir_optical_frame + 0.3 + 3.0 + true + depth/color/points + 0.3 + + + + + \ No newline at end of file diff --git a/src/custom_xarm_description/urdf/camera/realsense_d435i.urdf.xacro b/src/custom_xarm_description/urdf/camera/realsense_d435i.urdf.xacro new file mode 100644 index 0000000..bc9a7ef --- /dev/null +++ b/src/custom_xarm_description/urdf/camera/realsense_d435i.urdf.xacro @@ -0,0 +1,95 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/custom_xarm_description/urdf/common/common.gazebo.xacro b/src/custom_xarm_description/urdf/common/common.gazebo.xacro new file mode 100755 index 0000000..68f9b9e --- /dev/null +++ b/src/custom_xarm_description/urdf/common/common.gazebo.xacro @@ -0,0 +1,17 @@ + + + + + + + + gazebo_ros2_control/GazeboSystem + + ${ros2_control_params} + + + + + + + diff --git a/src/custom_xarm_description/urdf/dual_xarm_device.urdf.xacro b/src/custom_xarm_description/urdf/dual_xarm_device.urdf.xacro new file mode 100755 index 0000000..425c36e --- /dev/null +++ b/src/custom_xarm_description/urdf/dual_xarm_device.urdf.xacro @@ -0,0 +1,102 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/custom_xarm_description/urdf/gripper/lite_gripper.urdf.xacro b/src/custom_xarm_description/urdf/gripper/lite_gripper.urdf.xacro new file mode 100644 index 0000000..c1eeb13 --- /dev/null +++ b/src/custom_xarm_description/urdf/gripper/lite_gripper.urdf.xacro @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/custom_xarm_description/urdf/gripper/xarm_gripper.gazebo.xacro b/src/custom_xarm_description/urdf/gripper/xarm_gripper.gazebo.xacro new file mode 100644 index 0000000..04251ce --- /dev/null +++ b/src/custom_xarm_description/urdf/gripper/xarm_gripper.gazebo.xacro @@ -0,0 +1,53 @@ + + + + + + + + ${following_joint} + ${mimic_joint} + + + + ${multiplier} + ${offset} + ${sensitiveness} + ${max_effort} + + + + + + + + false + + + + false + + + + false + + + + false + + + + false + + + + false + + + + false + + + + + \ No newline at end of file diff --git a/src/custom_xarm_description/urdf/gripper/xarm_gripper.ros2_control.xacro b/src/custom_xarm_description/urdf/gripper/xarm_gripper.ros2_control.xacro new file mode 100644 index 0000000..761e72d --- /dev/null +++ b/src/custom_xarm_description/urdf/gripper/xarm_gripper.ros2_control.xacro @@ -0,0 +1,23 @@ + + + + + + ${ros2_control_plugin} + + + + + + -3.14 + 3.14 + + + + + + + + \ No newline at end of file diff --git a/src/custom_xarm_description/urdf/gripper/xarm_gripper.transmission.xacro b/src/custom_xarm_description/urdf/gripper/xarm_gripper.transmission.xacro new file mode 100644 index 0000000..4c8e417 --- /dev/null +++ b/src/custom_xarm_description/urdf/gripper/xarm_gripper.transmission.xacro @@ -0,0 +1,18 @@ + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + \ No newline at end of file diff --git a/src/custom_xarm_description/urdf/gripper/xarm_gripper.urdf.xacro b/src/custom_xarm_description/urdf/gripper/xarm_gripper.urdf.xacro new file mode 100755 index 0000000..55dcf7f --- /dev/null +++ b/src/custom_xarm_description/urdf/gripper/xarm_gripper.urdf.xacro @@ -0,0 +1,415 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/custom_xarm_description/urdf/gripper/xarm_gripper_macro.xacro b/src/custom_xarm_description/urdf/gripper/xarm_gripper_macro.xacro new file mode 100644 index 0000000..325336b --- /dev/null +++ b/src/custom_xarm_description/urdf/gripper/xarm_gripper_macro.xacro @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/custom_xarm_description/urdf/lite6/lite6.gazebo.xacro b/src/custom_xarm_description/urdf/lite6/lite6.gazebo.xacro new file mode 100755 index 0000000..cc416f9 --- /dev/null +++ b/src/custom_xarm_description/urdf/lite6/lite6.gazebo.xacro @@ -0,0 +1,36 @@ + + + + + + + true + + + + true + + + + true + + + + true + + + + true + + + + true + + + + true + + + + + diff --git a/src/custom_xarm_description/urdf/lite6/lite6.ros2_control.xacro b/src/custom_xarm_description/urdf/lite6/lite6.ros2_control.xacro new file mode 100755 index 0000000..a604ddc --- /dev/null +++ b/src/custom_xarm_description/urdf/lite6/lite6.ros2_control.xacro @@ -0,0 +1,110 @@ + + + + + + ${ros2_control_plugin} + + ${prefix}${hw_ns} + ${velocity_control} + P${prefix} + R${robot_ip} + ${report_type} + 6 + ${baud_checkset} + ${default_gripper_baud} + lite + ${add_gripper} + + + + + ${joint1_lower_limit} + ${joint1_upper_limit} + + + -3.14 + 3.14 + + + + + + + + ${joint2_lower_limit} + ${joint2_upper_limit} + + + -3.14 + 3.14 + + + + + + + + ${joint3_lower_limit} + ${joint3_upper_limit} + + + -3.14 + 3.14 + + + + + + + + ${joint4_lower_limit} + ${joint4_upper_limit} + + + -3.14 + 3.14 + + + + + + + + ${joint5_lower_limit} + ${joint5_upper_limit} + + + -3.14 + 3.14 + + + + + + + + ${joint6_lower_limit} + ${joint6_upper_limit} + + + -3.14 + 3.14 + + + + + + + + \ No newline at end of file diff --git a/src/custom_xarm_description/urdf/lite6/lite6.transmission.xacro b/src/custom_xarm_description/urdf/lite6/lite6.transmission.xacro new file mode 100755 index 0000000..b443d4d --- /dev/null +++ b/src/custom_xarm_description/urdf/lite6/lite6.transmission.xacro @@ -0,0 +1,75 @@ + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + + diff --git a/src/custom_xarm_description/urdf/lite6/lite6.urdf.xacro b/src/custom_xarm_description/urdf/lite6/lite6.urdf.xacro new file mode 100755 index 0000000..f0682a3 --- /dev/null +++ b/src/custom_xarm_description/urdf/lite6/lite6.urdf.xacro @@ -0,0 +1,367 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/custom_xarm_description/urdf/lite6/lite6_robot_macro.xacro b/src/custom_xarm_description/urdf/lite6/lite6_robot_macro.xacro new file mode 100755 index 0000000..aaa0860 --- /dev/null +++ b/src/custom_xarm_description/urdf/lite6/lite6_robot_macro.xacro @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/custom_xarm_description/urdf/other/other_geometry.urdf.xacro b/src/custom_xarm_description/urdf/other/other_geometry.urdf.xacro new file mode 100644 index 0000000..3b55460 --- /dev/null +++ b/src/custom_xarm_description/urdf/other/other_geometry.urdf.xacro @@ -0,0 +1,122 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/custom_xarm_description/urdf/vacuum_gripper/lite_vacuum_gripper.urdf.xacro b/src/custom_xarm_description/urdf/vacuum_gripper/lite_vacuum_gripper.urdf.xacro new file mode 100644 index 0000000..9dea9e6 --- /dev/null +++ b/src/custom_xarm_description/urdf/vacuum_gripper/lite_vacuum_gripper.urdf.xacro @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/custom_xarm_description/urdf/vacuum_gripper/xarm_vacuum_gripper.urdf.xacro b/src/custom_xarm_description/urdf/vacuum_gripper/xarm_vacuum_gripper.urdf.xacro new file mode 100644 index 0000000..299ca36 --- /dev/null +++ b/src/custom_xarm_description/urdf/vacuum_gripper/xarm_vacuum_gripper.urdf.xacro @@ -0,0 +1,69 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/custom_xarm_description/urdf/xarm5/xarm5.gazebo.xacro b/src/custom_xarm_description/urdf/xarm5/xarm5.gazebo.xacro new file mode 100755 index 0000000..fabfb9e --- /dev/null +++ b/src/custom_xarm_description/urdf/xarm5/xarm5.gazebo.xacro @@ -0,0 +1,32 @@ + + + + + + + true + + + + true + + + + true + + + + true + + + + true + + + + true + + + + + \ No newline at end of file diff --git a/src/custom_xarm_description/urdf/xarm5/xarm5.ros2_control.xacro b/src/custom_xarm_description/urdf/xarm5/xarm5.ros2_control.xacro new file mode 100644 index 0000000..10ab27c --- /dev/null +++ b/src/custom_xarm_description/urdf/xarm5/xarm5.ros2_control.xacro @@ -0,0 +1,96 @@ + + + + + + ${ros2_control_plugin} + + ${prefix}${hw_ns} + ${velocity_control} + P${prefix} + R${robot_ip} + ${report_type} + 5 + ${baud_checkset} + ${default_gripper_baud} + xarm + ${add_gripper} + + + + + ${joint1_lower_limit} + ${joint1_upper_limit} + + + -3.14 + 3.14 + + + + + + + + ${joint2_lower_limit} + ${joint2_upper_limit} + + + -3.14 + 3.14 + + + + + + + + ${joint3_lower_limit} + ${joint3_upper_limit} + + + -3.14 + 3.14 + + + + + + + + ${joint4_lower_limit} + ${joint4_upper_limit} + + + -3.14 + 3.14 + + + + + + + + ${joint5_lower_limit} + ${joint5_upper_limit} + + + -3.14 + 3.14 + + + + + + + + \ No newline at end of file diff --git a/src/custom_xarm_description/urdf/xarm5/xarm5.transmission.xacro b/src/custom_xarm_description/urdf/xarm5/xarm5.transmission.xacro new file mode 100755 index 0000000..f90e970 --- /dev/null +++ b/src/custom_xarm_description/urdf/xarm5/xarm5.transmission.xacro @@ -0,0 +1,64 @@ + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + + diff --git a/src/custom_xarm_description/urdf/xarm5/xarm5.urdf.xacro b/src/custom_xarm_description/urdf/xarm5/xarm5.urdf.xacro new file mode 100755 index 0000000..b3f7c18 --- /dev/null +++ b/src/custom_xarm_description/urdf/xarm5/xarm5.urdf.xacro @@ -0,0 +1,364 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/custom_xarm_description/urdf/xarm5/xarm5_robot_macro.xacro b/src/custom_xarm_description/urdf/xarm5/xarm5_robot_macro.xacro new file mode 100755 index 0000000..d460a2e --- /dev/null +++ b/src/custom_xarm_description/urdf/xarm5/xarm5_robot_macro.xacro @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/custom_xarm_description/urdf/xarm6/xarm6.gazebo.xacro b/src/custom_xarm_description/urdf/xarm6/xarm6.gazebo.xacro new file mode 100755 index 0000000..99ed6c0 --- /dev/null +++ b/src/custom_xarm_description/urdf/xarm6/xarm6.gazebo.xacro @@ -0,0 +1,36 @@ + + + + + + + true + + + + true + + + + true + + + + true + + + + true + + + + true + + + + true + + + + + diff --git a/src/custom_xarm_description/urdf/xarm6/xarm6.ros2_control.xacro b/src/custom_xarm_description/urdf/xarm6/xarm6.ros2_control.xacro new file mode 100644 index 0000000..1ec5259 --- /dev/null +++ b/src/custom_xarm_description/urdf/xarm6/xarm6.ros2_control.xacro @@ -0,0 +1,118 @@ + + + + + + ${ros2_control_plugin} + + ${prefix}${hw_ns} + ${velocity_control} + P${prefix} + R${robot_ip} + ${report_type} + 6 + ${baud_checkset} + ${default_gripper_baud} + xarm + ${add_gripper} + + + + + + + + + + + + + ${joint1_lower_limit} + ${joint1_upper_limit} + + + -3.14 + 3.14 + + + + + + + + ${joint2_lower_limit} + ${joint2_upper_limit} + + + -3.14 + 3.14 + + + + + + + + ${joint3_lower_limit} + ${joint3_upper_limit} + + + -3.14 + 3.14 + + + + + + + + ${joint4_lower_limit} + ${joint4_upper_limit} + + + -3.14 + 3.14 + + + + + + + + ${joint5_lower_limit} + ${joint5_upper_limit} + + + -3.14 + 3.14 + + + + + + + + ${joint6_lower_limit} + ${joint6_upper_limit} + + + -3.14 + 3.14 + + + + + + + + \ No newline at end of file diff --git a/src/custom_xarm_description/urdf/xarm6/xarm6.transmission.xacro b/src/custom_xarm_description/urdf/xarm6/xarm6.transmission.xacro new file mode 100755 index 0000000..ad43571 --- /dev/null +++ b/src/custom_xarm_description/urdf/xarm6/xarm6.transmission.xacro @@ -0,0 +1,75 @@ + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + + diff --git a/src/custom_xarm_description/urdf/xarm6/xarm6.urdf.xacro b/src/custom_xarm_description/urdf/xarm6/xarm6.urdf.xacro new file mode 100755 index 0000000..eb1c55e --- /dev/null +++ b/src/custom_xarm_description/urdf/xarm6/xarm6.urdf.xacro @@ -0,0 +1,335 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/custom_xarm_description/urdf/xarm6/xarm6_robot_macro.xacro b/src/custom_xarm_description/urdf/xarm6/xarm6_robot_macro.xacro new file mode 100755 index 0000000..e49d8f8 --- /dev/null +++ b/src/custom_xarm_description/urdf/xarm6/xarm6_robot_macro.xacro @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/custom_xarm_description/urdf/xarm7/xarm7.gazebo.xacro b/src/custom_xarm_description/urdf/xarm7/xarm7.gazebo.xacro new file mode 100755 index 0000000..27b2c9d --- /dev/null +++ b/src/custom_xarm_description/urdf/xarm7/xarm7.gazebo.xacro @@ -0,0 +1,40 @@ + + + + + + + true + + + + true + + + + true + + + + true + + + + true + + + + true + + + + true + + + + true + + + + + diff --git a/src/custom_xarm_description/urdf/xarm7/xarm7.ros2_control.xacro b/src/custom_xarm_description/urdf/xarm7/xarm7.ros2_control.xacro new file mode 100644 index 0000000..e0b255a --- /dev/null +++ b/src/custom_xarm_description/urdf/xarm7/xarm7.ros2_control.xacro @@ -0,0 +1,124 @@ + + + + + + ${ros2_control_plugin} + + ${prefix}${hw_ns} + ${velocity_control} + P${prefix} + R${robot_ip} + ${report_type} + 7 + ${baud_checkset} + ${default_gripper_baud} + xarm + ${add_gripper} + + + + + ${joint1_lower_limit} + ${joint1_upper_limit} + + + -3.14 + 3.14 + + + + + + + + ${joint2_lower_limit} + ${joint2_upper_limit} + + + -3.14 + 3.14 + + + + + + + + ${joint3_lower_limit} + ${joint3_upper_limit} + + + -3.14 + 3.14 + + + + + + + + ${joint4_lower_limit} + ${joint4_upper_limit} + + + -3.14 + 3.14 + + + + + + + + ${joint5_lower_limit} + ${joint5_upper_limit} + + + -3.14 + 3.14 + + + + + + + + ${joint6_lower_limit} + ${joint6_upper_limit} + + + -3.14 + 3.14 + + + + + + + + ${joint7_lower_limit} + ${joint7_upper_limit} + + + -3.14 + 3.14 + + + + + + + + \ No newline at end of file diff --git a/src/custom_xarm_description/urdf/xarm7/xarm7.transmission.xacro b/src/custom_xarm_description/urdf/xarm7/xarm7.transmission.xacro new file mode 100755 index 0000000..b956401 --- /dev/null +++ b/src/custom_xarm_description/urdf/xarm7/xarm7.transmission.xacro @@ -0,0 +1,86 @@ + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + transmission_interface/SimpleTransmission + + hardware_interface/${hard_interface} + + + hardware_interface/${hard_interface} + ${reduction} + + + + + + diff --git a/src/custom_xarm_description/urdf/xarm7/xarm7.urdf.xacro b/src/custom_xarm_description/urdf/xarm7/xarm7.urdf.xacro new file mode 100755 index 0000000..2ed35ea --- /dev/null +++ b/src/custom_xarm_description/urdf/xarm7/xarm7.urdf.xacro @@ -0,0 +1,480 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/custom_xarm_description/urdf/xarm7/xarm7_robot_macro.xacro b/src/custom_xarm_description/urdf/xarm7/xarm7_robot_macro.xacro new file mode 100644 index 0000000..bbd0511 --- /dev/null +++ b/src/custom_xarm_description/urdf/xarm7/xarm7_robot_macro.xacro @@ -0,0 +1,69 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/custom_xarm_description/urdf/xarm_device.urdf.xacro b/src/custom_xarm_description/urdf/xarm_device.urdf.xacro new file mode 100755 index 0000000..46ab860 --- /dev/null +++ b/src/custom_xarm_description/urdf/xarm_device.urdf.xacro @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/custom_xarm_description/urdf/xarm_device_macro.xacro b/src/custom_xarm_description/urdf/xarm_device_macro.xacro new file mode 100644 index 0000000..a85e7cb --- /dev/null +++ b/src/custom_xarm_description/urdf/xarm_device_macro.xacro @@ -0,0 +1,140 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/custom_xarm_gazebo/CMakeLists.txt b/src/custom_xarm_gazebo/CMakeLists.txt new file mode 100644 index 0000000..dfffff1 --- /dev/null +++ b/src/custom_xarm_gazebo/CMakeLists.txt @@ -0,0 +1,76 @@ +cmake_minimum_required(VERSION 3.5) +project(custom_xarm_gazebo) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(control_toolbox REQUIRED) +find_package(controller_manager REQUIRED) +find_package(gazebo_ros REQUIRED) + +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +include_directories( + include + ${ament_INCLUDE_DIRS} + ${GAZEBO_INCLUDE_DIRS} +) + +link_directories(${GAZEBO_LIBRARY_DIRS}) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") + +add_library(gazebo_mimic_joint_plugin SHARED src/mimic_joint_plugin.cpp) +ament_target_dependencies(gazebo_mimic_joint_plugin + control_toolbox + controller_manager + gazebo_ros +) +target_link_libraries(gazebo_mimic_joint_plugin ${ament_LIBRARIES} ${GAZEBO_LIBRARIES}) + +install( + DIRECTORY include/ + DESTINATION include +) + +install(TARGETS +gazebo_mimic_joint_plugin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + + +install(DIRECTORY + worlds + launch + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/src/custom_xarm_gazebo/include/xarm_gazebo/mimic_joint_plugin.h b/src/custom_xarm_gazebo/include/xarm_gazebo/mimic_joint_plugin.h new file mode 100644 index 0000000..e2407ec --- /dev/null +++ b/src/custom_xarm_gazebo/include/xarm_gazebo/mimic_joint_plugin.h @@ -0,0 +1,79 @@ +/* + +Copyright (c) 2014, Konstantinos Chatzilygeroudis +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer + in the documentation and/or other materials provided with the distribution. +3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived + from this software without specific prior written permission. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, +BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT +SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE +OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +Refer to: https://github.com/roboticsgroup/roboticsgroup_upatras_gazebo_plugins + +Modified for ROS2 compatibility and UFACTORY xArm gripper simulation + +Modified by: Vinman +============================================================================*/ + +#ifndef XARM_GAZEBO_MIMIC_JOINT_PLUGIN_H +#define XARM_GAZEBO_MIMIC_JOINT_PLUGIN_H + +#include + +#include "controller_manager/controller_manager.hpp" + +// ros_control +#include + +// Gazebo includes +#include +#include +#include +#include +#include + +namespace gazebo { + class GazeboMimicJointPlugin : public ModelPlugin { + public: + GazeboMimicJointPlugin(); + virtual ~GazeboMimicJointPlugin() override; + + // Overloaded Gazebo entry point + virtual void Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf) override; + + private: + void OnUpdate(); + + // Node Handles + gazebo_ros::Node::SharedPtr model_nh_; + // Pointer to the update event connection + gazebo::event::ConnectionPtr update_connection_; + + // PID controller if needed + std::shared_ptr pid_; + + // Pointers to the joints + physics::JointPtr joint_, mimic_joint_; + + // Pointer to the model + physics::ModelPtr model_; + + // Parameters + std::string joint_name_, mimic_joint_name_; + double multiplier_, offset_, sensitiveness_, max_effort_; + bool has_pid_; + + std::shared_ptr loop_rate_; + }; +} + + +#endif \ No newline at end of file diff --git a/src/custom_xarm_gazebo/launch/_dual_robot_beside_table_gazebo.launch.py b/src/custom_xarm_gazebo/launch/_dual_robot_beside_table_gazebo.launch.py new file mode 100644 index 0000000..9d1cd3f --- /dev/null +++ b/src/custom_xarm_gazebo/launch/_dual_robot_beside_table_gazebo.launch.py @@ -0,0 +1,258 @@ +#!/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 +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_ros.substitutions import FindPackageShare +from launch.event_handlers import OnProcessExit +from launch.actions import OpaqueFunction + + +def launch_setup(context, *args, **kwargs): + prefix_1 = LaunchConfiguration('prefix_1', default='L_') + prefix_2 = LaunchConfiguration('prefix_2', default='R_') + dof = LaunchConfiguration('dof', default=7) + dof_1 = LaunchConfiguration('dof_1', default=dof) + dof_2 = LaunchConfiguration('dof_2', default=dof) + robot_type = LaunchConfiguration('robot_type', default='xarm') + robot_type_1 = LaunchConfiguration('robot_type_1', default=robot_type) + robot_type_2 = LaunchConfiguration('robot_type_2', default=robot_type) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_gripper_1 = LaunchConfiguration('add_gripper_1', default=add_gripper) + add_gripper_2 = LaunchConfiguration('add_gripper_2', default=add_gripper) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + add_vacuum_gripper_1 = LaunchConfiguration('add_vacuum_gripper_1', default=add_vacuum_gripper) + add_vacuum_gripper_2 = LaunchConfiguration('add_vacuum_gripper_2', default=add_vacuum_gripper) + 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) + ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='gazebo_ros2_control/GazeboSystem') + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + add_realsense_d435i_1 = LaunchConfiguration('add_realsense_d435i_1', default=add_realsense_d435i) + add_realsense_d435i_2 = LaunchConfiguration('add_realsense_d435i_2', default=add_realsense_d435i) + + add_other_geometry = LaunchConfiguration('add_other_geometry', default=False) + add_other_geometry_1 = LaunchConfiguration('add_other_geometry_1', default=add_other_geometry) + add_other_geometry_2 = LaunchConfiguration('add_other_geometry_2', default=add_other_geometry) + + geometry_type = LaunchConfiguration('geometry_type', default='box') + geometry_type_1 = LaunchConfiguration('geometry_type_1', default=geometry_type) + geometry_type_2 = LaunchConfiguration('geometry_type_2', default=geometry_type) + + geometry_mass = LaunchConfiguration('geometry_mass', default=0.1) + geometry_mass_1 = LaunchConfiguration('geometry_mass_1', default=geometry_mass) + geometry_mass_2 = LaunchConfiguration('geometry_mass_2', default=geometry_mass) + + geometry_height = LaunchConfiguration('geometry_height', default=0.1) + geometry_height_1 = LaunchConfiguration('geometry_height_1', default=geometry_height) + geometry_height_2 = LaunchConfiguration('geometry_height_2', default=geometry_height) + + geometry_radius = LaunchConfiguration('geometry_radius', default=0.1) + geometry_radius_1 = LaunchConfiguration('geometry_radius_1', default=geometry_radius) + geometry_radius_2 = LaunchConfiguration('geometry_radius_2', default=geometry_radius) + + geometry_length = LaunchConfiguration('geometry_length', default=0.1) + geometry_length_1 = LaunchConfiguration('geometry_length_1', default=geometry_length) + geometry_length_2 = LaunchConfiguration('geometry_length_2', default=geometry_length) + + geometry_width = LaunchConfiguration('geometry_width', default=0.1) + geometry_width_1 = LaunchConfiguration('geometry_width_1', default=geometry_width) + geometry_width_2 = LaunchConfiguration('geometry_width_2', default=geometry_width) + + geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='') + geometry_mesh_filename_1 = LaunchConfiguration('geometry_mesh_filename_1', default=geometry_mesh_filename) + geometry_mesh_filename_2 = LaunchConfiguration('geometry_mesh_filename_2', default=geometry_mesh_filename) + + geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"') + geometry_mesh_origin_xyz_1 = LaunchConfiguration('geometry_mesh_origin_xyz_1', default=geometry_mesh_origin_xyz) + geometry_mesh_origin_xyz_2 = LaunchConfiguration('geometry_mesh_origin_xyz_2', default=geometry_mesh_origin_xyz) + + geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"') + geometry_mesh_origin_rpy_1 = LaunchConfiguration('geometry_mesh_origin_rpy_1', default=geometry_mesh_origin_rpy) + geometry_mesh_origin_rpy_2 = LaunchConfiguration('geometry_mesh_origin_rpy_2', default=geometry_mesh_origin_rpy) + + geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"') + geometry_mesh_tcp_xyz_1 = LaunchConfiguration('geometry_mesh_tcp_xyz_1', default=geometry_mesh_tcp_xyz) + geometry_mesh_tcp_xyz_2 = LaunchConfiguration('geometry_mesh_tcp_xyz_2', default=geometry_mesh_tcp_xyz) + + geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"') + geometry_mesh_tcp_rpy_1 = LaunchConfiguration('geometry_mesh_tcp_rpy_1', default=geometry_mesh_tcp_rpy) + geometry_mesh_tcp_rpy_2 = LaunchConfiguration('geometry_mesh_tcp_rpy_2', default=geometry_mesh_tcp_rpy) + + load_controller = LaunchConfiguration('load_controller', default=False) + + ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context) + + # 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_dual_ros2_control_params_temp_file = getattr(mod, 'generate_dual_ros2_control_params_temp_file') + ros2_control_params = generate_dual_ros2_control_params_temp_file( + os.path.join(get_package_share_directory('xarm_controller'), 'config', '{}{}_controllers.yaml'.format(robot_type_1.perform(context), dof_1.perform(context))), + os.path.join(get_package_share_directory('xarm_controller'), 'config', '{}{}_controllers.yaml'.format(robot_type_2.perform(context), dof_2.perform(context))), + prefix_1=prefix_1.perform(context), + prefix_2=prefix_2.perform(context), + add_gripper_1=add_gripper_1.perform(context) in ('True', 'true'), + add_gripper_2=add_gripper_2.perform(context) in ('True', 'true'), + ros_namespace=ros_namespace, + update_rate=1000, + ) + + # 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('xarm_description'), 'urdf', 'dual_xarm_device.urdf.xacro']), + arguments={ + 'prefix_1': prefix_1, + 'prefix_2': prefix_2, + 'dof_1': dof_1, + 'dof_2': dof_2, + 'robot_type_1': robot_type_1, + 'robot_type_2': robot_type_2, + 'add_gripper_1': add_gripper_1, + 'add_gripper_2': add_gripper_2, + 'add_vacuum_gripper_1': add_vacuum_gripper_1, + 'add_vacuum_gripper_2': add_vacuum_gripper_2, + '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_1': add_realsense_d435i_1, + 'add_realsense_d435i_2': add_realsense_d435i_2, + 'add_other_geometry_1': add_other_geometry_1, + 'add_other_geometry_2': add_other_geometry_2, + 'geometry_type_1': geometry_type_1, + 'geometry_type_2': geometry_type_2, + 'geometry_mass_1': geometry_mass_1, + 'geometry_mass_2': geometry_mass_2, + 'geometry_height_1': geometry_height_1, + 'geometry_height_2': geometry_height_2, + 'geometry_radius_1': geometry_radius_1, + 'geometry_radius_2': geometry_radius_2, + 'geometry_length_1': geometry_length_1, + 'geometry_length_2': geometry_length_2, + 'geometry_width_1': geometry_width_1, + 'geometry_width_2': geometry_width_2, + 'geometry_mesh_filename_1': geometry_mesh_filename_1, + 'geometry_mesh_filename_2': geometry_mesh_filename_2, + 'geometry_mesh_origin_xyz_1': geometry_mesh_origin_xyz_1, + 'geometry_mesh_origin_xyz_2': geometry_mesh_origin_xyz_2, + 'geometry_mesh_origin_rpy_1': geometry_mesh_origin_rpy_1, + 'geometry_mesh_origin_rpy_2': geometry_mesh_origin_rpy_2, + 'geometry_mesh_tcp_xyz_1': geometry_mesh_tcp_xyz_1, + 'geometry_mesh_tcp_xyz_2': geometry_mesh_tcp_xyz_2, + 'geometry_mesh_tcp_rpy_1': geometry_mesh_tcp_rpy_1, + 'geometry_mesh_tcp_rpy_2': geometry_mesh_tcp_rpy_2, + } + ), + } + + # 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('xarm_gazebo'), 'worlds', 'table.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', + # 'pause': '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', 'dual_xarm', + '-x', '0.5', + '-y', '-0.5', + '-z', '1.021', + '-Y', '1.571', + ], + parameters=[{'use_sim_time': True}], + ) + + # Load controllers + controllers = [ + 'joint_state_broadcaster', + '{}{}{}_traj_controller'.format(prefix_1.perform(context), robot_type_1.perform(context), dof_1.perform(context)), + '{}{}{}_traj_controller'.format(prefix_2.perform(context), robot_type_2.perform(context), dof_2.perform(context)), + ] + # check robot_type is xarm + if robot_type_1.perform(context) == 'xarm' and add_gripper_1.perform(context) in ('True', 'true'): + controllers.append('{}{}_gripper_traj_controller'.format(prefix_1.perform(context), robot_type_1.perform(context))) + # check robot_type is xarm + if robot_type_2.perform(context) == 'xarm' and add_gripper_2.perform(context) in ('True', 'true'): + controllers.append('{}{}_gripper_traj_controller'.format(prefix_2.perform(context), robot_type_2.perform(context))) + load_controllers = [] + if load_controller.perform(context) in ('True', 'true'): + for controller in controllers: + load_controllers.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(load_controllers) > 0: + return [ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=gazebo_spawn_entity_node, + on_exit=load_controllers, + ) + ), + gazebo_launch, + robot_state_publisher_node, + gazebo_spawn_entity_node, + ] + else: + return [ + gazebo_launch, + robot_state_publisher_node, + gazebo_spawn_entity_node, + ] + + +def generate_launch_description(): + return LaunchDescription([ + OpaqueFunction(function=launch_setup) + ]) diff --git a/src/custom_xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py b/src/custom_xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py new file mode 100644 index 0000000..d8b02bc --- /dev/null +++ b/src/custom_xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py @@ -0,0 +1,184 @@ +#!/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 +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_ros.substitutions import FindPackageShare +from launch.event_handlers import OnProcessExit +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) + dof = LaunchConfiguration('dof', default=7) + 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_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"') + load_controller = LaunchConfiguration('load_controller', default=False) + + ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context) + + # 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))), + prefix=prefix.perform(context), + add_gripper=add_gripper.perform(context) in ('True', 'true'), + ros_namespace=LaunchConfiguration('ros_namespace', default='').perform(context), + update_rate=1000, + ) + + # 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('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']), + arguments={ + 'prefix': prefix, + 'dof': dof, + 'robot_type': robot_type, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_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_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, + } + ), + } + + # 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('xarm_gazebo'), 'worlds', 'table.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(robot_type.perform(context), dof.perform(context)), + '-x', '-0.2', + '-y', '-0.5', + '-z', '1.021', + '-Y', '1.571', + ], + parameters=[{'use_sim_time': True}], + ) + + # Load controllers + controllers = [ + 'joint_state_broadcaster', + '{}{}{}_traj_controller'.format(prefix.perform(context), robot_type.perform(context), dof.perform(context)), + ] + if robot_type.perform(context) == 'xarm' and add_gripper.perform(context) in ('True', 'true'): + controllers.append('{}{}_gripper_traj_controller'.format(prefix.perform(context), robot_type.perform(context))) + load_controllers = [] + if load_controller.perform(context) in ('True', 'true'): + for controller in controllers: + load_controllers.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(load_controllers) > 0: + return [ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=gazebo_spawn_entity_node, + on_exit=load_controllers, + ) + ), + gazebo_launch, + robot_state_publisher_node, + gazebo_spawn_entity_node, + ] + else: + return [ + gazebo_launch, + robot_state_publisher_node, + gazebo_spawn_entity_node, + ] + + +def generate_launch_description(): + return LaunchDescription([ + OpaqueFunction(function=launch_setup) + ]) diff --git a/src/custom_xarm_gazebo/launch/lite6_beside_table_gazebo.launch.py b/src/custom_xarm_gazebo/launch/lite6_beside_table_gazebo.launch.py new file mode 100644 index 0000000..e23163f --- /dev/null +++ b/src/custom_xarm_gazebo/launch/lite6_beside_table_gazebo.launch.py @@ -0,0 +1,70 @@ +#!/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 IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir + +def generate_launch_description(): + 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_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # robot gazebo launch + # xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py + robot_gazobo_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/_robot_beside_table_gazebo.launch.py']), + launch_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '6', + 'robot_type': 'lite', + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + return LaunchDescription([ + robot_gazobo_launch + ]) diff --git a/src/custom_xarm_gazebo/launch/xarm5_beside_table_gazebo.launch.py b/src/custom_xarm_gazebo/launch/xarm5_beside_table_gazebo.launch.py new file mode 100644 index 0000000..cd099e4 --- /dev/null +++ b/src/custom_xarm_gazebo/launch/xarm5_beside_table_gazebo.launch.py @@ -0,0 +1,70 @@ +#!/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 IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir + +def generate_launch_description(): + 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_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # robot gazebo launch + # xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py + robot_gazobo_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/_robot_beside_table_gazebo.launch.py']), + launch_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '5', + 'robot_type': 'xarm', + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + return LaunchDescription([ + robot_gazobo_launch + ]) diff --git a/src/custom_xarm_gazebo/launch/xarm6_beside_table_gazebo.launch.py b/src/custom_xarm_gazebo/launch/xarm6_beside_table_gazebo.launch.py new file mode 100644 index 0000000..241a461 --- /dev/null +++ b/src/custom_xarm_gazebo/launch/xarm6_beside_table_gazebo.launch.py @@ -0,0 +1,70 @@ +#!/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 IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir + +def generate_launch_description(): + 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_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # robot gazebo launch + # xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py + robot_gazobo_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/_robot_beside_table_gazebo.launch.py']), + launch_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '6', + 'robot_type': 'xarm', + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + return LaunchDescription([ + robot_gazobo_launch + ]) diff --git a/src/custom_xarm_gazebo/launch/xarm7_beside_table_gazebo.launch.py b/src/custom_xarm_gazebo/launch/xarm7_beside_table_gazebo.launch.py new file mode 100644 index 0000000..92f95f1 --- /dev/null +++ b/src/custom_xarm_gazebo/launch/xarm7_beside_table_gazebo.launch.py @@ -0,0 +1,70 @@ +#!/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 IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir + +def generate_launch_description(): + 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_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # robot gazebo launch + # xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py + robot_gazobo_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/_robot_beside_table_gazebo.launch.py']), + launch_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '7', + 'robot_type': 'xarm', + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + return LaunchDescription([ + robot_gazobo_launch + ]) diff --git a/src/custom_xarm_gazebo/package.xml b/src/custom_xarm_gazebo/package.xml new file mode 100644 index 0000000..42d73b5 --- /dev/null +++ b/src/custom_xarm_gazebo/package.xml @@ -0,0 +1,31 @@ + + + + custom_xarm_gazebo + 0.0.0 + TODO: Package description + + TODO: License declaration + + ament_cmake + + rclcpp + control_toolbox + controller_manager + gazebo_ros + xarm_description + xarm_controller + + launch + launch_ros + gazebo_plugins + gazebo_ros2_control + robot_state_publisher + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/custom_xarm_gazebo/src/mimic_joint_plugin.cpp b/src/custom_xarm_gazebo/src/mimic_joint_plugin.cpp new file mode 100644 index 0000000..b81ece4 --- /dev/null +++ b/src/custom_xarm_gazebo/src/mimic_joint_plugin.cpp @@ -0,0 +1,166 @@ +/* + +Copyright (c) 2014, Konstantinos Chatzilygeroudis +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer + in the documentation and/or other materials provided with the distribution. +3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived + from this software without specific prior written permission. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, +BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT +SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE +OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +Refer to: https://github.com/roboticsgroup/roboticsgroup_upatras_gazebo_plugins + +Modified for ROS2 compatibility and UFACTORY xArm gripper simulation + +Modified by: Vinman +============================================================================*/ + +#include "xarm_gazebo/mimic_joint_plugin.h" + + +namespace gazebo { + GazeboMimicJointPlugin::GazeboMimicJointPlugin() + { + joint_.reset(); + mimic_joint_.reset(); + } + + GazeboMimicJointPlugin::~GazeboMimicJointPlugin() + { + update_connection_.reset(); + } + + void GazeboMimicJointPlugin::Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf) + { + model_ = parent; + model_nh_ = gazebo_ros::Node::Get(sdf); + + RCLCPP_INFO( + model_nh_->get_logger(), "Starting GazeboMimicJointPlugin in namespace: %s, name: %s", + model_nh_->get_namespace(), model_nh_->get_name()); + + // Error message if the model couldn't be found + if (!model_) { + RCLCPP_ERROR_STREAM(model_nh_->get_logger(), "parent model is NULL"); + return; + } + + // Check that ROS has been initialized + if (!rclcpp::ok()) { + RCLCPP_FATAL_STREAM( + model_nh_->get_logger(), + "A ROS node for Gazebo has not been initialized, unable to load plugin. " << + "Load the Gazebo system plugin 'libgazebo_ros2_mimic_joint_plugin.so' in the gazebo_ros package)"); + return; + } + + // Check for joint element + if (!sdf->HasElement("joint")) { + RCLCPP_ERROR(model_nh_->get_logger(), "No joint element present. GazeboMimicJointPlugin could not be loaded."); + return; + } + + joint_name_ = sdf->GetElement("joint")->Get(); + + // Check for mimicJoint element + if (!sdf->HasElement("mimicJoint")) { + RCLCPP_ERROR(model_nh_->get_logger(), "No mimicJoint element present. GazeboMimicJointPlugin could not be loaded."); + return; + } + + mimic_joint_name_ = sdf->GetElement("mimicJoint")->Get(); + + // Check if PID controller wanted + has_pid_ = sdf->HasElement("hasPID"); + if (has_pid_) { + const std::string prefix = "gains." + joint_name_; + const auto k_p = model_nh_->declare_parameter(prefix + ".p", 10.0); + const auto k_i = model_nh_->declare_parameter(prefix + ".i", 0.1); + const auto k_d = model_nh_->declare_parameter(prefix + ".d", 0.0); + const auto i_clamp = model_nh_->declare_parameter(prefix + ".i_clamp", 0.2); + // Initialize PID + pid_ = std::make_shared(k_p, k_i, k_d, i_clamp, -i_clamp); + } + + // Check for multiplier element + multiplier_ = 1.0; + if (sdf->HasElement("multiplier")) + multiplier_ = sdf->GetElement("multiplier")->Get(); + + // Check for offset element + offset_ = 0.0; + if (sdf->HasElement("offset")) + offset_ = sdf->GetElement("offset")->Get(); + + // Check for sensitiveness element + sensitiveness_ = 0.0; + if (sdf->HasElement("sensitiveness")) + sensitiveness_ = sdf->GetElement("sensitiveness")->Get(); + + // Get pointers to joints + joint_ = model_->GetJoint(joint_name_); + if (!joint_) { + RCLCPP_ERROR(model_nh_->get_logger(), "No joint named \"%s\". GazeboMimicJointPlugin could not be loaded.", joint_name_.c_str()); + return; + } + mimic_joint_ = model_->GetJoint(mimic_joint_name_); + if (!mimic_joint_) { + RCLCPP_ERROR(model_nh_->get_logger(), "No (mimic) joint named \"%s\". GazeboMimicJointPlugin could not be loaded.", mimic_joint_name_.c_str()); + return; + } + + // Check for max effort + if (sdf->HasElement("maxEffort")) { + max_effort_ = sdf->GetElement("maxEffort")->Get(); + } + else { + max_effort_ = mimic_joint_->GetEffortLimit(0); + } + + // Set max effort + if (!has_pid_) { + mimic_joint_->SetParam("fmax", 0, max_effort_); + } + + loop_rate_ = rclcpp::Rate::make_shared(1.0 / model_->GetWorld()->Physics()->GetMaxStepSize()); + + // Listen to the update event. This event is broadcast every + // simulation iteration. + update_connection_ = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboMimicJointPlugin::OnUpdate, this)); + + // Output some confirmation + RCLCPP_INFO_STREAM(model_nh_->get_logger(), "GazeboMimicJointPlugin loaded! Joint: \"" << joint_name_ << "\", Mimic joint: \"" << mimic_joint_name_ << "\"" + << ", Multiplier: " << multiplier_ << ", Offset: " << offset_ + << ", MaxEffort: " << max_effort_ << ", Sensitiveness: " << sensitiveness_); + } + + void GazeboMimicJointPlugin::OnUpdate() + { + // Set mimic joint's angle based on joint's angle + double angle = joint_->Position(0) * multiplier_ + offset_; + double a = mimic_joint_->Position(0); + + if (fabs(angle - a) >= sensitiveness_) { + if (has_pid_) { + double error = angle - a; + double effort = ignition::math::clamp(pid_->computeCommand(error, loop_rate_->period().count()), -max_effort_, max_effort_); + mimic_joint_->SetForce(0, effort); + } + else { + mimic_joint_->SetPosition(0, angle, true); + } + } + } + + GZ_REGISTER_MODEL_PLUGIN(GazeboMimicJointPlugin) + +} // namespace gazebo \ No newline at end of file diff --git a/src/custom_xarm_gazebo/worlds/table.world b/src/custom_xarm_gazebo/worlds/table.world new file mode 100755 index 0000000..8c54831 --- /dev/null +++ b/src/custom_xarm_gazebo/worlds/table.world @@ -0,0 +1,25 @@ + + + + + model://ground_plane + + + model://sun + + + model://table + table + 0.0 -0.84 0 0 0 0 + + + + 0.001 + 1 + 1000 + + 0 0 0 + + + + diff --git a/src/custom_xarm_moveit_config/CMakeLists.txt b/src/custom_xarm_moveit_config/CMakeLists.txt new file mode 100644 index 0000000..c6d10cb --- /dev/null +++ b/src/custom_xarm_moveit_config/CMakeLists.txt @@ -0,0 +1,43 @@ +cmake_minimum_required(VERSION 3.5) +project(custom_xarm_moveit_config) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +install(DIRECTORY + config + rviz + srdf + launch + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/src/custom_xarm_moveit_config/config/lite6/controllers.yaml b/src/custom_xarm_moveit_config/config/lite6/controllers.yaml new file mode 100644 index 0000000..215a627 --- /dev/null +++ b/src/custom_xarm_moveit_config/config/lite6/controllers.yaml @@ -0,0 +1,14 @@ +controller_names: + - lite6_traj_controller + +lite6_traj_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 diff --git a/src/custom_xarm_moveit_config/config/lite6/fake_controllers.yaml b/src/custom_xarm_moveit_config/config/lite6/fake_controllers.yaml new file mode 100644 index 0000000..215a627 --- /dev/null +++ b/src/custom_xarm_moveit_config/config/lite6/fake_controllers.yaml @@ -0,0 +1,14 @@ +controller_names: + - lite6_traj_controller + +lite6_traj_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 diff --git a/src/custom_xarm_moveit_config/config/lite6/joint_limits.yaml b/src/custom_xarm_moveit_config/config/lite6/joint_limits.yaml new file mode 100755 index 0000000..ee294c8 --- /dev/null +++ b/src/custom_xarm_moveit_config/config/lite6/joint_limits.yaml @@ -0,0 +1,34 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed +# 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: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0.0 + joint2: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0.0 + joint3: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0.0 + joint4: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0.0 + joint5: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0.0 + joint6: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0.0 diff --git a/src/custom_xarm_moveit_config/config/lite6/kinematics.yaml b/src/custom_xarm_moveit_config/config/lite6/kinematics.yaml new file mode 100755 index 0000000..f5173f6 --- /dev/null +++ b/src/custom_xarm_moveit_config/config/lite6/kinematics.yaml @@ -0,0 +1,6 @@ +lite6: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + # kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 diff --git a/src/custom_xarm_moveit_config/config/lite6/ompl_planning.yaml b/src/custom_xarm_moveit_config/config/lite6/ompl_planning.yaml new file mode 100755 index 0000000..771f93c --- /dev/null +++ b/src/custom_xarm_moveit_config/config/lite6/ompl_planning.yaml @@ -0,0 +1,149 @@ +planner_configs: + SBL: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECE: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECE: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECE: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRT: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRT: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMT: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDST: + type: geometric::PDST + STRIDE: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRT: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRT: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwo: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 + +lite6: + default_planner_config: RRT + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo diff --git a/src/custom_xarm_moveit_config/config/xarm5/controllers.yaml b/src/custom_xarm_moveit_config/config/xarm5/controllers.yaml new file mode 100644 index 0000000..f6aa421 --- /dev/null +++ b/src/custom_xarm_moveit_config/config/xarm5/controllers.yaml @@ -0,0 +1,13 @@ +controller_names: + - xarm5_traj_controller + +xarm5_traj_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 diff --git a/src/custom_xarm_moveit_config/config/xarm5/fake_controllers.yaml b/src/custom_xarm_moveit_config/config/xarm5/fake_controllers.yaml new file mode 100644 index 0000000..f6aa421 --- /dev/null +++ b/src/custom_xarm_moveit_config/config/xarm5/fake_controllers.yaml @@ -0,0 +1,13 @@ +controller_names: + - xarm5_traj_controller + +xarm5_traj_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 diff --git a/src/custom_xarm_moveit_config/config/xarm5/joint_limits.yaml b/src/custom_xarm_moveit_config/config/xarm5/joint_limits.yaml new file mode 100644 index 0000000..6e2eba5 --- /dev/null +++ b/src/custom_xarm_moveit_config/config/xarm5/joint_limits.yaml @@ -0,0 +1,29 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed +# 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: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0.0 + joint2: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0.0 + joint3: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0.0 + joint4: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0.0 + joint5: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0.0 diff --git a/src/custom_xarm_moveit_config/config/xarm5/kinematics.yaml b/src/custom_xarm_moveit_config/config/xarm5/kinematics.yaml new file mode 100644 index 0000000..45b7674 --- /dev/null +++ b/src/custom_xarm_moveit_config/config/xarm5/kinematics.yaml @@ -0,0 +1,6 @@ +xarm5: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + # kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 \ No newline at end of file diff --git a/src/custom_xarm_moveit_config/config/xarm5/ompl_planning.yaml b/src/custom_xarm_moveit_config/config/xarm5/ompl_planning.yaml new file mode 100644 index 0000000..f6da751 --- /dev/null +++ b/src/custom_xarm_moveit_config/config/xarm5/ompl_planning.yaml @@ -0,0 +1,149 @@ +planner_configs: + SBL: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECE: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECE: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECE: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRT: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRT: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMT: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDST: + type: geometric::PDST + STRIDE: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRT: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRT: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwo: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 + +xarm5: + default_planner_config: RRTConnect + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo diff --git a/src/custom_xarm_moveit_config/config/xarm6/controllers.yaml b/src/custom_xarm_moveit_config/config/xarm6/controllers.yaml new file mode 100644 index 0000000..6dbb924 --- /dev/null +++ b/src/custom_xarm_moveit_config/config/xarm6/controllers.yaml @@ -0,0 +1,14 @@ +controller_names: + - xarm6_traj_controller + +xarm6_traj_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 diff --git a/src/custom_xarm_moveit_config/config/xarm6/fake_controllers.yaml b/src/custom_xarm_moveit_config/config/xarm6/fake_controllers.yaml new file mode 100644 index 0000000..6dbb924 --- /dev/null +++ b/src/custom_xarm_moveit_config/config/xarm6/fake_controllers.yaml @@ -0,0 +1,14 @@ +controller_names: + - xarm6_traj_controller + +xarm6_traj_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 diff --git a/src/custom_xarm_moveit_config/config/xarm6/joint_limits.yaml b/src/custom_xarm_moveit_config/config/xarm6/joint_limits.yaml new file mode 100755 index 0000000..ee294c8 --- /dev/null +++ b/src/custom_xarm_moveit_config/config/xarm6/joint_limits.yaml @@ -0,0 +1,34 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed +# 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: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0.0 + joint2: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0.0 + joint3: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0.0 + joint4: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0.0 + joint5: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0.0 + joint6: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0.0 diff --git a/src/custom_xarm_moveit_config/config/xarm6/kinematics.yaml b/src/custom_xarm_moveit_config/config/xarm6/kinematics.yaml new file mode 100755 index 0000000..01206c4 --- /dev/null +++ b/src/custom_xarm_moveit_config/config/xarm6/kinematics.yaml @@ -0,0 +1,6 @@ +xarm6: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + # kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 diff --git a/src/custom_xarm_moveit_config/config/xarm6/ompl_planning.yaml b/src/custom_xarm_moveit_config/config/xarm6/ompl_planning.yaml new file mode 100755 index 0000000..4cbcf0c --- /dev/null +++ b/src/custom_xarm_moveit_config/config/xarm6/ompl_planning.yaml @@ -0,0 +1,149 @@ +planner_configs: + SBL: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECE: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECE: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECE: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRT: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRT: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMT: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDST: + type: geometric::PDST + STRIDE: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRT: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRT: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwo: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 + +xarm6: + default_planner_config: RRT + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo diff --git a/src/custom_xarm_moveit_config/config/xarm7/controllers.yaml b/src/custom_xarm_moveit_config/config/xarm7/controllers.yaml new file mode 100644 index 0000000..0046b4c --- /dev/null +++ b/src/custom_xarm_moveit_config/config/xarm7/controllers.yaml @@ -0,0 +1,15 @@ +controller_names: + - xarm7_traj_controller + +xarm7_traj_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + - joint7 diff --git a/src/custom_xarm_moveit_config/config/xarm7/fake_controllers.yaml b/src/custom_xarm_moveit_config/config/xarm7/fake_controllers.yaml new file mode 100644 index 0000000..0046b4c --- /dev/null +++ b/src/custom_xarm_moveit_config/config/xarm7/fake_controllers.yaml @@ -0,0 +1,15 @@ +controller_names: + - xarm7_traj_controller + +xarm7_traj_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + - joint7 diff --git a/src/custom_xarm_moveit_config/config/xarm7/joint_limits.yaml b/src/custom_xarm_moveit_config/config/xarm7/joint_limits.yaml new file mode 100755 index 0000000..bce1e6e --- /dev/null +++ b/src/custom_xarm_moveit_config/config/xarm7/joint_limits.yaml @@ -0,0 +1,39 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed +# 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: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0.0 + joint2: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0.0 + joint3: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0.0 + joint4: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0.0 + joint5: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0.0 + joint6: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0.0 + joint7: + has_velocity_limits: true + max_velocity: 2.14 + has_acceleration_limits: false + max_acceleration: 0.0 diff --git a/src/custom_xarm_moveit_config/config/xarm7/kinematics.yaml b/src/custom_xarm_moveit_config/config/xarm7/kinematics.yaml new file mode 100755 index 0000000..8780f21 --- /dev/null +++ b/src/custom_xarm_moveit_config/config/xarm7/kinematics.yaml @@ -0,0 +1,6 @@ +xarm7: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + # kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 \ No newline at end of file diff --git a/src/custom_xarm_moveit_config/config/xarm7/ompl_planning.yaml b/src/custom_xarm_moveit_config/config/xarm7/ompl_planning.yaml new file mode 100755 index 0000000..c38cda6 --- /dev/null +++ b/src/custom_xarm_moveit_config/config/xarm7/ompl_planning.yaml @@ -0,0 +1,148 @@ +planner_configs: + SBL: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECE: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECE: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECE: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRT: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRT: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMT: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDST: + type: geometric::PDST + STRIDE: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRT: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRT: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwo: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 + +xarm7: + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo diff --git a/src/custom_xarm_moveit_config/config/xarm_gripper/controllers.yaml b/src/custom_xarm_moveit_config/config/xarm_gripper/controllers.yaml new file mode 100644 index 0000000..24d2abd --- /dev/null +++ b/src/custom_xarm_moveit_config/config/xarm_gripper/controllers.yaml @@ -0,0 +1,9 @@ +controller_names: + - xarm_gripper + +xarm_gripper: + action_ns: gripper_action + type: GripperCommand + default: true + joints: + - drive_joint diff --git a/src/custom_xarm_moveit_config/config/xarm_gripper/fake_controllers.yaml b/src/custom_xarm_moveit_config/config/xarm_gripper/fake_controllers.yaml new file mode 100644 index 0000000..2221a3b --- /dev/null +++ b/src/custom_xarm_moveit_config/config/xarm_gripper/fake_controllers.yaml @@ -0,0 +1,9 @@ +controller_names: + - xarm_gripper_traj_controller + +xarm_gripper_traj_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - drive_joint diff --git a/src/custom_xarm_moveit_config/config/xarm_gripper/joint_limits.yaml b/src/custom_xarm_moveit_config/config/xarm_gripper/joint_limits.yaml new file mode 100755 index 0000000..34631ab --- /dev/null +++ b/src/custom_xarm_moveit_config/config/xarm_gripper/joint_limits.yaml @@ -0,0 +1,34 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed +# 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: + drive_joint: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 10.0 + left_finger_joint: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 10.0 + left_inner_knuckle_joint: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 10.0 + right_finger_joint: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 10.0 + right_inner_knuckle_joint: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 10.0 + right_outer_knuckle_joint: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 10.0 \ No newline at end of file diff --git a/src/custom_xarm_moveit_config/config/xarm_gripper/ompl_planning.yaml b/src/custom_xarm_moveit_config/config/xarm_gripper/ompl_planning.yaml new file mode 100755 index 0000000..4c2ba18 --- /dev/null +++ b/src/custom_xarm_moveit_config/config/xarm_gripper/ompl_planning.yaml @@ -0,0 +1,26 @@ +xarm_gripper: + default_planner_config: RRTConnect + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo \ No newline at end of file diff --git a/src/custom_xarm_moveit_config/launch/_dual_robot_moveit_common.launch.py b/src/custom_xarm_moveit_config/launch/_dual_robot_moveit_common.launch.py new file mode 100644 index 0000000..5b58a18 --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/_dual_robot_moveit_common.launch.py @@ -0,0 +1,355 @@ +#!/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 +from launch.launch_description_sources import load_python_launch_file_as_module +from launch import LaunchDescription +from launch.actions import OpaqueFunction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.actions import RegisterEventHandler, EmitEvent +from launch.event_handlers import OnProcessExit +from launch.events import Shutdown + + +def launch_setup(context, *args, **kwargs): + prefix_1 = LaunchConfiguration('prefix_1', default='L_') + prefix_2 = LaunchConfiguration('prefix_2', default='R_') + dof = LaunchConfiguration('dof', default=7) + dof_1 = LaunchConfiguration('dof_1', default=dof) + dof_2 = LaunchConfiguration('dof_2', default=dof) + robot_type = LaunchConfiguration('robot_type', default='xarm') + robot_type_1 = LaunchConfiguration('robot_type_1', default=robot_type) + robot_type_2 = LaunchConfiguration('robot_type_2', default=robot_type) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_gripper_1 = LaunchConfiguration('add_gripper_1', default=add_gripper) + add_gripper_2 = LaunchConfiguration('add_gripper_2', default=add_gripper) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + add_vacuum_gripper_1 = LaunchConfiguration('add_vacuum_gripper_1', default=add_vacuum_gripper) + add_vacuum_gripper_2 = LaunchConfiguration('add_vacuum_gripper_2', default=add_vacuum_gripper) + 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) + no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False) + ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='uf_robot_hardware/UFRobotFakeSystemHardware') + controllers_name = LaunchConfiguration('controllers_name', default='fake_controllers') + 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') + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + add_realsense_d435i_1 = LaunchConfiguration('add_realsense_d435i_1', default=add_realsense_d435i) + add_realsense_d435i_2 = LaunchConfiguration('add_realsense_d435i_2', default=add_realsense_d435i) + + add_other_geometry = LaunchConfiguration('add_other_geometry', default=False) + add_other_geometry_1 = LaunchConfiguration('add_other_geometry_1', default=add_other_geometry) + add_other_geometry_2 = LaunchConfiguration('add_other_geometry_2', default=add_other_geometry) + + geometry_type = LaunchConfiguration('geometry_type', default='box') + geometry_type_1 = LaunchConfiguration('geometry_type_1', default=geometry_type) + geometry_type_2 = LaunchConfiguration('geometry_type_2', default=geometry_type) + + geometry_mass = LaunchConfiguration('geometry_mass', default=0.1) + geometry_mass_1 = LaunchConfiguration('geometry_mass_1', default=geometry_mass) + geometry_mass_2 = LaunchConfiguration('geometry_mass_2', default=geometry_mass) + + geometry_height = LaunchConfiguration('geometry_height', default=0.1) + geometry_height_1 = LaunchConfiguration('geometry_height_1', default=geometry_height) + geometry_height_2 = LaunchConfiguration('geometry_height_2', default=geometry_height) + + geometry_radius = LaunchConfiguration('geometry_radius', default=0.1) + geometry_radius_1 = LaunchConfiguration('geometry_radius_1', default=geometry_radius) + geometry_radius_2 = LaunchConfiguration('geometry_radius_2', default=geometry_radius) + + geometry_length = LaunchConfiguration('geometry_length', default=0.1) + geometry_length_1 = LaunchConfiguration('geometry_length_1', default=geometry_length) + geometry_length_2 = LaunchConfiguration('geometry_length_2', default=geometry_length) + + geometry_width = LaunchConfiguration('geometry_width', default=0.1) + geometry_width_1 = LaunchConfiguration('geometry_width_1', default=geometry_width) + geometry_width_2 = LaunchConfiguration('geometry_width_2', default=geometry_width) + + geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='') + geometry_mesh_filename_1 = LaunchConfiguration('geometry_mesh_filename_1', default=geometry_mesh_filename) + geometry_mesh_filename_2 = LaunchConfiguration('geometry_mesh_filename_2', default=geometry_mesh_filename) + + geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"') + geometry_mesh_origin_xyz_1 = LaunchConfiguration('geometry_mesh_origin_xyz_1', default=geometry_mesh_origin_xyz) + geometry_mesh_origin_xyz_2 = LaunchConfiguration('geometry_mesh_origin_xyz_2', default=geometry_mesh_origin_xyz) + + geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"') + geometry_mesh_origin_rpy_1 = LaunchConfiguration('geometry_mesh_origin_rpy_1', default=geometry_mesh_origin_rpy) + geometry_mesh_origin_rpy_2 = LaunchConfiguration('geometry_mesh_origin_rpy_2', default=geometry_mesh_origin_rpy) + + geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"') + geometry_mesh_tcp_xyz_1 = LaunchConfiguration('geometry_mesh_tcp_xyz_1', default=geometry_mesh_tcp_xyz) + geometry_mesh_tcp_xyz_2 = LaunchConfiguration('geometry_mesh_tcp_xyz_2', default=geometry_mesh_tcp_xyz) + + geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"') + geometry_mesh_tcp_rpy_1 = LaunchConfiguration('geometry_mesh_tcp_rpy_1', default=geometry_mesh_tcp_rpy) + geometry_mesh_tcp_rpy_2 = LaunchConfiguration('geometry_mesh_tcp_rpy_2', default=geometry_mesh_tcp_rpy) + + use_sim_time = LaunchConfiguration('use_sim_time', default=False) + + moveit_config_package_name = 'xarm_moveit_config' + xarm_type = '{}{}'.format(robot_type_1.perform(context), dof_1.perform(context)) + + # robot_description_parameters + # xarm_moveit_config/launch/lib/robot_moveit_config_lib.py + 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')) + get_xarm_robot_description_parameters = getattr(mod, 'get_xarm_robot_description_parameters') + robot_description_parameters = get_xarm_robot_description_parameters( + xacro_urdf_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'dual_xarm_device.urdf.xacro']), + xacro_srdf_file=PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'srdf', 'dual_xarm.srdf.xacro']), + urdf_arguments={ + 'prefix_1': prefix_1, + 'prefix_2': prefix_2, + 'dof_1': dof_1, + 'dof_2': dof_2, + 'robot_type_1': robot_type_1, + 'robot_type_2': robot_type_2, + 'add_gripper_1': add_gripper_1, + 'add_gripper_2': add_gripper_2, + 'add_vacuum_gripper_1': add_vacuum_gripper_1, + 'add_vacuum_gripper_2': add_vacuum_gripper_2, + 'hw_ns': hw_ns.perform(context).strip('/'), + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'ros2_control_plugin': ros2_control_plugin, + 'add_realsense_d435i_1': add_realsense_d435i_1, + 'add_realsense_d435i_2': add_realsense_d435i_2, + 'add_other_geometry_1': add_other_geometry_1, + 'add_other_geometry_2': add_other_geometry_2, + 'geometry_type_1': geometry_type_1, + 'geometry_type_2': geometry_type_2, + 'geometry_mass_1': geometry_mass_1, + 'geometry_mass_2': geometry_mass_2, + 'geometry_height_1': geometry_height_1, + 'geometry_height_2': geometry_height_2, + 'geometry_radius_1': geometry_radius_1, + 'geometry_radius_2': geometry_radius_2, + 'geometry_length_1': geometry_length_1, + 'geometry_length_2': geometry_length_2, + 'geometry_width_1': geometry_width_1, + 'geometry_width_2': geometry_width_2, + 'geometry_mesh_filename_1': geometry_mesh_filename_1, + 'geometry_mesh_filename_2': geometry_mesh_filename_2, + 'geometry_mesh_origin_xyz_1': geometry_mesh_origin_xyz_1, + 'geometry_mesh_origin_xyz_2': geometry_mesh_origin_xyz_2, + 'geometry_mesh_origin_rpy_1': geometry_mesh_origin_rpy_1, + 'geometry_mesh_origin_rpy_2': geometry_mesh_origin_rpy_2, + 'geometry_mesh_tcp_xyz_1': geometry_mesh_tcp_xyz_1, + 'geometry_mesh_tcp_xyz_2': geometry_mesh_tcp_xyz_2, + 'geometry_mesh_tcp_rpy_1': geometry_mesh_tcp_rpy_1, + 'geometry_mesh_tcp_rpy_2': geometry_mesh_tcp_rpy_2, + }, + srdf_arguments={ + 'prefix_1': prefix_1, + 'prefix_2': prefix_2, + 'dof_1': dof_1, + 'dof_2': dof_2, + 'robot_type_1': robot_type_1, + 'robot_type_2': robot_type_2, + 'add_gripper_1': add_gripper_1, + 'add_gripper_2': add_gripper_2, + 'add_vacuum_gripper_1': add_vacuum_gripper_1, + 'add_vacuum_gripper_2': add_vacuum_gripper_2, + 'add_other_geometry_1': add_other_geometry_1, + 'add_other_geometry_2': add_other_geometry_2, + }, + arguments={ + 'context': context, + 'xarm_type': xarm_type + } + ) + + load_yaml = getattr(mod, 'load_yaml') + + controllers_yaml_1 = load_yaml(moveit_config_package_name, 'config', xarm_type, '{}.yaml'.format(controllers_name.perform(context))) + ompl_planning_yaml_1 = load_yaml(moveit_config_package_name, 'config', xarm_type, 'ompl_planning.yaml') + kinematics_yaml_1 = load_yaml(moveit_config_package_name, 'config', xarm_type, 'kinematics.yaml') + joint_limits_yaml_1 = load_yaml(moveit_config_package_name, 'config', xarm_type, 'joint_limits.yaml') + + xarm_type = '{}{}'.format(robot_type_2.perform(context), dof_2.perform(context)) + controllers_yaml_2 = load_yaml(moveit_config_package_name, 'config', xarm_type, '{}.yaml'.format(controllers_name.perform(context))) + ompl_planning_yaml_2 = load_yaml(moveit_config_package_name, 'config', xarm_type, 'ompl_planning.yaml') + kinematics_yaml_2 = load_yaml(moveit_config_package_name, 'config', xarm_type, 'kinematics.yaml') + joint_limits_yaml_2 = load_yaml(moveit_config_package_name, 'config', xarm_type, 'joint_limits.yaml') + + + if add_gripper_1.perform(context) in ('True', 'true'): + gripper_controllers_yaml_1 = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type_1.perform(context)), '{}.yaml'.format(controllers_name.perform(context))) + gripper_ompl_planning_yaml_1 = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type_1.perform(context)), 'ompl_planning.yaml') + # gripper_kinematics_yaml_1 = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type_1.perform(context)), 'kinematics.yaml') + gripper_joint_limits_yaml_1 = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type_1.perform(context)), 'joint_limits.yaml') + + if gripper_controllers_yaml_1 and 'controller_names' in gripper_controllers_yaml_1: + for name in gripper_controllers_yaml_1['controller_names']: + if name not in gripper_controllers_yaml_1: + continue + if name not in controllers_yaml_1['controller_names']: + controllers_yaml_1['controller_names'].append(name) + controllers_yaml_1[name] = gripper_controllers_yaml_1[name] + if gripper_ompl_planning_yaml_1: + ompl_planning_yaml_1.update(gripper_ompl_planning_yaml_1) + # kinematics_yaml_1.update(gripper_kinematics_yaml_1) + if joint_limits_yaml_1 and gripper_joint_limits_yaml_1: + joint_limits_yaml_1['joint_limits'].update(gripper_joint_limits_yaml_1['joint_limits']) + + if add_gripper_2.perform(context) in ('True', 'true'): + gripper_controllers_yaml_2 = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type_2.perform(context)), '{}.yaml'.format(controllers_name.perform(context))) + gripper_ompl_planning_yaml_2 = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type_2.perform(context)), 'ompl_planning.yaml') + gripper_joint_limits_yaml_2 = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type_2.perform(context)), 'joint_limits.yaml') + + if gripper_controllers_yaml_2 and 'controller_names' in gripper_controllers_yaml_2: + for name in gripper_controllers_yaml_2['controller_names']: + if name not in gripper_controllers_yaml_2: + continue + if name not in controllers_yaml_2['controller_names']: + controllers_yaml_2['controller_names'].append(name) + controllers_yaml_2[name] = gripper_controllers_yaml_2[name] + if gripper_ompl_planning_yaml_2: + ompl_planning_yaml_2.update(gripper_ompl_planning_yaml_2) + if joint_limits_yaml_2 and gripper_joint_limits_yaml_2: + joint_limits_yaml_2['joint_limits'].update(gripper_joint_limits_yaml_2['joint_limits']) + + + add_prefix_to_moveit_params = getattr(mod, 'add_prefix_to_moveit_params') + add_prefix_to_moveit_params( + controllers_yaml=controllers_yaml_1, ompl_planning_yaml=ompl_planning_yaml_1, + kinematics_yaml=kinematics_yaml_1, joint_limits_yaml=joint_limits_yaml_1, + prefix=prefix_1.perform(context)) + add_prefix_to_moveit_params( + controllers_yaml=controllers_yaml_2, ompl_planning_yaml=ompl_planning_yaml_2, + kinematics_yaml=kinematics_yaml_2, joint_limits_yaml=joint_limits_yaml_2, + prefix=prefix_2.perform(context)) + controllers_yaml = {} + controllers_yaml.update(controllers_yaml_1) + controllers_yaml.update(controllers_yaml_2) + controllers_yaml['controller_names'].extend(controllers_yaml_1['controller_names']) + ompl_planning_yaml = {} + ompl_planning_yaml.update(ompl_planning_yaml_1) + ompl_planning_yaml.update(ompl_planning_yaml_2) + kinematics_yaml = {} + kinematics_yaml.update(kinematics_yaml_1) + kinematics_yaml.update(kinematics_yaml_2) + robot_description_parameters['robot_description_kinematics'] = kinematics_yaml + if 'robot_description_planning' in robot_description_parameters: + joint_limits_yaml = {'joint_limits': {}} + joint_limits_yaml['joint_limits'].update(joint_limits_yaml_1['joint_limits']) + joint_limits_yaml['joint_limits'].update(joint_limits_yaml_2['joint_limits']) + robot_description_parameters['robot_description_planning'] = joint_limits_yaml + + # Planning Configuration + ompl_planning_pipeline_config = { + 'move_group': { + '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['move_group'].update(ompl_planning_yaml) + # Moveit controllers Configuration + moveit_controllers = { + moveit_controller_manager_key.perform(context): controllers_yaml, + 'moveit_controller_manager': moveit_controller_manager_value.perform(context), + } + + # 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', + output='screen', + parameters=[ + robot_description_parameters, + ompl_planning_pipeline_config, + trajectory_execution, + plan_execution, + moveit_controllers, + planning_scene_monitor_parameters, + {'use_sim_time': use_sim_time}, + ], + ) + + # rviz with moveit configuration + # rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'config', xarm_type, 'planner.rviz' if no_gui_ctrl.perform(context) == 'true' else 'moveit.rviz']) + rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'rviz', 'dual_planner.rviz' if no_gui_ctrl.perform(context) == 'true' else 'dual_moveit.rviz']) + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file], + parameters=[ + robot_description_parameters, + ompl_planning_pipeline_config, + {'use_sim_time': use_sim_time}, + ], + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ] + ) + + # Static TF + static_tf = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='static_transform_publisher', + output='screen', + arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'ground', 'link_base'], + parameters=[{'use_sim_time': use_sim_time}], + ) + + return [ + RegisterEventHandler(event_handler=OnProcessExit( + target_action=rviz2_node, + on_exit=[EmitEvent(event=Shutdown())] + )), + rviz2_node, + static_tf, + move_group_node, + ] + + +def generate_launch_description(): + return LaunchDescription([ + OpaqueFunction(function=launch_setup) + ]) diff --git a/src/custom_xarm_moveit_config/launch/_dual_robot_moveit_fake.launch.py b/src/custom_xarm_moveit_config/launch/_dual_robot_moveit_fake.launch.py new file mode 100644 index 0000000..3ce4069 --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/_dual_robot_moveit_fake.launch.py @@ -0,0 +1,300 @@ +#!/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 +from launch.launch_description_sources import load_python_launch_file_as_module +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 launch_ros.actions import Node + + +def launch_setup(context, *args, **kwargs): + prefix_1 = LaunchConfiguration('prefix_1', default='L_') + prefix_2 = LaunchConfiguration('prefix_2', default='R_') + dof = LaunchConfiguration('dof', default=7) + dof_1 = LaunchConfiguration('dof_1', default=dof) + dof_2 = LaunchConfiguration('dof_2', default=dof) + robot_type = LaunchConfiguration('robot_type', default='xarm') + robot_type_1 = LaunchConfiguration('robot_type_1', default=robot_type) + robot_type_2 = LaunchConfiguration('robot_type_2', default=robot_type) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_gripper_1 = LaunchConfiguration('add_gripper_1', default=add_gripper) + add_gripper_2 = LaunchConfiguration('add_gripper_2', default=add_gripper) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + add_vacuum_gripper_1 = LaunchConfiguration('add_vacuum_gripper_1', default=add_vacuum_gripper) + add_vacuum_gripper_2 = LaunchConfiguration('add_vacuum_gripper_2', default=add_vacuum_gripper) + 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) + no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False) + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + add_realsense_d435i_1 = LaunchConfiguration('add_realsense_d435i_1', default=add_realsense_d435i) + add_realsense_d435i_2 = LaunchConfiguration('add_realsense_d435i_2', default=add_realsense_d435i) + + add_other_geometry = LaunchConfiguration('add_other_geometry', default=False) + add_other_geometry_1 = LaunchConfiguration('add_other_geometry_1', default=add_other_geometry) + add_other_geometry_2 = LaunchConfiguration('add_other_geometry_2', default=add_other_geometry) + + geometry_type = LaunchConfiguration('geometry_type', default='box') + geometry_type_1 = LaunchConfiguration('geometry_type_1', default=geometry_type) + geometry_type_2 = LaunchConfiguration('geometry_type_2', default=geometry_type) + + geometry_mass = LaunchConfiguration('geometry_mass', default=0.1) + geometry_mass_1 = LaunchConfiguration('geometry_mass_1', default=geometry_mass) + geometry_mass_2 = LaunchConfiguration('geometry_mass_2', default=geometry_mass) + + geometry_height = LaunchConfiguration('geometry_height', default=0.1) + geometry_height_1 = LaunchConfiguration('geometry_height_1', default=geometry_height) + geometry_height_2 = LaunchConfiguration('geometry_height_2', default=geometry_height) + + geometry_radius = LaunchConfiguration('geometry_radius', default=0.1) + geometry_radius_1 = LaunchConfiguration('geometry_radius_1', default=geometry_radius) + geometry_radius_2 = LaunchConfiguration('geometry_radius_2', default=geometry_radius) + + geometry_length = LaunchConfiguration('geometry_length', default=0.1) + geometry_length_1 = LaunchConfiguration('geometry_length_1', default=geometry_length) + geometry_length_2 = LaunchConfiguration('geometry_length_2', default=geometry_length) + + geometry_width = LaunchConfiguration('geometry_width', default=0.1) + geometry_width_1 = LaunchConfiguration('geometry_width_1', default=geometry_width) + geometry_width_2 = LaunchConfiguration('geometry_width_2', default=geometry_width) + + geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='') + geometry_mesh_filename_1 = LaunchConfiguration('geometry_mesh_filename_1', default=geometry_mesh_filename) + geometry_mesh_filename_2 = LaunchConfiguration('geometry_mesh_filename_2', default=geometry_mesh_filename) + + geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"') + geometry_mesh_origin_xyz_1 = LaunchConfiguration('geometry_mesh_origin_xyz_1', default=geometry_mesh_origin_xyz) + geometry_mesh_origin_xyz_2 = LaunchConfiguration('geometry_mesh_origin_xyz_2', default=geometry_mesh_origin_xyz) + + geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"') + geometry_mesh_origin_rpy_1 = LaunchConfiguration('geometry_mesh_origin_rpy_1', default=geometry_mesh_origin_rpy) + geometry_mesh_origin_rpy_2 = LaunchConfiguration('geometry_mesh_origin_rpy_2', default=geometry_mesh_origin_rpy) + + geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"') + geometry_mesh_tcp_xyz_1 = LaunchConfiguration('geometry_mesh_tcp_xyz_1', default=geometry_mesh_tcp_xyz) + geometry_mesh_tcp_xyz_2 = LaunchConfiguration('geometry_mesh_tcp_xyz_2', default=geometry_mesh_tcp_xyz) + + geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"') + geometry_mesh_tcp_rpy_1 = LaunchConfiguration('geometry_mesh_tcp_rpy_1', default=geometry_mesh_tcp_rpy) + geometry_mesh_tcp_rpy_2 = LaunchConfiguration('geometry_mesh_tcp_rpy_2', default=geometry_mesh_tcp_rpy) + + 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' + ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context) + + # robot_description + # xarm_description/launch/lib/robot_description_lib.py + xacro_file = PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'dual_xarm_device.urdf.xacro']) + 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=xacro_file, + arguments={ + 'prefix_1': prefix_1, + 'prefix_2': prefix_2, + 'dof_1': dof_1, + 'dof_2': dof_2, + 'robot_type_1': robot_type_1, + 'robot_type_2': robot_type_2, + 'add_gripper_1': add_gripper_1, + 'add_gripper_2': add_gripper_2, + 'add_vacuum_gripper_1': add_vacuum_gripper_1, + 'add_vacuum_gripper_2': add_vacuum_gripper_2, + 'hw_ns': hw_ns.perform(context).strip('/'), + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'ros2_control_plugin': ros2_control_plugin, + 'add_realsense_d435i_1': add_realsense_d435i_1, + 'add_realsense_d435i_2': add_realsense_d435i_2, + 'add_other_geometry_1': add_other_geometry_1, + 'add_other_geometry_2': add_other_geometry_2, + 'geometry_type_1': geometry_type_1, + 'geometry_type_2': geometry_type_2, + 'geometry_mass_1': geometry_mass_1, + 'geometry_mass_2': geometry_mass_2, + 'geometry_height_1': geometry_height_1, + 'geometry_height_2': geometry_height_2, + 'geometry_radius_1': geometry_radius_1, + 'geometry_radius_2': geometry_radius_2, + 'geometry_length_1': geometry_length_1, + 'geometry_length_2': geometry_length_2, + 'geometry_width_1': geometry_width_1, + 'geometry_width_2': geometry_width_2, + 'geometry_mesh_filename_1': geometry_mesh_filename_1, + 'geometry_mesh_filename_2': geometry_mesh_filename_2, + 'geometry_mesh_origin_xyz_1': geometry_mesh_origin_xyz_1, + 'geometry_mesh_origin_xyz_2': geometry_mesh_origin_xyz_2, + 'geometry_mesh_origin_rpy_1': geometry_mesh_origin_rpy_1, + 'geometry_mesh_origin_rpy_2': geometry_mesh_origin_rpy_2, + 'geometry_mesh_tcp_xyz_1': geometry_mesh_tcp_xyz_1, + 'geometry_mesh_tcp_xyz_2': geometry_mesh_tcp_xyz_2, + 'geometry_mesh_tcp_rpy_1': geometry_mesh_tcp_rpy_1, + 'geometry_mesh_tcp_rpy_2': geometry_mesh_tcp_rpy_2, + } + ) + } + + # robot state publisher node + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + parameters=[robot_description], + remappings=[ + # ('joint_states', joint_states_remapping), + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ] + ) + + # robot moveit common launch + # xarm_moveit_config/launch/_dual_robot_moveit_common.launch.py + robot_moveit_common_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_dual_robot_moveit_common.launch.py'])), + launch_arguments={ + 'prefix_1': prefix_1, + 'prefix_2': prefix_2, + 'dof_1': dof_1, + 'dof_2': dof_2, + 'robot_type_1': robot_type_1, + 'robot_type_2': robot_type_2, + 'add_gripper_1': add_gripper_1, + 'add_gripper_2': add_gripper_2, + # 'add_gripper_1': add_gripper_1 if robot_type_1.perform(context) == 'xarm' else 'false', + # 'add_gripper_2': add_gripper_2 if robot_type_2.perform(context) == 'xarm' else 'false', + 'add_vacuum_gripper_1': add_vacuum_gripper_1, + 'add_vacuum_gripper_2': add_vacuum_gripper_2, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + '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, + 'add_realsense_d435i_1': add_realsense_d435i_1, + 'add_realsense_d435i_2': add_realsense_d435i_2, + 'add_other_geometry_1': add_other_geometry_1, + 'add_other_geometry_2': add_other_geometry_2, + 'geometry_type_1': geometry_type_1, + 'geometry_type_2': geometry_type_2, + 'geometry_mass_1': geometry_mass_1, + 'geometry_mass_2': geometry_mass_2, + 'geometry_height_1': geometry_height_1, + 'geometry_height_2': geometry_height_2, + 'geometry_radius_1': geometry_radius_1, + 'geometry_radius_2': geometry_radius_2, + 'geometry_length_1': geometry_length_1, + 'geometry_length_2': geometry_length_2, + 'geometry_width_1': geometry_width_1, + 'geometry_width_2': geometry_width_2, + 'geometry_mesh_filename_1': geometry_mesh_filename_1, + 'geometry_mesh_filename_2': geometry_mesh_filename_2, + 'geometry_mesh_origin_xyz_1': geometry_mesh_origin_xyz_1, + 'geometry_mesh_origin_xyz_2': geometry_mesh_origin_xyz_2, + 'geometry_mesh_origin_rpy_1': geometry_mesh_origin_rpy_1, + 'geometry_mesh_origin_rpy_2': geometry_mesh_origin_rpy_2, + 'geometry_mesh_tcp_xyz_1': geometry_mesh_tcp_xyz_1, + 'geometry_mesh_tcp_xyz_2': geometry_mesh_tcp_xyz_2, + 'geometry_mesh_tcp_rpy_1': geometry_mesh_tcp_rpy_1, + 'geometry_mesh_tcp_rpy_2': geometry_mesh_tcp_rpy_2, + }.items(), + ) + + remappings = [ + ('follow_joint_trajectory', '{}{}{}_traj_controller/follow_joint_trajectory'.format(prefix_1.perform(context), robot_type_1.perform(context), dof_1.perform(context))), + ('follow_joint_trajectory', '{}{}{}_traj_controller/follow_joint_trajectory'.format(prefix_2.perform(context), robot_type_2.perform(context), dof_2.perform(context))), + ] + controllers = [ + '{}{}{}_traj_controller'.format(prefix_1.perform(context), robot_type_1.perform(context), dof_1.perform(context)), + '{}{}{}_traj_controller'.format(prefix_2.perform(context), robot_type_2.perform(context), dof_2.perform(context)), + ] + if add_gripper_1.perform(context) in ('True', 'true') and robot_type_1.perform(context) == 'xarm': + remappings.append( + ('follow_joint_trajectory', '{}xarm_gripper_traj_controller/follow_joint_trajectory'.format(prefix_1.perform(context))) + ) + controllers.append('{}xarm_gripper_traj_controller'.format(prefix_1.perform(context))) + if add_gripper_2.perform(context) in ('True', 'true') and robot_type_2.perform(context) == 'xarm': + remappings.append( + ('follow_joint_trajectory', '{}xarm_gripper_traj_controller/follow_joint_trajectory'.format(prefix_2.perform(context))) + ) + controllers.append('{}xarm_gripper_traj_controller'.format(prefix_2.perform(context))) + # joint state publisher node + joint_state_publisher_node = Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + output='screen', + parameters=[{'source_list': ['joint_states']}], + remappings=remappings, + ) + + # ros2 control launch + # xarm_controller/launch/_dual_ros2_control.launch.py + ros2_control_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_controller'), 'launch', '_dual_ros2_control.launch.py'])), + launch_arguments={ + 'prefix_1': prefix_1, + 'prefix_2': prefix_2, + 'dof': dof, + 'dof_1': dof_1, + 'dof_2': dof_2, + 'robot_type_1': robot_type_1, + 'robot_type_2': robot_type_2, + 'add_gripper': add_gripper, + 'add_gripper_1': add_gripper_1, + 'add_gripper_2': add_gripper_2, + 'add_vacuum_gripper': add_vacuum_gripper, + 'add_vacuum_gripper_1': add_vacuum_gripper_1, + 'add_vacuum_gripper_2': add_vacuum_gripper_2, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'ros2_control_plugin': ros2_control_plugin, + }.items(), + ) + + # Load controllers + load_controllers = [] + for controller in controllers: + load_controllers.append(Node( + package='controller_manager', + executable='spawner', + output='screen', + arguments=[ + controller, + '--controller-manager', '{}/controller_manager'.format(ros_namespace) + ], + )) + + + return [ + robot_state_publisher_node, + robot_moveit_common_launch, + joint_state_publisher_node, + ros2_control_launch, + ] + load_controllers + + +def generate_launch_description(): + return LaunchDescription([ + OpaqueFunction(function=launch_setup) + ]) diff --git a/src/custom_xarm_moveit_config/launch/_dual_robot_moveit_gazebo.launch.py b/src/custom_xarm_moveit_config/launch/_dual_robot_moveit_gazebo.launch.py new file mode 100644 index 0000000..31406e5 --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/_dual_robot_moveit_gazebo.launch.py @@ -0,0 +1,212 @@ +#!/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.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node + + +def launch_setup(context, *args, **kwargs): + prefix_1 = LaunchConfiguration('prefix_1', default='L_') + prefix_2 = LaunchConfiguration('prefix_2', default='R_') + dof = LaunchConfiguration('dof', default=7) + dof_1 = LaunchConfiguration('dof_1', default=dof) + dof_2 = LaunchConfiguration('dof_2', default=dof) + robot_type = LaunchConfiguration('robot_type', default='xarm') + robot_type_1 = LaunchConfiguration('robot_type_1', default=robot_type) + robot_type_2 = LaunchConfiguration('robot_type_2', default=robot_type) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_gripper_1 = LaunchConfiguration('add_gripper_1', default=add_gripper) + add_gripper_2 = LaunchConfiguration('add_gripper_2', default=add_gripper) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + add_vacuum_gripper_1 = LaunchConfiguration('add_vacuum_gripper_1', default=add_vacuum_gripper) + add_vacuum_gripper_2 = LaunchConfiguration('add_vacuum_gripper_2', default=add_vacuum_gripper) + 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) + no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False) + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + add_realsense_d435i_1 = LaunchConfiguration('add_realsense_d435i_1', default=add_realsense_d435i) + add_realsense_d435i_2 = LaunchConfiguration('add_realsense_d435i_2', default=add_realsense_d435i) + + add_other_geometry = LaunchConfiguration('add_other_geometry', default=False) + add_other_geometry_1 = LaunchConfiguration('add_other_geometry_1', default=add_other_geometry) + add_other_geometry_2 = LaunchConfiguration('add_other_geometry_2', default=add_other_geometry) + + geometry_type = LaunchConfiguration('geometry_type', default='box') + geometry_type_1 = LaunchConfiguration('geometry_type_1', default=geometry_type) + geometry_type_2 = LaunchConfiguration('geometry_type_2', default=geometry_type) + + geometry_mass = LaunchConfiguration('geometry_mass', default=0.1) + geometry_mass_1 = LaunchConfiguration('geometry_mass_1', default=geometry_mass) + geometry_mass_2 = LaunchConfiguration('geometry_mass_2', default=geometry_mass) + + geometry_height = LaunchConfiguration('geometry_height', default=0.1) + geometry_height_1 = LaunchConfiguration('geometry_height_1', default=geometry_height) + geometry_height_2 = LaunchConfiguration('geometry_height_2', default=geometry_height) + + geometry_radius = LaunchConfiguration('geometry_radius', default=0.1) + geometry_radius_1 = LaunchConfiguration('geometry_radius_1', default=geometry_radius) + geometry_radius_2 = LaunchConfiguration('geometry_radius_2', default=geometry_radius) + + geometry_length = LaunchConfiguration('geometry_length', default=0.1) + geometry_length_1 = LaunchConfiguration('geometry_length_1', default=geometry_length) + geometry_length_2 = LaunchConfiguration('geometry_length_2', default=geometry_length) + + geometry_width = LaunchConfiguration('geometry_width', default=0.1) + geometry_width_1 = LaunchConfiguration('geometry_width_1', default=geometry_width) + geometry_width_2 = LaunchConfiguration('geometry_width_2', default=geometry_width) + + geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='') + geometry_mesh_filename_1 = LaunchConfiguration('geometry_mesh_filename_1', default=geometry_mesh_filename) + geometry_mesh_filename_2 = LaunchConfiguration('geometry_mesh_filename_2', default=geometry_mesh_filename) + + geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"') + geometry_mesh_origin_xyz_1 = LaunchConfiguration('geometry_mesh_origin_xyz_1', default=geometry_mesh_origin_xyz) + geometry_mesh_origin_xyz_2 = LaunchConfiguration('geometry_mesh_origin_xyz_2', default=geometry_mesh_origin_xyz) + + geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"') + geometry_mesh_origin_rpy_1 = LaunchConfiguration('geometry_mesh_origin_rpy_1', default=geometry_mesh_origin_rpy) + geometry_mesh_origin_rpy_2 = LaunchConfiguration('geometry_mesh_origin_rpy_2', default=geometry_mesh_origin_rpy) + + geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"') + geometry_mesh_tcp_xyz_1 = LaunchConfiguration('geometry_mesh_tcp_xyz_1', default=geometry_mesh_tcp_xyz) + geometry_mesh_tcp_xyz_2 = LaunchConfiguration('geometry_mesh_tcp_xyz_2', default=geometry_mesh_tcp_xyz) + + geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"') + geometry_mesh_tcp_rpy_1 = LaunchConfiguration('geometry_mesh_tcp_rpy_1', default=geometry_mesh_tcp_rpy) + geometry_mesh_tcp_rpy_2 = LaunchConfiguration('geometry_mesh_tcp_rpy_2', default=geometry_mesh_tcp_rpy) + + ros2_control_plugin = 'gazebo_ros2_control/GazeboSystem' + controllers_name = 'fake_controllers' + moveit_controller_manager_key = 'moveit_simple_controller_manager' + moveit_controller_manager_value = 'moveit_simple_controller_manager/MoveItSimpleControllerManager' + + # robot moveit common launch + # xarm_moveit_config/launch/_dual_robot_moveit_common.launch.py + robot_moveit_common_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_dual_robot_moveit_common.launch.py'])), + launch_arguments={ + 'prefix_1': prefix_1, + 'prefix_2': prefix_2, + 'dof_1': dof_1, + 'dof_2': dof_2, + 'robot_type_1': robot_type_1, + 'robot_type_2': robot_type_2, + 'add_gripper_1': add_gripper_1, + 'add_gripper_2': add_gripper_2, + # 'add_gripper_1': add_gripper_1 if robot_type_1.perform(context) == 'xarm' else 'false', + # 'add_gripper_2': add_gripper_2 if robot_type_2.perform(context) == 'xarm' else 'false', + 'add_vacuum_gripper_1': add_vacuum_gripper_1, + 'add_vacuum_gripper_2': add_vacuum_gripper_2, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + '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, + 'add_realsense_d435i_1': add_realsense_d435i_1, + 'add_realsense_d435i_2': add_realsense_d435i_2, + 'add_other_geometry_1': add_other_geometry_1, + 'add_other_geometry_2': add_other_geometry_2, + 'geometry_type_1': geometry_type_1, + 'geometry_type_2': geometry_type_2, + 'geometry_mass_1': geometry_mass_1, + 'geometry_mass_2': geometry_mass_2, + 'geometry_height_1': geometry_height_1, + 'geometry_height_2': geometry_height_2, + 'geometry_radius_1': geometry_radius_1, + 'geometry_radius_2': geometry_radius_2, + 'geometry_length_1': geometry_length_1, + 'geometry_length_2': geometry_length_2, + 'geometry_width_1': geometry_width_1, + 'geometry_width_2': geometry_width_2, + 'geometry_mesh_filename_1': geometry_mesh_filename_1, + 'geometry_mesh_filename_2': geometry_mesh_filename_2, + 'geometry_mesh_origin_xyz_1': geometry_mesh_origin_xyz_1, + 'geometry_mesh_origin_xyz_2': geometry_mesh_origin_xyz_2, + 'geometry_mesh_origin_rpy_1': geometry_mesh_origin_rpy_1, + 'geometry_mesh_origin_rpy_2': geometry_mesh_origin_rpy_2, + 'geometry_mesh_tcp_xyz_1': geometry_mesh_tcp_xyz_1, + 'geometry_mesh_tcp_xyz_2': geometry_mesh_tcp_xyz_2, + 'geometry_mesh_tcp_rpy_1': geometry_mesh_tcp_rpy_1, + 'geometry_mesh_tcp_rpy_2': geometry_mesh_tcp_rpy_2, + 'use_sim_time': 'true' + }.items(), + ) + + # robot gazebo launch + # xarm_gazebo/launch/_dual_robot_beside_table_gazebo.launch.py + robot_gazebo_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_gazebo'), 'launch', '_dual_robot_beside_table_gazebo.launch.py'])), + launch_arguments={ + 'prefix_1': prefix_1, + 'prefix_2': prefix_2, + 'dof_1': dof_1, + 'dof_2': dof_2, + 'robot_type_1': robot_type_1, + 'robot_type_2': robot_type_2, + 'add_gripper_1': add_gripper_1, + 'add_gripper_2': add_gripper_2, + 'add_vacuum_gripper_1': add_vacuum_gripper_1, + 'add_vacuum_gripper_2': add_vacuum_gripper_2, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'no_gui_ctrl': no_gui_ctrl, + 'ros2_control_plugin': ros2_control_plugin, + 'add_realsense_d435i_1': add_realsense_d435i_1, + 'add_realsense_d435i_2': add_realsense_d435i_2, + 'add_other_geometry_1': add_other_geometry_1, + 'add_other_geometry_2': add_other_geometry_2, + 'geometry_type_1': geometry_type_1, + 'geometry_type_2': geometry_type_2, + 'geometry_mass_1': geometry_mass_1, + 'geometry_mass_2': geometry_mass_2, + 'geometry_height_1': geometry_height_1, + 'geometry_height_2': geometry_height_2, + 'geometry_radius_1': geometry_radius_1, + 'geometry_radius_2': geometry_radius_2, + 'geometry_length_1': geometry_length_1, + 'geometry_length_2': geometry_length_2, + 'geometry_width_1': geometry_width_1, + 'geometry_width_2': geometry_width_2, + 'geometry_mesh_filename_1': geometry_mesh_filename_1, + 'geometry_mesh_filename_2': geometry_mesh_filename_2, + 'geometry_mesh_origin_xyz_1': geometry_mesh_origin_xyz_1, + 'geometry_mesh_origin_xyz_2': geometry_mesh_origin_xyz_2, + 'geometry_mesh_origin_rpy_1': geometry_mesh_origin_rpy_1, + 'geometry_mesh_origin_rpy_2': geometry_mesh_origin_rpy_2, + 'geometry_mesh_tcp_xyz_1': geometry_mesh_tcp_xyz_1, + 'geometry_mesh_tcp_xyz_2': geometry_mesh_tcp_xyz_2, + 'geometry_mesh_tcp_rpy_1': geometry_mesh_tcp_rpy_1, + 'geometry_mesh_tcp_rpy_2': geometry_mesh_tcp_rpy_2, + 'load_controller': 'true', + }.items(), + ) + + + return [ + robot_gazebo_launch, + robot_moveit_common_launch + ] + + +def generate_launch_description(): + return LaunchDescription([ + OpaqueFunction(function=launch_setup) + ]) diff --git a/src/custom_xarm_moveit_config/launch/_dual_robot_moveit_realmove.launch.py b/src/custom_xarm_moveit_config/launch/_dual_robot_moveit_realmove.launch.py new file mode 100644 index 0000000..3c5a175 --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/_dual_robot_moveit_realmove.launch.py @@ -0,0 +1,373 @@ +#!/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 +from launch.launch_description_sources import load_python_launch_file_as_module +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 launch_ros.actions import Node + + +def launch_setup(context, *args, **kwargs): + robot_ip_1 = LaunchConfiguration('robot_ip_1') + robot_ip_2 = LaunchConfiguration('robot_ip_2') + report_type = LaunchConfiguration('report_type', default='normal') + report_type_1 = LaunchConfiguration('report_type_1', default=report_type) + report_type_2 = LaunchConfiguration('report_type_2', default=report_type) + prefix_1 = LaunchConfiguration('prefix_1', default='L_') + prefix_2 = LaunchConfiguration('prefix_2', default='R_') + dof = LaunchConfiguration('dof', default=7) + dof_1 = LaunchConfiguration('dof_1', default=dof) + dof_2 = LaunchConfiguration('dof_2', default=dof) + robot_type = LaunchConfiguration('robot_type', default='xarm') + robot_type_1 = LaunchConfiguration('robot_type_1', default=robot_type) + robot_type_2 = LaunchConfiguration('robot_type_2', default=robot_type) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_gripper_1 = LaunchConfiguration('add_gripper_1', default=add_gripper) + add_gripper_2 = LaunchConfiguration('add_gripper_2', default=add_gripper) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + add_vacuum_gripper_1 = LaunchConfiguration('add_vacuum_gripper_1', default=add_vacuum_gripper) + add_vacuum_gripper_2 = LaunchConfiguration('add_vacuum_gripper_2', default=add_vacuum_gripper) + 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) + no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False) + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + add_realsense_d435i_1 = LaunchConfiguration('add_realsense_d435i_1', default=add_realsense_d435i) + add_realsense_d435i_2 = LaunchConfiguration('add_realsense_d435i_2', default=add_realsense_d435i) + + add_other_geometry = LaunchConfiguration('add_other_geometry', default=False) + add_other_geometry_1 = LaunchConfiguration('add_other_geometry_1', default=add_other_geometry) + add_other_geometry_2 = LaunchConfiguration('add_other_geometry_2', default=add_other_geometry) + + geometry_type = LaunchConfiguration('geometry_type', default='box') + geometry_type_1 = LaunchConfiguration('geometry_type_1', default=geometry_type) + geometry_type_2 = LaunchConfiguration('geometry_type_2', default=geometry_type) + + geometry_mass = LaunchConfiguration('geometry_mass', default=0.1) + geometry_mass_1 = LaunchConfiguration('geometry_mass_1', default=geometry_mass) + geometry_mass_2 = LaunchConfiguration('geometry_mass_2', default=geometry_mass) + + geometry_height = LaunchConfiguration('geometry_height', default=0.1) + geometry_height_1 = LaunchConfiguration('geometry_height_1', default=geometry_height) + geometry_height_2 = LaunchConfiguration('geometry_height_2', default=geometry_height) + + geometry_radius = LaunchConfiguration('geometry_radius', default=0.1) + geometry_radius_1 = LaunchConfiguration('geometry_radius_1', default=geometry_radius) + geometry_radius_2 = LaunchConfiguration('geometry_radius_2', default=geometry_radius) + + geometry_length = LaunchConfiguration('geometry_length', default=0.1) + geometry_length_1 = LaunchConfiguration('geometry_length_1', default=geometry_length) + geometry_length_2 = LaunchConfiguration('geometry_length_2', default=geometry_length) + + geometry_width = LaunchConfiguration('geometry_width', default=0.1) + geometry_width_1 = LaunchConfiguration('geometry_width_1', default=geometry_width) + geometry_width_2 = LaunchConfiguration('geometry_width_2', default=geometry_width) + + geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='') + geometry_mesh_filename_1 = LaunchConfiguration('geometry_mesh_filename_1', default=geometry_mesh_filename) + geometry_mesh_filename_2 = LaunchConfiguration('geometry_mesh_filename_2', default=geometry_mesh_filename) + + geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"') + geometry_mesh_origin_xyz_1 = LaunchConfiguration('geometry_mesh_origin_xyz_1', default=geometry_mesh_origin_xyz) + geometry_mesh_origin_xyz_2 = LaunchConfiguration('geometry_mesh_origin_xyz_2', default=geometry_mesh_origin_xyz) + + geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"') + geometry_mesh_origin_rpy_1 = LaunchConfiguration('geometry_mesh_origin_rpy_1', default=geometry_mesh_origin_rpy) + geometry_mesh_origin_rpy_2 = LaunchConfiguration('geometry_mesh_origin_rpy_2', default=geometry_mesh_origin_rpy) + + geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"') + geometry_mesh_tcp_xyz_1 = LaunchConfiguration('geometry_mesh_tcp_xyz_1', default=geometry_mesh_tcp_xyz) + geometry_mesh_tcp_xyz_2 = LaunchConfiguration('geometry_mesh_tcp_xyz_2', default=geometry_mesh_tcp_xyz) + + geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"') + geometry_mesh_tcp_rpy_1 = LaunchConfiguration('geometry_mesh_tcp_rpy_1', default=geometry_mesh_tcp_rpy) + geometry_mesh_tcp_rpy_2 = LaunchConfiguration('geometry_mesh_tcp_rpy_2', default=geometry_mesh_tcp_rpy) + + baud_checkset = LaunchConfiguration('baud_checkset', default=True) + baud_checkset_1 = LaunchConfiguration('baud_checkset', default=baud_checkset) + baud_checkset_2 = LaunchConfiguration('baud_checkset', default=baud_checkset) + default_gripper_baud = LaunchConfiguration('default_gripper_baud', default=2000000) + default_gripper_baud_1 = LaunchConfiguration('default_gripper_baud', default=default_gripper_baud) + default_gripper_baud_2 = LaunchConfiguration('default_gripper_baud', default=default_gripper_baud) + + ros2_control_plugin = 'uf_robot_hardware/UFRobotSystemHardware' + controllers_name = 'controllers' + moveit_controller_manager_key = 'moveit_simple_controller_manager' + moveit_controller_manager_value = 'moveit_simple_controller_manager/MoveItSimpleControllerManager' + ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context) + + # # robot driver launch + # # xarm_api/launch/_robot_driver.launch.py + # robot_driver_launch_1 = IncludeLaunchDescription( + # PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_api'), 'launch', '_robot_driver.launch.py'])), + # launch_arguments={ + # 'robot_ip': robot_ip_1, + # 'report_type': report_type, + # 'dof': dof_1, + # 'hw_ns': hw_ns, + # 'add_gripper': add_gripper_1, + # 'prefix': prefix_1, + # 'baud_checkset': baud_checkset_1, + # 'default_gripper_baud': default_gripper_baud_1, + # 'robot_type': robot_type_1, + # }.items(), + # ) + # # robot driver launch + # # xarm_api/launch/_robot_driver.launch.py + # robot_driver_launch_2 = IncludeLaunchDescription( + # PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_api'), 'launch', '_robot_driver.launch.py'])), + # launch_arguments={ + # 'robot_ip': robot_ip_2, + # 'report_type': report_type, + # 'dof': dof_2, + # 'hw_ns': hw_ns, + # 'add_gripper': add_gripper_2, + # 'prefix': prefix_2, + # 'baud_checkset': baud_checkset_2, + # 'default_gripper_baud': default_gripper_baud_2, + # 'robot_type': robot_type_2, + # }.items(), + # ) + + # robot_description + # xarm_description/launch/lib/robot_description_lib.py + xacro_file = PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'dual_xarm_device.urdf.xacro']) + 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=xacro_file, + arguments={ + 'prefix_1': prefix_1, + 'prefix_2': prefix_2, + 'dof_1': dof_1, + 'dof_2': dof_2, + 'robot_type_1': robot_type_1, + 'robot_type_2': robot_type_2, + 'add_gripper_1': add_gripper_1, + 'add_gripper_2': add_gripper_2, + 'add_vacuum_gripper_1': add_vacuum_gripper_1, + 'add_vacuum_gripper_2': add_vacuum_gripper_2, + 'hw_ns': hw_ns.perform(context).strip('/'), + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'ros2_control_plugin': ros2_control_plugin, + 'add_realsense_d435i_1': add_realsense_d435i_1, + 'add_realsense_d435i_2': add_realsense_d435i_2, + 'add_other_geometry_1': add_other_geometry_1, + 'add_other_geometry_2': add_other_geometry_2, + 'geometry_type_1': geometry_type_1, + 'geometry_type_2': geometry_type_2, + 'geometry_mass_1': geometry_mass_1, + 'geometry_mass_2': geometry_mass_2, + 'geometry_height_1': geometry_height_1, + 'geometry_height_2': geometry_height_2, + 'geometry_radius_1': geometry_radius_1, + 'geometry_radius_2': geometry_radius_2, + 'geometry_length_1': geometry_length_1, + 'geometry_length_2': geometry_length_2, + 'geometry_width_1': geometry_width_1, + 'geometry_width_2': geometry_width_2, + 'geometry_mesh_filename_1': geometry_mesh_filename_1, + 'geometry_mesh_filename_2': geometry_mesh_filename_2, + 'geometry_mesh_origin_xyz_1': geometry_mesh_origin_xyz_1, + 'geometry_mesh_origin_xyz_2': geometry_mesh_origin_xyz_2, + 'geometry_mesh_origin_rpy_1': geometry_mesh_origin_rpy_1, + 'geometry_mesh_origin_rpy_2': geometry_mesh_origin_rpy_2, + 'geometry_mesh_tcp_xyz_1': geometry_mesh_tcp_xyz_1, + 'geometry_mesh_tcp_xyz_2': geometry_mesh_tcp_xyz_2, + 'geometry_mesh_tcp_rpy_1': geometry_mesh_tcp_rpy_1, + 'geometry_mesh_tcp_rpy_2': geometry_mesh_tcp_rpy_2, + } + ) + } + + # robot state publisher node + robot_state_publisher_node = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output="screen", + parameters=[robot_description], + remappings=[ + # ('joint_states', joint_states_remapping), + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ] + ) + + # robot moveit common launch + # xarm_moveit_config/launch/_dual_robot_moveit_common.launch.py + robot_moveit_common_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_dual_robot_moveit_common.launch.py'])), + launch_arguments={ + 'prefix_1': prefix_1, + 'prefix_2': prefix_2, + 'dof_1': dof_1, + 'dof_2': dof_2, + 'robot_type_1': robot_type_1, + 'robot_type_2': robot_type_2, + 'add_gripper_1': add_gripper_1, + 'add_gripper_2': add_gripper_2, + # 'add_gripper_1': add_gripper_1 if robot_type_1.perform(context) == 'xarm' else 'false', + # 'add_gripper_2': add_gripper_2 if robot_type_2.perform(context) == 'xarm' else 'false', + 'add_vacuum_gripper_1': add_vacuum_gripper_1, + 'add_vacuum_gripper_2': add_vacuum_gripper_2, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + '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, + 'add_realsense_d435i_1': add_realsense_d435i_1, + 'add_realsense_d435i_2': add_realsense_d435i_2, + 'add_other_geometry_1': add_other_geometry_1, + 'add_other_geometry_2': add_other_geometry_2, + 'geometry_type_1': geometry_type_1, + 'geometry_type_2': geometry_type_2, + 'geometry_mass_1': geometry_mass_1, + 'geometry_mass_2': geometry_mass_2, + 'geometry_height_1': geometry_height_1, + 'geometry_height_2': geometry_height_2, + 'geometry_radius_1': geometry_radius_1, + 'geometry_radius_2': geometry_radius_2, + 'geometry_length_1': geometry_length_1, + 'geometry_length_2': geometry_length_2, + 'geometry_width_1': geometry_width_1, + 'geometry_width_2': geometry_width_2, + 'geometry_mesh_filename_1': geometry_mesh_filename_1, + 'geometry_mesh_filename_2': geometry_mesh_filename_2, + 'geometry_mesh_origin_xyz_1': geometry_mesh_origin_xyz_1, + 'geometry_mesh_origin_xyz_2': geometry_mesh_origin_xyz_2, + 'geometry_mesh_origin_rpy_1': geometry_mesh_origin_rpy_1, + 'geometry_mesh_origin_rpy_2': geometry_mesh_origin_rpy_2, + 'geometry_mesh_tcp_xyz_1': geometry_mesh_tcp_xyz_1, + 'geometry_mesh_tcp_xyz_2': geometry_mesh_tcp_xyz_2, + 'geometry_mesh_tcp_rpy_1': geometry_mesh_tcp_rpy_1, + 'geometry_mesh_tcp_rpy_2': geometry_mesh_tcp_rpy_2, + }.items(), + ) + + # joint state publisher node + joint_state_publisher_node = Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + output='screen', + parameters=[{ + 'source_list': [ + '{}{}/joint_states'.format(prefix_1.perform(context), hw_ns.perform(context)), + '{}{}/joint_states'.format(prefix_2.perform(context), hw_ns.perform(context)) + ], + }], + remappings=[ + ('follow_joint_trajectory', '{}{}{}_traj_controller/follow_joint_trajectory'.format(prefix_1.perform(context), robot_type_1.perform(context), dof_1.perform(context))), + ('follow_joint_trajectory', '{}{}{}_traj_controller/follow_joint_trajectory'.format(prefix_2.perform(context), robot_type_2.perform(context), dof_2.perform(context))), + ], + ) + + # ros2 control launch + # xarm_controller/launch/_dual_ros2_control.launch.py + ros2_control_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_controller'), 'launch', '_dual_ros2_control.launch.py'])), + launch_arguments={ + 'prefix_1': prefix_1, + 'prefix_2': prefix_2, + 'dof': dof, + 'dof_1': dof_1, + 'dof_2': dof_2, + 'robot_type_1': robot_type_1, + 'robot_type_2': robot_type_2, + 'add_gripper': add_gripper, + 'add_gripper_1': add_gripper_1, + 'add_gripper_2': add_gripper_2, + 'add_vacuum_gripper': add_vacuum_gripper, + 'add_vacuum_gripper_1': add_vacuum_gripper_1, + 'add_vacuum_gripper_2': add_vacuum_gripper_2, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'ros2_control_plugin': ros2_control_plugin, + 'add_realsense_d435i_1': add_realsense_d435i_1, + 'add_realsense_d435i_2': add_realsense_d435i_2, + 'add_other_geometry_1': add_other_geometry_1, + 'add_other_geometry_2': add_other_geometry_2, + 'geometry_type_1': geometry_type_1, + 'geometry_type_2': geometry_type_2, + 'geometry_mass_1': geometry_mass_1, + 'geometry_mass_2': geometry_mass_2, + 'geometry_height_1': geometry_height_1, + 'geometry_height_2': geometry_height_2, + 'geometry_radius_1': geometry_radius_1, + 'geometry_radius_2': geometry_radius_2, + 'geometry_length_1': geometry_length_1, + 'geometry_length_2': geometry_length_2, + 'geometry_width_1': geometry_width_1, + 'geometry_width_2': geometry_width_2, + 'geometry_mesh_filename_1': geometry_mesh_filename_1, + 'geometry_mesh_filename_2': geometry_mesh_filename_2, + 'geometry_mesh_origin_xyz_1': geometry_mesh_origin_xyz_1, + 'geometry_mesh_origin_xyz_2': geometry_mesh_origin_xyz_2, + 'geometry_mesh_origin_rpy_1': geometry_mesh_origin_rpy_1, + 'geometry_mesh_origin_rpy_2': geometry_mesh_origin_rpy_2, + 'geometry_mesh_tcp_xyz_1': geometry_mesh_tcp_xyz_1, + 'geometry_mesh_tcp_xyz_2': geometry_mesh_tcp_xyz_2, + 'geometry_mesh_tcp_rpy_1': geometry_mesh_tcp_rpy_1, + 'geometry_mesh_tcp_rpy_2': geometry_mesh_tcp_rpy_2, + 'robot_ip_1': robot_ip_1, + 'robot_ip_2': robot_ip_2, + 'report_type_1': report_type_1, + 'report_type_2': report_type_2, + 'baud_checkset_1': baud_checkset_1, + 'baud_checkset_2': baud_checkset_2, + 'default_gripper_baud_1': default_gripper_baud_1, + 'default_gripper_baud_2': default_gripper_baud_2, + }.items(), + ) + + # Load controllers + load_controllers = [] + for controller in [ + '{}{}{}_traj_controller'.format(prefix_1.perform(context), robot_type_1.perform(context), dof_1.perform(context)), + '{}{}{}_traj_controller'.format(prefix_2.perform(context), robot_type_2.perform(context), dof_2.perform(context)), + ]: + load_controllers.append(Node( + package='controller_manager', + executable='spawner', + output='screen', + arguments=[ + controller, + '--controller-manager', '{}/controller_manager'.format(ros_namespace) + ], + )) + + return [ + robot_state_publisher_node, + robot_moveit_common_launch, + joint_state_publisher_node, + ros2_control_launch, + # robot_driver_launch_1, + # robot_driver_launch_2, + ] + load_controllers + + +def generate_launch_description(): + return LaunchDescription([ + OpaqueFunction(function=launch_setup) + ]) diff --git a/src/custom_xarm_moveit_config/launch/_robot_moveit_common.launch.py b/src/custom_xarm_moveit_config/launch/_robot_moveit_common.launch.py new file mode 100644 index 0000000..734df13 --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/_robot_moveit_common.launch.py @@ -0,0 +1,237 @@ +#!/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 +from launch.launch_description_sources import load_python_launch_file_as_module +from launch import LaunchDescription +from launch.actions import OpaqueFunction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.actions import RegisterEventHandler, EmitEvent +from launch.event_handlers import OnProcessExit +from launch.events import Shutdown + + +def launch_setup(context, *args, **kwargs): + 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) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + dof = LaunchConfiguration('dof', default=7) + robot_type = LaunchConfiguration('robot_type', default='xarm') + no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False) + ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='uf_robot_hardware/UFRobotFakeSystemHardware') + controllers_name = LaunchConfiguration('controllers_name', default='fake_controllers') + 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') + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + 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)) + + # robot_description_parameters + # xarm_moveit_config/launch/lib/robot_moveit_config_lib.py + 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')) + get_xarm_robot_description_parameters = getattr(mod, 'get_xarm_robot_description_parameters') + robot_description_parameters = get_xarm_robot_description_parameters( + xacro_urdf_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']), + xacro_srdf_file=PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']), + urdf_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns.perform(context).strip('/'), + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': dof, + 'robot_type': robot_type, + 'ros2_control_plugin': ros2_control_plugin, + 'add_realsense_d435i': add_realsense_d435i, + '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, + }, + srdf_arguments={ + 'prefix': prefix, + 'dof': dof, + 'robot_type': robot_type, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'add_other_geometry': add_other_geometry, + }, + arguments={ + 'context': context, + 'xarm_type': xarm_type, + } + ) + + 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') + kinematics_yaml = robot_description_parameters['robot_description_kinematics'] + joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None) + + if add_gripper.perform(context) in ('True', 'true'): + gripper_controllers_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), '{}.yaml'.format(controllers_name.perform(context))) + gripper_ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), 'ompl_planning.yaml') + gripper_joint_limits_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), 'joint_limits.yaml') + + if gripper_controllers_yaml and 'controller_names' in gripper_controllers_yaml: + for name in gripper_controllers_yaml['controller_names']: + if name in gripper_controllers_yaml: + if name not in controllers_yaml['controller_names']: + controllers_yaml['controller_names'].append(name) + controllers_yaml[name] = gripper_controllers_yaml[name] + if gripper_ompl_planning_yaml: + ompl_planning_yaml.update(gripper_ompl_planning_yaml) + if joint_limits_yaml and gripper_joint_limits_yaml: + joint_limits_yaml['joint_limits'].update(gripper_joint_limits_yaml['joint_limits']) + + add_prefix_to_moveit_params = getattr(mod, 'add_prefix_to_moveit_params') + add_prefix_to_moveit_params( + controllers_yaml=controllers_yaml, ompl_planning_yaml=ompl_planning_yaml, + kinematics_yaml=kinematics_yaml, joint_limits_yaml=joint_limits_yaml, + prefix=prefix.perform(context)) + + # Planning Configuration + ompl_planning_pipeline_config = { + 'move_group': { + '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['move_group'].update(ompl_planning_yaml) + + # Moveit controllers Configuration + moveit_controllers = { + moveit_controller_manager_key.perform(context): controllers_yaml, + 'moveit_controller_manager': moveit_controller_manager_value.perform(context), + } + + # 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', + output='screen', + parameters=[ + robot_description_parameters, + ompl_planning_pipeline_config, + trajectory_execution, + plan_execution, + moveit_controllers, + planning_scene_monitor_parameters, + {'use_sim_time': use_sim_time}, + ], + ) + + # rviz with moveit configuration + # rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'config', xarm_type, 'planner.rviz' if no_gui_ctrl.perform(context) == 'true' else 'moveit.rviz']) + 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_parameters, + ompl_planning_pipeline_config, + {'use_sim_time': use_sim_time}, + ], + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ] + ) + + # Static TF + static_tf = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='static_transform_publisher', + output='screen', + arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'world', 'link_base'], + parameters=[{'use_sim_time': use_sim_time}], + ) + + return [ + RegisterEventHandler(event_handler=OnProcessExit( + target_action=rviz2_node, + on_exit=[EmitEvent(event=Shutdown())] + )), + rviz2_node, + static_tf, + move_group_node, + ] + + +def generate_launch_description(): + return LaunchDescription([ + OpaqueFunction(function=launch_setup) + ]) diff --git a/src/custom_xarm_moveit_config/launch/_robot_moveit_fake.launch.py b/src/custom_xarm_moveit_config/launch/_robot_moveit_fake.launch.py new file mode 100644 index 0000000..f1c38eb --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/_robot_moveit_fake.launch.py @@ -0,0 +1,193 @@ +#!/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.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node + + +def launch_setup(context, *args, **kwargs): + 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) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + dof = LaunchConfiguration('dof', default=7) + robot_type = LaunchConfiguration('robot_type', default='xarm') + no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False) + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + 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' + xarm_type = '{}{}'.format(robot_type.perform(context), dof.perform(context)) + ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context) + + # 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={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': dof, + 'robot_type': robot_type, + 'ros2_control_plugin': ros2_control_plugin, + 'joint_states_remapping': 'joint_states', + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + # 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={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + # 'add_gripper': add_gripper if robot_type.perform(context) == 'xarm' else 'false', + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': dof, + 'robot_type': robot_type, + '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, + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + remappings = [ + ('follow_joint_trajectory', '{}{}_traj_controller/follow_joint_trajectory'.format(prefix.perform(context), xarm_type)), + ] + controllers = ['{}{}_traj_controller'.format(prefix.perform(context), xarm_type)] + if add_gripper.perform(context) in ('True', 'true') and robot_type.perform(context) == 'xarm': + remappings.append( + ('follow_joint_trajectory', '{}xarm_gripper_traj_controller/follow_joint_trajectory'.format(prefix.perform(context))) + ) + controllers.append('{}xarm_gripper_traj_controller'.format(prefix.perform(context))) + # joint state publisher node + joint_state_publisher_node = Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + output='screen', + parameters=[{'source_list': ['joint_states']}], + remappings=remappings, + ) + + # ros2 control launch + # xarm_controller/launch/_ros2_control.launch.py + ros2_control_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_controller'), 'launch', '_ros2_control.launch.py'])), + launch_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': dof, + 'robot_type': robot_type, + 'ros2_control_plugin': ros2_control_plugin, + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + # Load controllers + load_controllers = [] + for controller in controllers: + load_controllers.append(Node( + package='controller_manager', + executable='spawner', + output='screen', + arguments=[ + controller, + '--controller-manager', '{}/controller_manager'.format(ros_namespace) + ], + )) + + return [ + robot_description_launch, + robot_moveit_common_launch, + joint_state_publisher_node, + ros2_control_launch, + ] + load_controllers + + +def generate_launch_description(): + return LaunchDescription([ + OpaqueFunction(function=launch_setup) + ]) diff --git a/src/custom_xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py b/src/custom_xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py new file mode 100644 index 0000000..d57bd47 --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py @@ -0,0 +1,113 @@ +#!/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.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def launch_setup(context, *args, **kwargs): + 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) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + dof = LaunchConfiguration('dof', default=7) + robot_type = LaunchConfiguration('robot_type', default='xarm') + no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False) + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + ros2_control_plugin = 'gazebo_ros2_control/GazeboSystem' + controllers_name = 'fake_controllers' + moveit_controller_manager_key = 'moveit_simple_controller_manager' + 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={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + # 'add_gripper': add_gripper if robot_type.perform(context) == 'xarm' else 'false', + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': dof, + 'robot_type': robot_type, + '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, + 'add_realsense_d435i': add_realsense_d435i, + '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, + 'use_sim_time': 'true' + }.items(), + ) + + # 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={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': dof, + 'robot_type': robot_type, + 'ros2_control_plugin': ros2_control_plugin, + 'load_controller': 'true', + }.items(), + ) + + return [ + robot_gazebo_launch, + robot_moveit_common_launch, + ] + + +def generate_launch_description(): + return LaunchDescription([ + OpaqueFunction(function=launch_setup) + ]) diff --git a/src/custom_xarm_moveit_config/launch/_robot_moveit_realmove.launch.py b/src/custom_xarm_moveit_config/launch/_robot_moveit_realmove.launch.py new file mode 100644 index 0000000..db7b48c --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/_robot_moveit_realmove.launch.py @@ -0,0 +1,197 @@ +#!/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.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node + + +def launch_setup(context, *args, **kwargs): + robot_ip = LaunchConfiguration('robot_ip') + report_type = LaunchConfiguration('report_type', default='normal') + 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) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + dof = LaunchConfiguration('dof', default=7) + robot_type = LaunchConfiguration('robot_type', default='xarm') + no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False) + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + 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"') + + baud_checkset = LaunchConfiguration('baud_checkset', default=True) + default_gripper_baud = LaunchConfiguration('default_gripper_baud', default=2000000) + + ros2_control_plugin = 'uf_robot_hardware/UFRobotSystemHardware' + controllers_name = 'controllers' + moveit_controller_manager_key = 'moveit_simple_controller_manager' + moveit_controller_manager_value = 'moveit_simple_controller_manager/MoveItSimpleControllerManager' + xarm_type = '{}{}'.format(robot_type.perform(context), dof.perform(context)) + ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context) + + # # robot driver launch + # # xarm_api/launch/_robot_driver.launch.py + # robot_driver_launch = IncludeLaunchDescription( + # PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_api'), 'launch', '_robot_driver.launch.py'])), + # launch_arguments={ + # 'robot_ip': robot_ip, + # 'report_type': report_type, + # 'dof': dof, + # 'hw_ns': hw_ns, + # 'add_gripper': add_gripper, + # 'prefix': prefix, + # 'baud_checkset': baud_checkset, + # 'default_gripper_baud': default_gripper_baud, + # 'robot_type': robot_type, + # }.items(), + # ) + + # 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={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': dof, + 'robot_type': robot_type, + 'ros2_control_plugin': ros2_control_plugin, + 'joint_states_remapping': PathJoinSubstitution(['/', ros_namespace, hw_ns, 'joint_states']), + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + # 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={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + # 'add_gripper': add_gripper if robot_type.perform(context) == 'xarm' else 'false', + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': dof, + 'robot_type': robot_type, + '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, + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + # joint state publisher node + joint_state_publisher_node = Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + output='screen', + parameters=[{'source_list': ['{}/joint_states'.format(hw_ns.perform(context))]}], + remappings=[ + ('follow_joint_trajectory', '{}{}_traj_controller/follow_joint_trajectory'.format(prefix.perform(context), xarm_type)), + ], + ) + + # ros2 control launch + # xarm_controller/launch/_ros2_control.launch.py + ros2_control_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_controller'), 'launch', '_ros2_control.launch.py'])), + launch_arguments={ + 'robot_ip': robot_ip, + 'report_type': report_type, + 'baud_checkset': baud_checkset, + 'default_gripper_baud': default_gripper_baud, + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': dof, + 'robot_type': robot_type, + 'ros2_control_plugin': ros2_control_plugin, + }.items(), + ) + + control_node = Node( + package='controller_manager', + executable='spawner', + output='screen', + arguments=[ + '{}{}_traj_controller'.format(prefix.perform(context), xarm_type), + '--controller-manager', '{}/controller_manager'.format(ros_namespace) + ], + ) + + return [ + robot_description_launch, + robot_moveit_common_launch, + joint_state_publisher_node, + ros2_control_launch, + control_node, + # robot_driver_launch, + ] + + +def generate_launch_description(): + return LaunchDescription([ + OpaqueFunction(function=launch_setup) + ]) diff --git a/src/custom_xarm_moveit_config/launch/dual_lite6_moveit_fake.launch.py b/src/custom_xarm_moveit_config/launch/dual_lite6_moveit_fake.launch.py new file mode 100644 index 0000000..a8c512e --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/dual_lite6_moveit_fake.launch.py @@ -0,0 +1,71 @@ +#!/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 IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + hw_ns = LaunchConfiguration('hw_ns', default='ufactory') + limited = LaunchConfiguration('limited', default=True) + 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_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # robot moveit fake launch + # xarm_moveit_config/launch/_dual_robot_moveit_fake.launch.py + robot_moveit_fake_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_dual_robot_moveit_fake.launch.py'])), + launch_arguments={ + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '6', + 'robot_type': 'lite', + 'no_gui_ctrl': 'false', + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + return LaunchDescription([ + robot_moveit_fake_launch + ]) diff --git a/src/custom_xarm_moveit_config/launch/dual_lite6_moveit_realmove.launch.py b/src/custom_xarm_moveit_config/launch/dual_lite6_moveit_realmove.launch.py new file mode 100644 index 0000000..6f20e7c --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/dual_lite6_moveit_realmove.launch.py @@ -0,0 +1,77 @@ +#!/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 IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + robot_ip_1 = LaunchConfiguration('robot_ip_1') + robot_ip_2 = LaunchConfiguration('robot_ip_2') + report_type = LaunchConfiguration('report_type', default='normal') + hw_ns = LaunchConfiguration('hw_ns', default='ufactory') + limited = LaunchConfiguration('limited', default=True) + 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_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # robot moveit realmove launch + # xarm_moveit_config/launch/_dual_robot_moveit_realmove.launch.py + robot_moveit_realmove_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_dual_robot_moveit_realmove.launch.py'])), + launch_arguments={ + 'robot_ip_1': robot_ip_1, + 'robot_ip_2': robot_ip_2, + 'report_type': report_type, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '6', + 'robot_type': 'lite', + 'no_gui_ctrl': 'false', + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + return LaunchDescription([ + robot_moveit_realmove_launch + ]) diff --git a/src/custom_xarm_moveit_config/launch/dual_xarm5_moveit_fake.launch.py b/src/custom_xarm_moveit_config/launch/dual_xarm5_moveit_fake.launch.py new file mode 100644 index 0000000..a8b39ca --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/dual_xarm5_moveit_fake.launch.py @@ -0,0 +1,71 @@ +#!/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 IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + 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) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # robot moveit fake launch + # xarm_moveit_config/launch/_dual_robot_moveit_fake.launch.py + robot_moveit_fake_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_dual_robot_moveit_fake.launch.py'])), + launch_arguments={ + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '5', + 'robot_type': 'xarm', + 'no_gui_ctrl': 'false', + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + return LaunchDescription([ + robot_moveit_fake_launch + ]) diff --git a/src/custom_xarm_moveit_config/launch/dual_xarm5_moveit_realmove.launch.py b/src/custom_xarm_moveit_config/launch/dual_xarm5_moveit_realmove.launch.py new file mode 100644 index 0000000..1565e40 --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/dual_xarm5_moveit_realmove.launch.py @@ -0,0 +1,77 @@ +#!/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 IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + robot_ip_1 = LaunchConfiguration('robot_ip_1') + robot_ip_2 = LaunchConfiguration('robot_ip_2') + report_type = LaunchConfiguration('report_type', default='normal') + 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) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # robot moveit realmove launch + # xarm_moveit_config/launch/_dual_robot_moveit_realmove.launch.py + robot_moveit_realmove_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_dual_robot_moveit_realmove.launch.py'])), + launch_arguments={ + 'robot_ip_1': robot_ip_1, + 'robot_ip_2': robot_ip_2, + 'report_type': report_type, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '5', + 'robot_type': 'xarm', + 'no_gui_ctrl': 'false', + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + return LaunchDescription([ + robot_moveit_realmove_launch + ]) diff --git a/src/custom_xarm_moveit_config/launch/dual_xarm6_moveit_fake.launch.py b/src/custom_xarm_moveit_config/launch/dual_xarm6_moveit_fake.launch.py new file mode 100644 index 0000000..5e2a93d --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/dual_xarm6_moveit_fake.launch.py @@ -0,0 +1,71 @@ +#!/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 IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + 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) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # robot moveit fake launch + # xarm_moveit_config/launch/_dual_robot_moveit_fake.launch.py + robot_moveit_fake_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_dual_robot_moveit_fake.launch.py'])), + launch_arguments={ + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '6', + 'robot_type': 'xarm', + 'no_gui_ctrl': 'false', + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + return LaunchDescription([ + robot_moveit_fake_launch + ]) diff --git a/src/custom_xarm_moveit_config/launch/dual_xarm6_moveit_realmove.launch.py b/src/custom_xarm_moveit_config/launch/dual_xarm6_moveit_realmove.launch.py new file mode 100644 index 0000000..4038fb7 --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/dual_xarm6_moveit_realmove.launch.py @@ -0,0 +1,77 @@ +#!/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 IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + robot_ip_1 = LaunchConfiguration('robot_ip_1') + robot_ip_2 = LaunchConfiguration('robot_ip_2') + report_type = LaunchConfiguration('report_type', default='normal') + 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) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # robot moveit realmove launch + # xarm_moveit_config/launch/_dual_robot_moveit_realmove.launch.py + robot_moveit_realmove_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_dual_robot_moveit_realmove.launch.py'])), + launch_arguments={ + 'robot_ip_1': robot_ip_1, + 'robot_ip_2': robot_ip_2, + 'report_type': report_type, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '6', + 'robot_type': 'xarm', + 'no_gui_ctrl': 'false', + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + return LaunchDescription([ + robot_moveit_realmove_launch + ]) diff --git a/src/custom_xarm_moveit_config/launch/dual_xarm7_moveit_fake.launch.py b/src/custom_xarm_moveit_config/launch/dual_xarm7_moveit_fake.launch.py new file mode 100644 index 0000000..70fd62f --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/dual_xarm7_moveit_fake.launch.py @@ -0,0 +1,71 @@ +#!/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 IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + 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) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # robot moveit fake launch + # xarm_moveit_config/launch/_dual_robot_moveit_fake.launch.py + robot_moveit_fake_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_dual_robot_moveit_fake.launch.py'])), + launch_arguments={ + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '7', + 'robot_type': 'xarm', + 'no_gui_ctrl': 'false', + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + return LaunchDescription([ + robot_moveit_fake_launch + ]) diff --git a/src/custom_xarm_moveit_config/launch/dual_xarm7_moveit_realmove.launch.py b/src/custom_xarm_moveit_config/launch/dual_xarm7_moveit_realmove.launch.py new file mode 100644 index 0000000..f7adb97 --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/dual_xarm7_moveit_realmove.launch.py @@ -0,0 +1,77 @@ +#!/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 IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + robot_ip_1 = LaunchConfiguration('robot_ip_1') + robot_ip_2 = LaunchConfiguration('robot_ip_2') + report_type = LaunchConfiguration('report_type', default='normal') + 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) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # robot moveit realmove launch + # xarm_moveit_config/launch/_dual_robot_moveit_realmove.launch.py + robot_moveit_realmove_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_dual_robot_moveit_realmove.launch.py'])), + launch_arguments={ + 'robot_ip_1': robot_ip_1, + 'robot_ip_2': robot_ip_2, + 'report_type': report_type, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '7', + 'robot_type': 'xarm', + 'no_gui_ctrl': 'false', + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + return LaunchDescription([ + robot_moveit_realmove_launch + ]) diff --git a/src/custom_xarm_moveit_config/launch/lib/robot_moveit_config_lib.py b/src/custom_xarm_moveit_config/launch/lib/robot_moveit_config_lib.py new file mode 100644 index 0000000..151bb3d --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/lib/robot_moveit_config_lib.py @@ -0,0 +1,92 @@ +#!/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.substitutions import Command, FindExecutable, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def load_file(package_name, *file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, *file_path) + if not os.path.exists(absolute_file_path): + return {} + try: + with open(absolute_file_path, 'r') as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return {} + except: + return {} + +def load_yaml(package_name, *file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, *file_path) + if not os.path.exists(absolute_file_path): + return {} + try: + with open(absolute_file_path, 'r') as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return {} + except: + return {} + + +def add_prefix_to_moveit_params(controllers_yaml=None, ompl_planning_yaml=None, kinematics_yaml=None, joint_limits_yaml=None, prefix=''): + if not prefix: + return + if controllers_yaml: + for i, name in enumerate(controllers_yaml['controller_names']): + joints = controllers_yaml.get(name, {}).get('joints', []) + for j, joint in enumerate(joints): + joints[j] = '{}{}'.format(prefix, joint) + controllers_yaml['controller_names'][i] = '{}{}'.format(prefix, name) + if name in controllers_yaml: + controllers_yaml['{}{}'.format(prefix, name)] = controllers_yaml.pop(name) + if ompl_planning_yaml: + for name in list(ompl_planning_yaml.keys()): + if name != 'planner_configs': + ompl_planning_yaml['{}{}'.format(prefix, name)] = ompl_planning_yaml.pop(name) + if kinematics_yaml: + for name in list(kinematics_yaml.keys()): + kinematics_yaml['{}{}'.format(prefix, name)] = kinematics_yaml.pop(name) + if joint_limits_yaml: + for name in list(joint_limits_yaml['joint_limits']): + joint_limits_yaml['joint_limits']['{}{}'.format(prefix, name)] = joint_limits_yaml['joint_limits'].pop(name) + + +def get_xarm_robot_description_parameters( + xacro_urdf_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']), + xacro_srdf_file=PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']), + urdf_arguments={}, + srdf_arguments={}, + arguments={}): + urdf_arguments['ros2_control_plugin'] = urdf_arguments.get('ros2_control_plugin', 'uf_robot_hardware/UFRobotSystemHardware') + moveit_config_package_name = 'xarm_moveit_config' + xarm_type = arguments.get('xarm_type', None) + + # 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') + + return { + 'robot_description': get_xacro_file_content( + xacro_file=xacro_urdf_file, + arguments=urdf_arguments + ), + 'robot_description_semantic': get_xacro_file_content( + xacro_file=xacro_srdf_file, + arguments=srdf_arguments + ), + 'robot_description_planning': load_yaml(moveit_config_package_name, 'config', xarm_type, 'joint_limits.yaml'), + 'robot_description_kinematics': load_yaml(moveit_config_package_name, 'config', xarm_type, 'kinematics.yaml') + } diff --git a/src/custom_xarm_moveit_config/launch/lite6_moveit_fake.launch.py b/src/custom_xarm_moveit_config/launch/lite6_moveit_fake.launch.py new file mode 100644 index 0000000..f23c779 --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/lite6_moveit_fake.launch.py @@ -0,0 +1,73 @@ +#!/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 IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + prefix = LaunchConfiguration('prefix', default='') + hw_ns = LaunchConfiguration('hw_ns', default='ufactory') + limited = LaunchConfiguration('limited', default=True) + 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_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # robot moveit fake launch + # xarm_moveit_config/launch/_robot_moveit_fake.launch.py + robot_moveit_fake_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_robot_moveit_fake.launch.py'])), + launch_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '6', + 'robot_type': 'lite', + 'no_gui_ctrl': 'false', + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + return LaunchDescription([ + robot_moveit_fake_launch + ]) diff --git a/src/custom_xarm_moveit_config/launch/lite6_moveit_gazebo.launch.py b/src/custom_xarm_moveit_config/launch/lite6_moveit_gazebo.launch.py new file mode 100644 index 0000000..eefa2ff --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/lite6_moveit_gazebo.launch.py @@ -0,0 +1,73 @@ +#!/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 IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + prefix = LaunchConfiguration('prefix', default='') + hw_ns = LaunchConfiguration('hw_ns', default='ufactory') + limited = LaunchConfiguration('limited', default=True) + 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_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # robot moveit gazebo launch + # xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py + robot_moveit_gazebo_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_robot_moveit_gazebo.launch.py'])), + launch_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '6', + 'robot_type': 'lite', + 'no_gui_ctrl': 'false', + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + return LaunchDescription([ + robot_moveit_gazebo_launch + ]) diff --git a/src/custom_xarm_moveit_config/launch/lite6_moveit_realmove.launch.py b/src/custom_xarm_moveit_config/launch/lite6_moveit_realmove.launch.py new file mode 100644 index 0000000..4afbf17 --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/lite6_moveit_realmove.launch.py @@ -0,0 +1,77 @@ +#!/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 IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + robot_ip = LaunchConfiguration('robot_ip') + report_type = LaunchConfiguration('report_type', default='normal') + prefix = LaunchConfiguration('prefix', default='') + hw_ns = LaunchConfiguration('hw_ns', default='ufactory') + limited = LaunchConfiguration('limited', default=True) + 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_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # robot moveit realmove launch + # xarm_moveit_config/launch/_robot_moveit_realmove.launch.py + robot_moveit_realmove_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_robot_moveit_realmove.launch.py'])), + launch_arguments={ + 'robot_ip': robot_ip, + 'report_type': report_type, + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '6', + 'robot_type': 'lite', + 'no_gui_ctrl': 'false', + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + return LaunchDescription([ + robot_moveit_realmove_launch + ]) diff --git a/src/custom_xarm_moveit_config/launch/xarm5_moveit_fake.launch.py b/src/custom_xarm_moveit_config/launch/xarm5_moveit_fake.launch.py new file mode 100644 index 0000000..6eddda2 --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/xarm5_moveit_fake.launch.py @@ -0,0 +1,73 @@ +#!/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 IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + 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) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # robot moveit fake launch + # xarm_moveit_config/launch/_robot_moveit_fake.launch.py + robot_moveit_fake_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_robot_moveit_fake.launch.py'])), + launch_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '5', + 'robot_type': 'xarm', + 'no_gui_ctrl': 'false', + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + return LaunchDescription([ + robot_moveit_fake_launch + ]) diff --git a/src/custom_xarm_moveit_config/launch/xarm5_moveit_gazebo.launch.py b/src/custom_xarm_moveit_config/launch/xarm5_moveit_gazebo.launch.py new file mode 100644 index 0000000..033c22c --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/xarm5_moveit_gazebo.launch.py @@ -0,0 +1,73 @@ +#!/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 IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + 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) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # robot moveit gazebo launch + # xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py + robot_moveit_gazebo_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_robot_moveit_gazebo.launch.py'])), + launch_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '5', + 'robot_type': 'xarm', + 'no_gui_ctrl': 'false', + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + return LaunchDescription([ + robot_moveit_gazebo_launch + ]) diff --git a/src/custom_xarm_moveit_config/launch/xarm5_moveit_realmove.launch.py b/src/custom_xarm_moveit_config/launch/xarm5_moveit_realmove.launch.py new file mode 100644 index 0000000..1bd48f6 --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/xarm5_moveit_realmove.launch.py @@ -0,0 +1,77 @@ +#!/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 IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + robot_ip = LaunchConfiguration('robot_ip') + report_type = LaunchConfiguration('report_type', default='normal') + 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) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # robot moveit realmove launch + # xarm_moveit_config/launch/_robot_moveit_realmove.launch.py + robot_moveit_realmove_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_robot_moveit_realmove.launch.py'])), + launch_arguments={ + 'robot_ip': robot_ip, + 'report_type': report_type, + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '5', + 'robot_type': 'xarm', + 'no_gui_ctrl': 'false', + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + return LaunchDescription([ + robot_moveit_realmove_launch + ]) diff --git a/src/custom_xarm_moveit_config/launch/xarm6_moveit_fake.launch.py b/src/custom_xarm_moveit_config/launch/xarm6_moveit_fake.launch.py new file mode 100644 index 0000000..4884eb3 --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/xarm6_moveit_fake.launch.py @@ -0,0 +1,73 @@ +#!/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 IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + 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) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # robot moveit fake launch + # xarm_moveit_config/launch/_robot_moveit_fake.launch.py + robot_moveit_fake_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_robot_moveit_fake.launch.py'])), + launch_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '6', + 'robot_type': 'xarm', + 'no_gui_ctrl': 'false', + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + return LaunchDescription([ + robot_moveit_fake_launch + ]) diff --git a/src/custom_xarm_moveit_config/launch/xarm6_moveit_gazebo.launch.py b/src/custom_xarm_moveit_config/launch/xarm6_moveit_gazebo.launch.py new file mode 100644 index 0000000..0bc0201 --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/xarm6_moveit_gazebo.launch.py @@ -0,0 +1,73 @@ +#!/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 IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + 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) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # robot moveit gazebo launch + # xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py + robot_moveit_gazebo_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_robot_moveit_gazebo.launch.py'])), + launch_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '6', + 'robot_type': 'xarm', + 'no_gui_ctrl': 'false', + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + return LaunchDescription([ + robot_moveit_gazebo_launch + ]) diff --git a/src/custom_xarm_moveit_config/launch/xarm6_moveit_realmove.launch.py b/src/custom_xarm_moveit_config/launch/xarm6_moveit_realmove.launch.py new file mode 100644 index 0000000..74806f1 --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/xarm6_moveit_realmove.launch.py @@ -0,0 +1,77 @@ +#!/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 IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + robot_ip = LaunchConfiguration('robot_ip') + report_type = LaunchConfiguration('report_type', default='normal') + 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) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # robot moveit realmove launch + # xarm_moveit_config/launch/_robot_moveit_realmove.launch.py + robot_moveit_realmove_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_robot_moveit_realmove.launch.py'])), + launch_arguments={ + 'robot_ip': robot_ip, + 'report_type': report_type, + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '6', + 'robot_type': 'xarm', + 'no_gui_ctrl': 'false', + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + return LaunchDescription([ + robot_moveit_realmove_launch + ]) diff --git a/src/custom_xarm_moveit_config/launch/xarm7_moveit_fake.launch.py b/src/custom_xarm_moveit_config/launch/xarm7_moveit_fake.launch.py new file mode 100644 index 0000000..e92e171 --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/xarm7_moveit_fake.launch.py @@ -0,0 +1,73 @@ +#!/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 IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + 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) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # robot moveit fake launch + # xarm_moveit_config/launch/_robot_moveit_fake.launch.py + robot_moveit_fake_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_robot_moveit_fake.launch.py'])), + launch_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '7', + 'robot_type': 'xarm', + 'no_gui_ctrl': 'false', + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + return LaunchDescription([ + robot_moveit_fake_launch + ]) diff --git a/src/custom_xarm_moveit_config/launch/xarm7_moveit_gazebo.launch.py b/src/custom_xarm_moveit_config/launch/xarm7_moveit_gazebo.launch.py new file mode 100644 index 0000000..4974552 --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/xarm7_moveit_gazebo.launch.py @@ -0,0 +1,72 @@ +#!/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 IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + 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) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + 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"') + + # robot moveit gazebo launch + # xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py + robot_moveit_gazebo_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_robot_moveit_gazebo.launch.py'])), + launch_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '7', + 'robot_type': 'xarm', + 'no_gui_ctrl': 'false', + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + return LaunchDescription([ + robot_moveit_gazebo_launch + ]) diff --git a/src/custom_xarm_moveit_config/launch/xarm7_moveit_realmove.launch.py b/src/custom_xarm_moveit_config/launch/xarm7_moveit_realmove.launch.py new file mode 100644 index 0000000..1a3d5e9 --- /dev/null +++ b/src/custom_xarm_moveit_config/launch/xarm7_moveit_realmove.launch.py @@ -0,0 +1,77 @@ +#!/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 IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + robot_ip = LaunchConfiguration('robot_ip') + report_type = LaunchConfiguration('report_type', default='normal') + 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) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + + add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) + + 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"') + + # robot moveit realmove launch + # xarm_moveit_config/launch/_robot_moveit_realmove.launch.py + robot_moveit_realmove_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_robot_moveit_realmove.launch.py'])), + launch_arguments={ + 'robot_ip': robot_ip, + 'report_type': report_type, + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': '7', + 'robot_type': 'xarm', + 'no_gui_ctrl': 'false', + 'add_realsense_d435i': add_realsense_d435i, + '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, + }.items(), + ) + + return LaunchDescription([ + robot_moveit_realmove_launch + ]) diff --git a/src/custom_xarm_moveit_config/package.xml b/src/custom_xarm_moveit_config/package.xml new file mode 100644 index 0000000..a11d512 --- /dev/null +++ b/src/custom_xarm_moveit_config/package.xml @@ -0,0 +1,36 @@ + + + + custom_xarm_moveit_config + 0.0.0 + TODO: Package description + + TODO: License declaration + + ament_cmake + + backward_ros + xarm_description + xarm_controller + xarm_gazebo + + rviz2 + xacro + urdf + launch + launch_ros + tf2_ros + joint_state_publisher + robot_state_publisher + controller_manager + joint_trajectory_controller + joint_state_broadcaster + moveit_ros_move_group + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/custom_xarm_moveit_config/rviz/dual_moveit.rviz b/src/custom_xarm_moveit_config/rviz/dual_moveit.rviz new file mode 100644 index 0000000..2256ff3 --- /dev/null +++ b/src/custom_xarm_moveit_config/rviz/dual_moveit.rviz @@ -0,0 +1,546 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /RobotModel1 + Splitter Ratio: 0.46875 + Tree Height: 130 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + - /Current View1/Focal Point1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + L_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_link_eef: + Alpha: 1 + Show Axes: false + Show Trail: false + Link Tree Style: Links in Alphabetic Order + R_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_link_eef: + Alpha: 1 + Show Axes: false + Show Trail: false + ground: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: L_xarm5 + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + L_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_link_eef: + Alpha: 1 + Show Axes: false + Show Trail: false + Link Tree Style: Links in Alphabetic Order + R_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_link_eef: + Alpha: 1 + Show Axes: false + Show Trail: false + ground: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + L_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_link_eef: + Alpha: 1 + Show Axes: false + Show Trail: false + Link Tree Style: Links in Alphabetic Order + R_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_link_eef: + Alpha: 1 + Show Axes: false + Show Trail: false + ground: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: moveit_rviz_plugin/Trajectory + Color Enabled: false + Enabled: true + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + L_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_link_eef: + Alpha: 1 + Show Axes: false + Show Trail: false + Link Tree Style: Links in Alphabetic Order + R_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_link_eef: + Alpha: 1 + Show Axes: false + Show Trail: false + ground: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Name: Trajectory + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Robot Description: robot_description + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: ground + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.286684513092041 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0.5 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.23539818823337555 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.1385722160339355 + Saved: ~ +Window Geometry: + " - Trajectory Slider": + collapsed: false + Displays: + collapsed: false + Height: 812 + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1533 + X: 256 + Y: 231 diff --git a/src/custom_xarm_moveit_config/rviz/dual_planner.rviz b/src/custom_xarm_moveit_config/rviz/dual_planner.rviz new file mode 100644 index 0000000..adcab54 --- /dev/null +++ b/src/custom_xarm_moveit_config/rviz/dual_planner.rviz @@ -0,0 +1,392 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + - /MotionPlanning1/Scene Robot1 + - /MotionPlanning1/Planning Request1 + - /MotionPlanning1/Planned Path1 + - /MarkerArray1 + Splitter Ratio: 0.46875 + Tree Height: 583 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_eef: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: xarm7 + Query Goal State: false + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_eef: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_eef: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.005836248397827 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5753980278968811 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.4903981685638428 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 812 + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1533 + X: 255 + Y: 231 diff --git a/src/custom_xarm_moveit_config/rviz/moveit.rviz b/src/custom_xarm_moveit_config/rviz/moveit.rviz new file mode 100644 index 0000000..c92f9d4 --- /dev/null +++ b/src/custom_xarm_moveit_config/rviz/moveit.rviz @@ -0,0 +1,409 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /RobotModel1 + - /Trajectory1 + Splitter Ratio: 0.46875 + Tree Height: 130 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_eef: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: xarm5 + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_eef: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_eef: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: moveit_rviz_plugin/Trajectory + Color Enabled: false + Enabled: true + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_eef: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Name: Trajectory + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Robot Description: robot_description + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.005836248397827 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5753980278968811 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.4903981685638428 + Saved: ~ +Window Geometry: + " - Trajectory Slider": + collapsed: false + Displays: + collapsed: false + Height: 812 + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1533 + X: 2041 + Y: 192 diff --git a/src/custom_xarm_moveit_config/rviz/planner.rviz b/src/custom_xarm_moveit_config/rviz/planner.rviz new file mode 100644 index 0000000..adcab54 --- /dev/null +++ b/src/custom_xarm_moveit_config/rviz/planner.rviz @@ -0,0 +1,392 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + - /MotionPlanning1/Scene Robot1 + - /MotionPlanning1/Planning Request1 + - /MotionPlanning1/Planned Path1 + - /MarkerArray1 + Splitter Ratio: 0.46875 + Tree Height: 583 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_eef: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: xarm7 + Query Goal State: false + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_eef: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_eef: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.005836248397827 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5753980278968811 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.4903981685638428 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 812 + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1533 + X: 255 + Y: 231 diff --git a/src/custom_xarm_moveit_config/srdf/_lite6_macro.srdf.xacro b/src/custom_xarm_moveit_config/srdf/_lite6_macro.srdf.xacro new file mode 100644 index 0000000..afeb100 --- /dev/null +++ b/src/custom_xarm_moveit_config/srdf/_lite6_macro.srdf.xacro @@ -0,0 +1,103 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/custom_xarm_moveit_config/srdf/_xarm5_macro.srdf.xacro b/src/custom_xarm_moveit_config/srdf/_xarm5_macro.srdf.xacro new file mode 100644 index 0000000..97670b6 --- /dev/null +++ b/src/custom_xarm_moveit_config/srdf/_xarm5_macro.srdf.xacro @@ -0,0 +1,161 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/custom_xarm_moveit_config/srdf/_xarm6_macro.srdf.xacro b/src/custom_xarm_moveit_config/srdf/_xarm6_macro.srdf.xacro new file mode 100644 index 0000000..b873afc --- /dev/null +++ b/src/custom_xarm_moveit_config/srdf/_xarm6_macro.srdf.xacro @@ -0,0 +1,172 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/custom_xarm_moveit_config/srdf/_xarm7_macro.srdf.xacro b/src/custom_xarm_moveit_config/srdf/_xarm7_macro.srdf.xacro new file mode 100644 index 0000000..412269d --- /dev/null +++ b/src/custom_xarm_moveit_config/srdf/_xarm7_macro.srdf.xacro @@ -0,0 +1,187 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/custom_xarm_moveit_config/srdf/dual_xarm.srdf.xacro b/src/custom_xarm_moveit_config/srdf/dual_xarm.srdf.xacro new file mode 100644 index 0000000..d9b3485 --- /dev/null +++ b/src/custom_xarm_moveit_config/srdf/dual_xarm.srdf.xacro @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/custom_xarm_moveit_config/srdf/xarm.srdf.xacro b/src/custom_xarm_moveit_config/srdf/xarm.srdf.xacro new file mode 100644 index 0000000..6fbc6c9 --- /dev/null +++ b/src/custom_xarm_moveit_config/srdf/xarm.srdf.xacro @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/custom_xarm_moveit_config/srdf/xarm_macro.srdf.xacro b/src/custom_xarm_moveit_config/srdf/xarm_macro.srdf.xacro new file mode 100644 index 0000000..d3f5ade --- /dev/null +++ b/src/custom_xarm_moveit_config/srdf/xarm_macro.srdf.xacro @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/lite6_controller/launch/lite6_gazebo.launch.py b/src/lite6_controller/launch/lite6_gazebo.launch.py index 985c39b..a7139c0 100644 --- a/src/lite6_controller/launch/lite6_gazebo.launch.py +++ b/src/lite6_controller/launch/lite6_gazebo.launch.py @@ -57,7 +57,7 @@ def launch_setup(context, *args, **kwargs): # 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'])), + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('custom_xarm_moveit_config'), 'launch', '_robot_moveit_common.launch.py'])), launch_arguments={ 'prefix': prefix, 'hw_ns': hw_ns, @@ -171,7 +171,7 @@ def launch_setup(context, *args, **kwargs): " ", PathJoinSubstitution( [ - FindPackageShare('xarm_moveit_config'), + FindPackageShare('custom_xarm_moveit_config'), "srdf", #"_lite6_macro.srdf.xacro", "xarm.srdf.xacro", @@ -244,7 +244,7 @@ def launch_setup(context, *args, **kwargs): add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) - moveit_config_package_name = 'xarm_moveit_config' + moveit_config_package_name = 'custom_xarm_moveit_config' xarm_type = '{}{}'.format(robot_type.perform(context), dof.perform(context)) # robot_description_parameters @@ -252,8 +252,8 @@ def launch_setup(context, *args, **kwargs): 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')) get_xarm_robot_description_parameters = getattr(mod, 'get_xarm_robot_description_parameters') robot_description_parameters = get_xarm_robot_description_parameters( - xacro_urdf_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']), - xacro_srdf_file=PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']), + xacro_urdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'urdf', 'xarm_device.urdf.xacro']), + xacro_srdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']), urdf_arguments={ 'prefix': prefix, 'hw_ns': hw_ns.perform(context).strip('/'),