diff --git a/crane_x7_moveit_config/config/controllers.yaml b/crane_x7_moveit_config/config/controllers.yaml index e5c8c323..58c44026 100644 --- a/crane_x7_moveit_config/config/controllers.yaml +++ b/crane_x7_moveit_config/config/controllers.yaml @@ -1,23 +1,27 @@ -controller_names: - - crane_x7_arm_controller - - crane_x7_gripper_controller +# MoveIt uses this configuration for controller management +moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager -crane_x7_arm_controller: - action_ns: follow_joint_trajectory - type: FollowJointTrajectory - default: true - joints: - - crane_x7_shoulder_fixed_part_pan_joint - - crane_x7_shoulder_revolute_part_tilt_joint - - crane_x7_upper_arm_revolute_part_twist_joint - - crane_x7_upper_arm_revolute_part_rotate_joint - - crane_x7_lower_arm_fixed_part_joint - - crane_x7_lower_arm_revolute_part_joint - - crane_x7_wrist_joint +moveit_simple_controller_manager: + controller_names: + - crane_x7_arm_controller + - crane_x7_gripper_controller -crane_x7_gripper_controller: - action_ns: gripper_cmd - type: GripperCommand - default: true - joints: - - crane_x7_gripper_finger_a_joint \ No newline at end of file + crane_x7_arm_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - crane_x7_shoulder_fixed_part_pan_joint + - crane_x7_shoulder_revolute_part_tilt_joint + - crane_x7_upper_arm_revolute_part_twist_joint + - crane_x7_upper_arm_revolute_part_rotate_joint + - crane_x7_lower_arm_fixed_part_joint + - crane_x7_lower_arm_revolute_part_joint + - crane_x7_wrist_joint + + crane_x7_gripper_controller: + action_ns: gripper_cmd + type: GripperCommand + default: true + joints: + - crane_x7_gripper_finger_a_joint \ No newline at end of file diff --git a/crane_x7_moveit_config/config/kinematics.yaml b/crane_x7_moveit_config/config/kinematics.yaml index 46a23863..a1f17d08 100644 --- a/crane_x7_moveit_config/config/kinematics.yaml +++ b/crane_x7_moveit_config/config/kinematics.yaml @@ -2,3 +2,4 @@ arm: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 \ No newline at end of file diff --git a/crane_x7_moveit_config/config/ompl_planning.yaml b/crane_x7_moveit_config/config/ompl_planning.yaml index f98332c4..266ad7f1 100644 --- a/crane_x7_moveit_config/config/ompl_planning.yaml +++ b/crane_x7_moveit_config/config/ompl_planning.yaml @@ -1,4 +1,25 @@ +planning_plugins: + - ompl_interface/OMPLPlanner +# To optionally use Ruckig for jerk-limited smoothing, add this line to the request adapters below +# default_planning_request_adapters/AddRuckigTrajectorySmoothing +request_adapters: + - default_planning_request_adapters/ResolveConstraintFrames + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision +response_adapters: + - default_planning_response_adapters/AddTimeOptimalParameterization + - default_planning_response_adapters/ValidateSolution + - default_planning_response_adapters/DisplayMotionPath + planner_configs: + APSConfigDefault: + type: geometric::AnytimePathShortening + shortcut: 1 # Attempt to shortcut all new solution paths + hybridize: 1 # Compute hybrid solution trajectories + max_hybrid_paths: 36 # Number of hybrid paths generated per iteration + num_planners: 8 # The number of default planners to use for planning + planners: "RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect" # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]" SBLkConfigDefault: type: geometric::SBL range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() @@ -122,6 +143,7 @@ planner_configs: max_failures: 5000 # maximum consecutive failure limit. default: 5000 arm: planner_configs: + - APSConfigDefault - SBLkConfigDefault - ESTkConfigDefault - LBKPIECEkConfigDefault @@ -147,6 +169,7 @@ arm: - SPARStwokConfigDefault gripper: planner_configs: + - APSConfigDefault - SBLkConfigDefault - ESTkConfigDefault - LBKPIECEkConfigDefault diff --git a/crane_x7_moveit_config/launch/run_move_group.launch.py b/crane_x7_moveit_config/launch/run_move_group.launch.py index 001246af..0edfca4c 100644 --- a/crane_x7_moveit_config/launch/run_move_group.launch.py +++ b/crane_x7_moveit_config/launch/run_move_group.launch.py @@ -1,4 +1,4 @@ -# Copyright 2022 RT Corporation +# Copyright 2020 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -15,15 +15,16 @@ import os from ament_index_python.packages import get_package_share_directory -from crane_x7_description.robot_description_loader import RobotDescriptionLoader from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -import yaml - -# Reference: https://github.com/ros-planning/moveit2_tutorials/blob/humble/doc/ -# examples/move_group_interface/launch/move_group.launch.py +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launch_utils import DeclareBooleanLaunchArg +from moveit_configs_utils.launches import generate_move_group_launch +from moveit_configs_utils.launches import generate_moveit_rviz_launch +from moveit_configs_utils.launches \ + import generate_static_virtual_joint_tfs_launch +from moveit_configs_utils.launches import generate_rsp_launch def load_file(package_name, file_path): @@ -33,7 +34,9 @@ def load_file(package_name, file_path): try: with open(absolute_file_path, 'r') as file: return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + except ( + EnvironmentError + ): # parent of IOError, OSError *and* WindowsError where available return None @@ -44,114 +47,81 @@ def load_yaml(package_name, file_path): try: with open(absolute_file_path, 'r') as file: return yaml.safe_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + except ( + EnvironmentError + ): # parent of IOError, OSError *and* WindowsError where available return None def generate_launch_description(): - description_loader = RobotDescriptionLoader() + ld = LaunchDescription() declare_loaded_description = DeclareLaunchArgument( 'loaded_description', - default_value=description_loader.load(), + default_value='', description='Set robot_description text. \ - It is recommended to use RobotDescriptionLoader() in crane_x7_description.' + It is recommended to use RobotDescriptionLoader() in \ + crane_x7_description.', ) - declare_rviz_config_file = DeclareLaunchArgument( - 'rviz_config_file', - default_value=get_package_share_directory( - 'crane_x7_moveit_config') + '/launch/run_move_group.rviz', - description='Set the path to rviz configuration file.' + ld.add_action(declare_loaded_description) + + ld.add_action(DeclareBooleanLaunchArg('debug', default_value=False)) + + ld.add_action( + DeclareLaunchArgument( + 'rviz_config', + default_value=get_package_share_directory('crane_x7_moveit_config') + + '/config/moveit.rviz', + description='Set the path to rviz configuration file.', + ) ) - robot_description = {'robot_description': LaunchConfiguration('loaded_description')} + moveit_config = ( + MoveItConfigsBuilder('crane_x7') + .planning_scene_monitor( + publish_robot_description=True, + publish_robot_description_semantic=True, + ) + .robot_description( + file_path=os.path.join( + get_package_share_directory('crane_x7_description'), + 'urdf', + 'crane_x7.urdf.xacro', + ), + mappings={}, + ) + .robot_description_semantic( + file_path='config/crane_x7.srdf', + mappings={'model': 'crane_x7'}, + ) + .joint_limits(file_path='config/joint_limits.yaml') + .trajectory_execution( + file_path='config/controllers.yaml', moveit_manage_controllers=True + ) + .planning_pipelines(pipelines=['ompl']) + .robot_description_kinematics(file_path='config/kinematics.yaml') + .to_moveit_configs() + ) - robot_description_semantic_config = load_file( - 'crane_x7_moveit_config', 'config/crane_x7.srdf') - robot_description_semantic = { - 'robot_description_semantic': robot_description_semantic_config} + moveit_config.robot_description = { + 'robot_description': LaunchConfiguration('loaded_description') + } - joint_limits_yaml = load_yaml( - "crane_x7_moveit_config", "config/joint_limits.yaml" - ) - robot_description_planning = { - "robot_description_planning": joint_limits_yaml} - - kinematics_yaml = load_yaml('crane_x7_moveit_config', 'config/kinematics.yaml') - - # Planning Functionality - 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_yaml = load_yaml('crane_x7_moveit_config', 'config/ompl_planning.yaml') - ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml) - - # Trajectory Execution Functionality - controllers_yaml = load_yaml('crane_x7_moveit_config', 'config/controllers.yaml') - moveit_controllers = { - 'moveit_simple_controller_manager': controllers_yaml, - 'moveit_controller_manager': - 'moveit_simple_controller_manager/MoveItSimpleControllerManager'} - - 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.1} - - planning_scene_monitor_parameters = {'publish_planning_scene': True, - 'publish_geometry_updates': True, - 'publish_state_updates': True, - 'publish_transforms_updates': True} - - # Start the actual move_group node/action server - run_move_group_node = Node(package='moveit_ros_move_group', - executable='move_group', - output='screen', - parameters=[robot_description, - robot_description_semantic, - robot_description_planning, - kinematics_yaml, - ompl_planning_pipeline_config, - trajectory_execution, - moveit_controllers, - planning_scene_monitor_parameters]) + moveit_config.move_group_capabilities = { + 'capabilities': '' + } + + # Move group + ld.add_entity(generate_move_group_launch(moveit_config)) # RViz - rviz_config_file = LaunchConfiguration('rviz_config_file') - rviz_node = Node(package='rviz2', - executable='rviz2', - name='rviz2', - output='log', - arguments=['-d', rviz_config_file], - parameters=[robot_description, - robot_description_semantic, - ompl_planning_pipeline_config, - kinematics_yaml]) + ld.add_entity(generate_moveit_rviz_launch(moveit_config)) # Static TF - static_tf = Node(package='tf2_ros', - executable='static_transform_publisher', - name='static_transform_publisher', - output='log', - arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'world', 'base_link']) + ld.add_entity(generate_static_virtual_joint_tfs_launch(moveit_config)) # Publish TF - robot_state_publisher = Node(package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - output='both', - parameters=[robot_description]) - - return LaunchDescription([declare_loaded_description, - declare_rviz_config_file, - run_move_group_node, - rviz_node, - static_tf, - robot_state_publisher, - ]) + ld.add_entity(generate_rsp_launch(moveit_config)) + + return ld