From 465b48de18be81e25750676e69956ef5970b2560 Mon Sep 17 00:00:00 2001 From: Daniel Azanov Date: Thu, 11 Jul 2024 19:50:46 +0200 Subject: [PATCH 1/5] bkp joints with suffixes --- .../config/motoman_controllers.yaml | 14 ------ .../config/moveit_controllers.yaml | 17 +++++++ .../bringup_with_ros2_control.launch.py | 46 +++++++++++++++++-- .../urdf/common_macro.ros2_control.xacro | 29 +++++++++--- .../urdf/common_motoman.xacro | 4 +- 5 files changed, 85 insertions(+), 25 deletions(-) delete mode 100644 motoman_common_moveit/config/motoman_controllers.yaml create mode 100644 motoman_common_moveit/config/moveit_controllers.yaml diff --git a/motoman_common_moveit/config/motoman_controllers.yaml b/motoman_common_moveit/config/motoman_controllers.yaml deleted file mode 100644 index 8ca7918e..00000000 --- a/motoman_common_moveit/config/motoman_controllers.yaml +++ /dev/null @@ -1,14 +0,0 @@ -controller_names: - - follow_joint_trajectory - -follow_joint_trajectory: - action_ns: "" - type: FollowJointTrajectory - default: true - joints: - - group_1/joint_1 - - group_1/joint_2 - - group_1/joint_3 - - group_1/joint_4 - - group_1/joint_5 - - group_1/joint_6 diff --git a/motoman_common_moveit/config/moveit_controllers.yaml b/motoman_common_moveit/config/moveit_controllers.yaml new file mode 100644 index 00000000..ff79c382 --- /dev/null +++ b/motoman_common_moveit/config/moveit_controllers.yaml @@ -0,0 +1,17 @@ +moveit_simple_controller_manager: + controller_names: + - follow_joint_trajectory + + follow_joint_trajectory: + action_ns: "" + type: FollowJointTrajectory + default: true + joints: + - group_1/joint_1 + - group_1/joint_2 + - group_1/joint_3 + - group_1/joint_4 + - group_1/joint_5 + - group_1/joint_6 + +moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager diff --git a/motoman_common_moveit/launch/bringup_with_ros2_control.launch.py b/motoman_common_moveit/launch/bringup_with_ros2_control.launch.py index 51f4c0ab..68218f6c 100644 --- a/motoman_common_moveit/launch/bringup_with_ros2_control.launch.py +++ b/motoman_common_moveit/launch/bringup_with_ros2_control.launch.py @@ -66,6 +66,14 @@ def generate_launch_description(): have to be updated.", ) ) + declared_arguments.append( + DeclareLaunchArgument( + "joints_with_suffix", + default_value="false", + description="Whether the joint names have a suffix, \ + i.e. [joint_1_s, joint_2_l, joint_3_u, joint_4_r, joint_5_b, joint_6_t]", + ) + ) declared_arguments.append( DeclareLaunchArgument( "description_package", @@ -115,6 +123,15 @@ def generate_launch_description(): ) ) + declared_arguments.append( + DeclareLaunchArgument( + "moveit_controllers_file", + default_value="moveit_controllers.yaml", + description="YAML file with the controllers configuration. \ + The expected location of the file is '/config/'.", + ) + ) + declared_arguments.append( DeclareLaunchArgument( "semantic_description_file", @@ -144,6 +161,7 @@ def generate_launch_description(): # initialize arguments robot_name = LaunchConfiguration("robot_name") prefix = LaunchConfiguration("prefix") + joints_with_suffix = LaunchConfiguration("joints_with_suffix") description_package = LaunchConfiguration("description_package") description_macro_file = LaunchConfiguration("description_macro_file") configuration_package = LaunchConfiguration("configuration_package") @@ -152,6 +170,7 @@ def generate_launch_description(): use_mock_hardware = LaunchConfiguration("use_mock_hardware") moveit_config_package = LaunchConfiguration("moveit_config_package") + moveit_controllers_file = LaunchConfiguration("moveit_controllers_file") semantic_description_file = LaunchConfiguration("semantic_description_file") rviz_file = LaunchConfiguration("rviz_file") @@ -171,6 +190,9 @@ def generate_launch_description(): "prefix:=", prefix, " ", + "joints_with_suffix:=", + joints_with_suffix, + " ", "description_package:=", description_package, " ", @@ -282,7 +304,7 @@ def generate_launch_description(): "motoman_common_moveit", "config/pilz_planning.yaml" ) pilz_planning_pipeline_config["pilz"].update(pilz_planning_yaml) - + pilz_capabilities_yaml = load_yaml( "motoman_common_moveit", "config/pilz_capabilities.yaml" ) @@ -314,13 +336,29 @@ def generate_launch_description(): # robot_description_planning_config["robot_description_planning"].update(joint_limits_yaml) # Trajectory Execution Functionality - moveit_simple_controllers_yaml = load_yaml( - "motoman_common_moveit", "config/motoman_controllers.yaml" - ) + # moveit_simple_controllers_yaml = load_yaml( + # "motoman_common_moveit", "config/motoman_controllers.yaml" + # ) + joint_names = [f"joint_{i}" for i in range(1, 7)] + joint_suffixes = ["s", "l", "u", "r", "b", "t"] + if True: + joint_names = [joint_name + "_" + suffix for joint_name, suffix in zip(joint_names, joint_suffixes)] + moveit_simple_controllers_yaml = { + "controller_names": ["follow_joint_trajectory"], + "follow_joint_trajectory:": { + "action_ns": "", + "type": "FollowJointTrajectory", + "default": True, + "joints": joint_names, + } + } moveit_controllers = { "moveit_simple_controller_manager": moveit_simple_controllers_yaml, "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", } + # moveit_controllers = PathJoinSubstitution( + # [FindPackageShare(moveit_config_package), "config", moveit_controllers_file] + # ) trajectory_execution = { "moveit_manage_controllers": False, diff --git a/motoman_ros2_control_support/urdf/common_macro.ros2_control.xacro b/motoman_ros2_control_support/urdf/common_macro.ros2_control.xacro index cc72d736..66453a68 100644 --- a/motoman_ros2_control_support/urdf/common_macro.ros2_control.xacro +++ b/motoman_ros2_control_support/urdf/common_macro.ros2_control.xacro @@ -4,6 +4,7 @@ + + + + + + + + + + + + + + + + @@ -25,42 +42,42 @@ - + 0 - + 0 - + 0 - + 0 - + 0 - + diff --git a/motoman_ros2_control_support/urdf/common_motoman.xacro b/motoman_ros2_control_support/urdf/common_motoman.xacro index 1de0adbc..f2e70646 100644 --- a/motoman_ros2_control_support/urdf/common_motoman.xacro +++ b/motoman_ros2_control_support/urdf/common_motoman.xacro @@ -3,6 +3,7 @@ + @@ -23,6 +24,7 @@ - \ No newline at end of file + From 6537c4cddd43b1013a3b3d39595841dadde81617 Mon Sep 17 00:00:00 2001 From: Daniel Azanov Date: Fri, 12 Jul 2024 14:40:44 +0200 Subject: [PATCH 2/5] allow loading config files in xml launch file --- .../config/joint_limits.yaml | 121 +++---- motoman_common_moveit/config/kinematics.yaml | 10 +- .../config/move_group_config.yaml | 31 ++ .../config/moveit_controllers.yaml | 36 +- .../config/ompl_planning.yaml | 317 +++++++++--------- .../config/pilz_capabilities.yaml | 8 +- .../config/pilz_cartesian_limits.yaml | 13 +- .../config/pilz_planning.yaml | 11 +- .../config/stomp_planning.yaml | 35 +- .../bringup_with_ros2_control.launch.py | 28 +- .../bringup_with_ros2_control.launch.xml | 147 ++++++++ 11 files changed, 469 insertions(+), 288 deletions(-) create mode 100644 motoman_common_moveit/config/move_group_config.yaml create mode 100644 motoman_common_moveit/launch/bringup_with_ros2_control.launch.xml diff --git a/motoman_common_moveit/config/joint_limits.yaml b/motoman_common_moveit/config/joint_limits.yaml index 4ae84bef..e0e499fc 100644 --- a/motoman_common_moveit/config/joint_limits.yaml +++ b/motoman_common_moveit/config/joint_limits.yaml @@ -1,61 +1,64 @@ -# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed +/**: + ros__parameters: + robot_description_planning: + # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed -# For beginners, we downscale velocity and acceleration limits. -# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. -default_velocity_scaling_factor: 0.1 -default_acceleration_scaling_factor: 0.1 + # For beginners, we downscale velocity and acceleration limits. + # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. + default_velocity_scaling_factor: 0.1 + default_acceleration_scaling_factor: 0.1 -# 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: - group_1/joint_1: - has_velocity_limits: true - max_velocity: 1.832596 - has_acceleration_limits: true - max_acceleration: 3.0 - has_deceleration_limits: true - max_deceleration: 3.0 - group_1/joint_2: - has_velocity_limits: true - max_velocity: 1.640609 - has_acceleration_limits: true - max_acceleration: 5.0 - has_deceleration_limits: true - max_deceleration: 5.0 - group_1/joint_3: - has_velocity_limits: true - max_velocity: 1.745329 - has_acceleration_limits: true - max_acceleration: 5.0 - has_deceleration_limits: true - max_deceleration: 5.0 - group_1/joint_4: - has_position_limits: true - min_position: -0.436332 # -25 - max_position: 0.436332 # 25 - has_velocity_limits: true - max_velocity: 2.373648 - has_acceleration_limits: true - max_acceleration: 3.0 - has_deceleration_limits: true - max_deceleration: 3.0 - group_1/joint_5: - has_position_limits: true - min_position: -2.18166 # -125 - max_position: 0.0 - has_velocity_limits: true - max_velocity: 2.251475 - has_acceleration_limits: true - max_acceleration: 5.0 - has_deceleration_limits: true - max_deceleration: 5.0 - group_1/joint_6: - has_position_limits: true - min_position: -3.49066 # -200 - max_position: 3.49066 # 200 - has_velocity_limits: true - max_velocity: 3.595378 - has_acceleration_limits: true - max_acceleration: 7.0 - has_deceleration_limits: true - max_deceleration: 7.0 + # 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: + group_1/joint_1: + has_velocity_limits: true + max_velocity: 1.832596 + has_acceleration_limits: true + max_acceleration: 3.0 + has_deceleration_limits: true + max_deceleration: 3.0 + group_1/joint_2: + has_velocity_limits: true + max_velocity: 1.640609 + has_acceleration_limits: true + max_acceleration: 5.0 + has_deceleration_limits: true + max_deceleration: 5.0 + group_1/joint_3: + has_velocity_limits: true + max_velocity: 1.745329 + has_acceleration_limits: true + max_acceleration: 5.0 + has_deceleration_limits: true + max_deceleration: 5.0 + group_1/joint_4: + has_position_limits: true + min_position: -0.436332 # -25 + max_position: 0.436332 # 25 + has_velocity_limits: true + max_velocity: 2.373648 + has_acceleration_limits: true + max_acceleration: 3.0 + has_deceleration_limits: true + max_deceleration: 3.0 + group_1/joint_5: + has_position_limits: true + min_position: -2.18166 # -125 + max_position: 0.0 + has_velocity_limits: true + max_velocity: 2.251475 + has_acceleration_limits: true + max_acceleration: 5.0 + has_deceleration_limits: true + max_deceleration: 5.0 + group_1/joint_6: + has_position_limits: true + min_position: -3.49066 # -200 + max_position: 3.49066 # 200 + has_velocity_limits: true + max_velocity: 3.595378 + has_acceleration_limits: true + max_acceleration: 7.0 + has_deceleration_limits: true + max_deceleration: 7.0 diff --git a/motoman_common_moveit/config/kinematics.yaml b/motoman_common_moveit/config/kinematics.yaml index ad478edd..e0835ad5 100644 --- a/motoman_common_moveit/config/kinematics.yaml +++ b/motoman_common_moveit/config/kinematics.yaml @@ -1,4 +1,6 @@ -manipulator: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin - kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 \ No newline at end of file +/**: + ros__parameters: + manipulator: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 diff --git a/motoman_common_moveit/config/move_group_config.yaml b/motoman_common_moveit/config/move_group_config.yaml new file mode 100644 index 00000000..63069d6e --- /dev/null +++ b/motoman_common_moveit/config/move_group_config.yaml @@ -0,0 +1,31 @@ +/**: + ros__parameters: + planning_pipelines: ["ompl", "pilz", "stomp"] + capabilities: [] + + ompl: + planning_plugin: ompl_interface/OMPLPlanner + request_adapters: >- + default_planner_request_adapters/AddTimeOptimalParameterization + default_planner_request_adapters/ResolveConstraintFrames + default_planner_request_adapters/FixWorkspaceBounds + default_planner_request_adapters/FixStartStateBounds + default_planner_request_adapters/FixStartStatePathConstraints + start_state_max_bounds_error: 0.1 + + pilz: + planning_plugin: pilz_industrial_motion_planner/CommandPlanner + default_planner_config: PTP + + # Trajectory execution manager parameters + moveit_manage_controllers: False + trajectory_execution: + allowed_execution_duration_scaling: 100.0 + allowed_goal_duration_margin: 0.5 + allowed_start_tolerance: 0.01 + + # Planning scene monitor parameters + publish_planning_scene: true + publish_geometry_updates: true + publish_state_updates: true + publish_transforms_updates: true diff --git a/motoman_common_moveit/config/moveit_controllers.yaml b/motoman_common_moveit/config/moveit_controllers.yaml index ff79c382..2fc79b1e 100644 --- a/motoman_common_moveit/config/moveit_controllers.yaml +++ b/motoman_common_moveit/config/moveit_controllers.yaml @@ -1,17 +1,23 @@ -moveit_simple_controller_manager: - controller_names: - - follow_joint_trajectory +/**: + ros__parameters: + moveit_simple_controller_manager: + controller_names: + - follow_joint_trajectory - follow_joint_trajectory: - action_ns: "" - type: FollowJointTrajectory - default: true - joints: - - group_1/joint_1 - - group_1/joint_2 - - group_1/joint_3 - - group_1/joint_4 - - group_1/joint_5 - - group_1/joint_6 + follow_joint_trajectory: + action_ns: "" + type: FollowJointTrajectory + default: true + joints: + - group_1/joint_1 + - group_1/joint_2 + - group_1/joint_3 + - group_1/joint_4 + - group_1/joint_5 + - group_1/joint_6 -moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + + ompl: + manipulator: + projection_evaluator: joints(group_1/joint_1,group_1/joint_2) diff --git a/motoman_common_moveit/config/ompl_planning.yaml b/motoman_common_moveit/config/ompl_planning.yaml index 06b228ad..81bf9a4d 100644 --- a/motoman_common_moveit/config/ompl_planning.yaml +++ b/motoman_common_moveit/config/ompl_planning.yaml @@ -1,157 +1,160 @@ -planner_configs: - AnytimePathShortening: - type: geometric::AnytimePathShortening - shortcut: true # Attempt to shortcut all new solution paths - hybridize: true # Compute hybrid solution trajectories - max_hybrid_paths: 24 # Number of hybrid paths generated per iteration - num_planners: 4 # The number of default planners to use for planning - planners: "" # 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]" - 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 -manipulator: - planner_configs: - - AnytimePathShortening - - SBL - - EST - - LBKPIECE - - BKPIECE - - KPIECE - - RRT - - RRTConnect - - RRTstar - - TRRT - - PRM - - PRMstar - - FMT - - BFMT - - PDST - - STRIDE - - BiTRRT - - LBTRRT - - BiEST - - ProjEST - - LazyPRM - - LazyPRMstar - - SPARS - - SPARStwo - projection_evaluator: joints(group_1/joint_1,group_1/joint_2) - longest_valid_segment_fraction: 0.005 +/**: + ros__parameters: + ompl: + planner_configs: + AnytimePathShortening: + type: geometric::AnytimePathShortening + shortcut: true # Attempt to shortcut all new solution paths + hybridize: true # Compute hybrid solution trajectories + max_hybrid_paths: 24 # Number of hybrid paths generated per iteration + num_planners: 4 # The number of default planners to use for planning + planners: "" # 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]" + 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 + manipulator: + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + # projection_evaluator: joints(group_1/joint_1,group_1/joint_2) + longest_valid_segment_fraction: 0.005 diff --git a/motoman_common_moveit/config/pilz_capabilities.yaml b/motoman_common_moveit/config/pilz_capabilities.yaml index 2ddfe5aa..8b73dbad 100644 --- a/motoman_common_moveit/config/pilz_capabilities.yaml +++ b/motoman_common_moveit/config/pilz_capabilities.yaml @@ -1,3 +1,5 @@ -capabilities: >- - pilz_industrial_motion_planner/MoveGroupSequenceAction - pilz_industrial_motion_planner/MoveGroupSequenceService \ No newline at end of file +/**: + ros__parameters: + capabilities: >- + pilz_industrial_motion_planner/MoveGroupSequenceAction + pilz_industrial_motion_planner/MoveGroupSequenceService diff --git a/motoman_common_moveit/config/pilz_cartesian_limits.yaml b/motoman_common_moveit/config/pilz_cartesian_limits.yaml index f0dd3674..03584261 100644 --- a/motoman_common_moveit/config/pilz_cartesian_limits.yaml +++ b/motoman_common_moveit/config/pilz_cartesian_limits.yaml @@ -1,5 +1,8 @@ -cartesian_limits: - max_trans_vel: 1.0 - max_trans_acc: 2.25 - max_trans_dec: -5.0 - max_rot_vel: 1.57 \ No newline at end of file +/**: + ros__parameters: + robot_description_planning: + cartesian_limits: + max_trans_vel: 1.0 + max_trans_acc: 2.25 + max_trans_dec: -5.0 + max_rot_vel: 1.57 diff --git a/motoman_common_moveit/config/pilz_planning.yaml b/motoman_common_moveit/config/pilz_planning.yaml index b6926416..80b5f291 100644 --- a/motoman_common_moveit/config/pilz_planning.yaml +++ b/motoman_common_moveit/config/pilz_planning.yaml @@ -1,4 +1,7 @@ -request_adapters: >- - default_planner_request_adapters/FixWorkspaceBounds - default_planner_request_adapters/FixStartStateBounds - default_planner_request_adapters/FixStartStatePathConstraints \ No newline at end of file +/**: + ros__parameters: + pilz: + request_adapters: >- + default_planner_request_adapters/FixWorkspaceBounds + default_planner_request_adapters/FixStartStateBounds + default_planner_request_adapters/FixStartStatePathConstraints diff --git a/motoman_common_moveit/config/stomp_planning.yaml b/motoman_common_moveit/config/stomp_planning.yaml index 399484a6..574f7512 100644 --- a/motoman_common_moveit/config/stomp_planning.yaml +++ b/motoman_common_moveit/config/stomp_planning.yaml @@ -1,17 +1,20 @@ -planning_plugin: stomp_moveit/StompPlanner -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 +/**: + ros__parameters: + stomp: + planning_plugin: stomp_moveit/StompPlanner + 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 -stomp_moveit: - num_timesteps: 60 - num_iterations: 40 - num_iterations_after_valid: 0 - num_rollouts: 30 - max_rollouts: 30 - exponentiated_cost_sensitivity: 0.5 - control_cost_weight: 0.1 - delta_t: 0.1 \ No newline at end of file + stomp_moveit: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + exponentiated_cost_sensitivity: 0.5 + control_cost_weight: 0.1 + delta_t: 0.1 diff --git a/motoman_common_moveit/launch/bringup_with_ros2_control.launch.py b/motoman_common_moveit/launch/bringup_with_ros2_control.launch.py index 68218f6c..2c1694cd 100644 --- a/motoman_common_moveit/launch/bringup_with_ros2_control.launch.py +++ b/motoman_common_moveit/launch/bringup_with_ros2_control.launch.py @@ -333,32 +333,10 @@ def generate_launch_description(): robot_description_planning_config["robot_description_planning"].update(joint_limits_yaml) - # robot_description_planning_config["robot_description_planning"].update(joint_limits_yaml) - # Trajectory Execution Functionality - # moveit_simple_controllers_yaml = load_yaml( - # "motoman_common_moveit", "config/motoman_controllers.yaml" - # ) - joint_names = [f"joint_{i}" for i in range(1, 7)] - joint_suffixes = ["s", "l", "u", "r", "b", "t"] - if True: - joint_names = [joint_name + "_" + suffix for joint_name, suffix in zip(joint_names, joint_suffixes)] - moveit_simple_controllers_yaml = { - "controller_names": ["follow_joint_trajectory"], - "follow_joint_trajectory:": { - "action_ns": "", - "type": "FollowJointTrajectory", - "default": True, - "joints": joint_names, - } - } - moveit_controllers = { - "moveit_simple_controller_manager": moveit_simple_controllers_yaml, - "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", - } - # moveit_controllers = PathJoinSubstitution( - # [FindPackageShare(moveit_config_package), "config", moveit_controllers_file] - # ) + moveit_controllers = PathJoinSubstitution( + [FindPackageShare(moveit_config_package), "config", moveit_controllers_file] + ) trajectory_execution = { "moveit_manage_controllers": False, diff --git a/motoman_common_moveit/launch/bringup_with_ros2_control.launch.xml b/motoman_common_moveit/launch/bringup_with_ros2_control.launch.xml new file mode 100644 index 00000000..920fb267 --- /dev/null +++ b/motoman_common_moveit/launch/bringup_with_ros2_control.launch.xml @@ -0,0 +1,147 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 0f549704460dad0529eb70d071fc94d6ee775c40 Mon Sep 17 00:00:00 2001 From: Daniel Azanov Date: Mon, 15 Jul 2024 16:12:58 +0200 Subject: [PATCH 3/5] fixes for moveit rolling --- motoman_common_moveit/config/kinematics.yaml | 9 +++++---- .../config/move_group_config.yaml | 17 ++++++++++------- motoman_common_moveit/config/pilz_planning.yaml | 9 +++++---- .../config/stomp_planning.yaml | 13 +++++++------ 4 files changed, 27 insertions(+), 21 deletions(-) diff --git a/motoman_common_moveit/config/kinematics.yaml b/motoman_common_moveit/config/kinematics.yaml index e0835ad5..5d70be8e 100644 --- a/motoman_common_moveit/config/kinematics.yaml +++ b/motoman_common_moveit/config/kinematics.yaml @@ -1,6 +1,7 @@ /**: ros__parameters: - manipulator: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin - kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 + robot_description_kinematics: + manipulator: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 diff --git a/motoman_common_moveit/config/move_group_config.yaml b/motoman_common_moveit/config/move_group_config.yaml index 63069d6e..e7126d7d 100644 --- a/motoman_common_moveit/config/move_group_config.yaml +++ b/motoman_common_moveit/config/move_group_config.yaml @@ -1,20 +1,23 @@ /**: ros__parameters: - planning_pipelines: ["ompl", "pilz", "stomp"] + # planning_pipelines: ["ompl", "pilz", "stomp"] + planning_pipelines: ["ompl", "pilz"] capabilities: [] ompl: planning_plugin: ompl_interface/OMPLPlanner - request_adapters: >- - default_planner_request_adapters/AddTimeOptimalParameterization - default_planner_request_adapters/ResolveConstraintFrames - default_planner_request_adapters/FixWorkspaceBounds - default_planner_request_adapters/FixStartStateBounds - default_planner_request_adapters/FixStartStatePathConstraints + planning_plugins: [ompl_interface/OMPLPlanner] + request_adapters: + # - default_planner_request_adapters/AddTimeOptimalParameterization + - default_planning_request_adapters/ResolveConstraintFrames + # - default_planner_request_adapters/FixWorkspaceBounds + # - default_planner_request_adapters/FixStartStateBounds + # - default_planner_request_adapters/FixStartStatePathConstraints start_state_max_bounds_error: 0.1 pilz: planning_plugin: pilz_industrial_motion_planner/CommandPlanner + planning_plugins: [pilz_industrial_motion_planner/CommandPlanner] default_planner_config: PTP # Trajectory execution manager parameters diff --git a/motoman_common_moveit/config/pilz_planning.yaml b/motoman_common_moveit/config/pilz_planning.yaml index 80b5f291..6f8e55e6 100644 --- a/motoman_common_moveit/config/pilz_planning.yaml +++ b/motoman_common_moveit/config/pilz_planning.yaml @@ -1,7 +1,8 @@ /**: ros__parameters: pilz: - request_adapters: >- - default_planner_request_adapters/FixWorkspaceBounds - default_planner_request_adapters/FixStartStateBounds - default_planner_request_adapters/FixStartStatePathConstraints + request_adapters: + - default_planning_request_adapters/CheckStartStateBounds + # - default_planner_request_adapters/FixWorkspaceBounds + # - default_planner_request_adapters/FixStartStateBounds + # - default_planner_request_adapters/FixStartStatePathConstraints diff --git a/motoman_common_moveit/config/stomp_planning.yaml b/motoman_common_moveit/config/stomp_planning.yaml index 574f7512..cf632243 100644 --- a/motoman_common_moveit/config/stomp_planning.yaml +++ b/motoman_common_moveit/config/stomp_planning.yaml @@ -2,12 +2,13 @@ ros__parameters: stomp: planning_plugin: stomp_moveit/StompPlanner - 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 + request_adapters: + - default_planning_request_adapters/CheckStartStateBounds + # - default_planning_request_adapters/AddTimeOptimalParameterization + # - default_planning_request_adapters/FixWorkspaceBounds + # - default_planning_request_adapters/FixStartStateBounds + # - default_planning_request_adapters/FixStartStateCollision + # - default_planning_request_adapters/FixStartStatePathConstraints stomp_moveit: num_timesteps: 60 From 1aa48ea97fe33190290b384a9eee4409fbf51716 Mon Sep 17 00:00:00 2001 From: Daniel Azanov Date: Mon, 15 Jul 2024 17:48:28 +0200 Subject: [PATCH 4/5] allow traj execution --- .../config/move_group_config.yaml | 4 +++- .../launch/bringup_with_ros2_control.launch.xml | 14 +++++++------- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/motoman_common_moveit/config/move_group_config.yaml b/motoman_common_moveit/config/move_group_config.yaml index e7126d7d..6783805a 100644 --- a/motoman_common_moveit/config/move_group_config.yaml +++ b/motoman_common_moveit/config/move_group_config.yaml @@ -2,6 +2,7 @@ ros__parameters: # planning_pipelines: ["ompl", "pilz", "stomp"] planning_pipelines: ["ompl", "pilz"] + default_planning_pipeline: ompl capabilities: [] ompl: @@ -21,7 +22,8 @@ default_planner_config: PTP # Trajectory execution manager parameters - moveit_manage_controllers: False + allow_trajectory_execution: true + moveit_manage_controllers: false trajectory_execution: allowed_execution_duration_scaling: 100.0 allowed_goal_duration_margin: 0.5 diff --git a/motoman_common_moveit/launch/bringup_with_ros2_control.launch.xml b/motoman_common_moveit/launch/bringup_with_ros2_control.launch.xml index 920fb267..fa145bb5 100644 --- a/motoman_common_moveit/launch/bringup_with_ros2_control.launch.xml +++ b/motoman_common_moveit/launch/bringup_with_ros2_control.launch.xml @@ -49,7 +49,7 @@ Source of this file are templates in https://github.com/StoglRobotics/ros_team_w description="YAML file with the controllers configuration for MoveIt."/> + description="YAML file with the move_group configuration."/> @@ -63,14 +63,14 @@ Source of this file are templates in https://github.com/StoglRobotics/ros_team_w default="$(find-pkg-share motoman_common_moveit)/config/pilz_planning.yaml" description="YAML file with the Pilz planning configuration."/> + default="$(find-pkg-share motoman_common_moveit)/config/stomp_planning.yaml" + description="YAML file with the STOMP planning configuration."/> + default="$(find-pkg-share motoman_common_moveit)/config/pilz_cartesian_limits.yaml" + description="YAML file with the Pilz cartesian limits configuration."/> + default="$(find-pkg-share motoman_common_moveit)/config/joint_limits.yaml" + description="YAML file with the joint limits configuration."/> From 43784bb8a412e136ab1233c2007e62ccad4b471a Mon Sep 17 00:00:00 2001 From: Daniel Azanov Date: Tue, 16 Jul 2024 09:17:12 +0200 Subject: [PATCH 5/5] moveit ompl, pilz, stomp are working --- motoman_common_moveit/config/kinematics.yaml | 1 + .../config/move_group_config.yaml | 26 +- .../config/ompl_planning.yaml | 12 + .../config/pilz_capabilities.yaml | 5 - .../config/pilz_planning.yaml | 12 +- .../config/stomp_planning.yaml | 14 +- .../launch/bringup.launch.py | 331 -------------- .../bringup_with_ros2_control.launch.py | 404 ------------------ .../bringup_with_ros2_control.launch.xml | 5 +- 9 files changed, 37 insertions(+), 773 deletions(-) delete mode 100644 motoman_common_moveit/config/pilz_capabilities.yaml delete mode 100644 motoman_common_moveit/launch/bringup.launch.py delete mode 100644 motoman_common_moveit/launch/bringup_with_ros2_control.launch.py diff --git a/motoman_common_moveit/config/kinematics.yaml b/motoman_common_moveit/config/kinematics.yaml index 5d70be8e..49039690 100644 --- a/motoman_common_moveit/config/kinematics.yaml +++ b/motoman_common_moveit/config/kinematics.yaml @@ -5,3 +5,4 @@ kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 diff --git a/motoman_common_moveit/config/move_group_config.yaml b/motoman_common_moveit/config/move_group_config.yaml index 6783805a..35066058 100644 --- a/motoman_common_moveit/config/move_group_config.yaml +++ b/motoman_common_moveit/config/move_group_config.yaml @@ -1,31 +1,17 @@ /**: ros__parameters: - # planning_pipelines: ["ompl", "pilz", "stomp"] - planning_pipelines: ["ompl", "pilz"] - default_planning_pipeline: ompl - capabilities: [] + planning_pipelines: ["ompl", "pilz", "stomp"] - ompl: - planning_plugin: ompl_interface/OMPLPlanner - planning_plugins: [ompl_interface/OMPLPlanner] - request_adapters: - # - default_planner_request_adapters/AddTimeOptimalParameterization - - default_planning_request_adapters/ResolveConstraintFrames - # - default_planner_request_adapters/FixWorkspaceBounds - # - default_planner_request_adapters/FixStartStateBounds - # - default_planner_request_adapters/FixStartStatePathConstraints - start_state_max_bounds_error: 0.1 - - pilz: - planning_plugin: pilz_industrial_motion_planner/CommandPlanner - planning_plugins: [pilz_industrial_motion_planner/CommandPlanner] - default_planner_config: PTP + capabilities: >- + move_group/MoveGroupExecuteTrajectoryAction + pilz_industrial_motion_planner/MoveGroupSequenceAction + pilz_industrial_motion_planner/MoveGroupSequenceService # Trajectory execution manager parameters allow_trajectory_execution: true moveit_manage_controllers: false trajectory_execution: - allowed_execution_duration_scaling: 100.0 + allowed_execution_duration_scaling: 1.2 allowed_goal_duration_margin: 0.5 allowed_start_tolerance: 0.01 diff --git a/motoman_common_moveit/config/ompl_planning.yaml b/motoman_common_moveit/config/ompl_planning.yaml index 81bf9a4d..2943ad06 100644 --- a/motoman_common_moveit/config/ompl_planning.yaml +++ b/motoman_common_moveit/config/ompl_planning.yaml @@ -1,6 +1,18 @@ /**: ros__parameters: ompl: + planning_plugins: [ompl_interface/OMPLPlanner] + request_adapters: + - default_planning_request_adapters/ResolveConstraintFrames + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision + response_adapters: + - default_planning_response_adapters/AddTimeOptimalParameterization + - default_planning_response_adapters/ValidateSolution + - default_planning_response_adapters/DisplayMotionPath + start_state_max_bounds_error: 0.1 + planner_configs: AnytimePathShortening: type: geometric::AnytimePathShortening diff --git a/motoman_common_moveit/config/pilz_capabilities.yaml b/motoman_common_moveit/config/pilz_capabilities.yaml deleted file mode 100644 index 8b73dbad..00000000 --- a/motoman_common_moveit/config/pilz_capabilities.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - capabilities: >- - pilz_industrial_motion_planner/MoveGroupSequenceAction - pilz_industrial_motion_planner/MoveGroupSequenceService diff --git a/motoman_common_moveit/config/pilz_planning.yaml b/motoman_common_moveit/config/pilz_planning.yaml index 6f8e55e6..743a2a8e 100644 --- a/motoman_common_moveit/config/pilz_planning.yaml +++ b/motoman_common_moveit/config/pilz_planning.yaml @@ -1,8 +1,14 @@ /**: ros__parameters: pilz: + planning_plugins: [pilz_industrial_motion_planner/CommandPlanner] + default_planner_config: PTP request_adapters: + - default_planning_request_adapters/ResolveConstraintFrames + - default_planning_request_adapters/ValidateWorkspaceBounds - default_planning_request_adapters/CheckStartStateBounds - # - default_planner_request_adapters/FixWorkspaceBounds - # - default_planner_request_adapters/FixStartStateBounds - # - default_planner_request_adapters/FixStartStatePathConstraints + - default_planning_request_adapters/CheckStartStateCollision + response_adapters: + - default_planning_response_adapters/AddTimeOptimalParameterization + - default_planning_response_adapters/ValidateSolution + - default_planning_response_adapters/DisplayMotionPath diff --git a/motoman_common_moveit/config/stomp_planning.yaml b/motoman_common_moveit/config/stomp_planning.yaml index cf632243..f83524ed 100644 --- a/motoman_common_moveit/config/stomp_planning.yaml +++ b/motoman_common_moveit/config/stomp_planning.yaml @@ -1,14 +1,16 @@ /**: ros__parameters: stomp: - planning_plugin: stomp_moveit/StompPlanner + planning_plugins: [stomp_moveit/StompPlanner] request_adapters: + - default_planning_request_adapters/ResolveConstraintFrames + - default_planning_request_adapters/ValidateWorkspaceBounds - default_planning_request_adapters/CheckStartStateBounds - # - default_planning_request_adapters/AddTimeOptimalParameterization - # - default_planning_request_adapters/FixWorkspaceBounds - # - default_planning_request_adapters/FixStartStateBounds - # - default_planning_request_adapters/FixStartStateCollision - # - default_planning_request_adapters/FixStartStatePathConstraints + - default_planning_request_adapters/CheckStartStateCollision + response_adapters: + - default_planning_response_adapters/AddTimeOptimalParameterization + - default_planning_response_adapters/ValidateSolution + - default_planning_response_adapters/DisplayMotionPath stomp_moveit: num_timesteps: 60 diff --git a/motoman_common_moveit/launch/bringup.launch.py b/motoman_common_moveit/launch/bringup.launch.py deleted file mode 100644 index 8c3f0380..00000000 --- a/motoman_common_moveit/launch/bringup.launch.py +++ /dev/null @@ -1,331 +0,0 @@ -# Copyright 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import os -import yaml -import sys -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution -from launch.conditions import IfCondition, UnlessCondition -from launch_ros.actions import Node -from launch.actions import ExecuteProcess -from ament_index_python.packages import get_package_share_directory -import xacro -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) - - try: - with open(absolute_file_path, "r") as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -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) - - 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 None - - -def generate_launch_description(): - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "robot_name", - default_value="motoman", - description="Name of the robot or application for unique identification.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value="group_1/", - description="Prefix of the joint names, useful for \ - multi-robot setup. If changed than also joint names in the controllers' configuration \ - have to be updated.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_package", - default_value = "motoman_gp250_support", - description="Description package with robot URDF/xacro files.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_macro_file", - default_value = "gp250.xacro", - description="URDF/XACRO description file with of the robot or application. \ - The expected location of the file is '/urdf/'.", - ) - ) - - declared_arguments.append( - DeclareLaunchArgument( - "configuration_package", - default_value="motoman_ros2_control_support", - description="The package with the yaml configurations", - ) - ) - - declared_arguments.append( - DeclareLaunchArgument( - "controllers_file", - default_value="motoman_6dof_controllers.yaml", - description="YAML file with the controllers configuration. \ - The expected location of the file is '/config/'.", - ) - ) - - declared_arguments.append( - DeclareLaunchArgument( - "use_mock_hardware", - default_value="true", - description="Start robot with fake hardware mirroring command to its states.", - ) - ) - - declared_arguments.append( - DeclareLaunchArgument( - "moveit_config_package", - default_value="motoman_common_moveit", - description="Package with semantic robot description needed for MoveIt2.", - ) - ) - - declared_arguments.append( - DeclareLaunchArgument( - "semantic_description_file", - default_value="common_motoman.srdf.xacro", - description="Semantic robot description file (SRDF/XACRO) with of the robot or \ - application. The expected location of the file is '/srdf/'.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "rviz_file", - default_value="moveit.rviz", - description="Rviz2 configuration file of the visualization. \ - The expected location of the file is '/rviz/'.", - ) - ) - - # initialize arguments - robot_name = LaunchConfiguration("robot_name") - prefix = LaunchConfiguration("prefix") - description_package = LaunchConfiguration("description_package") - description_macro_file = LaunchConfiguration("description_macro_file") - configuration_package = LaunchConfiguration("configuration_package") - controllers_file = LaunchConfiguration("controllers_file") - - use_mock_hardware = LaunchConfiguration("use_mock_hardware") - - moveit_config_package = LaunchConfiguration("moveit_config_package") - semantic_description_file = LaunchConfiguration("semantic_description_file") - rviz_file = LaunchConfiguration("rviz_file") - - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare(description_package), "urdf", description_macro_file] - ), - " ", - "robot_name:=", - robot_name, - " ", - "prefix:=", - prefix, - " ", - ] - ) - robot_description = {"robot_description": robot_description_content} - - # Publish TF - robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - name="robot_state_publisher_node", - output="both", - parameters=[robot_description] - ) - - # MoveIt2 Configuration - robot_description_semantic_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare(moveit_config_package), "srdf", semantic_description_file] - ), - " ", - "robot_name:=", - robot_name, - " ", - "prefix:=", - prefix, - ] - ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_content - } - - kinematics_yaml = load_yaml( - "motoman_common_moveit", "config/kinematics.yaml" - ) - move_group_config = { - "planning_pipelines": ["ompl", "pilz", "stomp"], - "capabilities": [], - } - # Planning Functionality - ompl_planning_pipeline_config = { - "ompl": { - "planning_plugin": "ompl_interface/OMPLPlanner", - "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStatePathConstraints""", - "start_state_max_bounds_error": 0.1, - } - } - - pilz_planning_pipeline_config = { - "pilz": { - "planning_plugin": "pilz_industrial_motion_planner/CommandPlanner", - "default_planner_config": "PTP" - }, - #"pilz_lin": { - # "planning_plugin": "pilz_industrial_motion_planner/CommandPlanner", - # "default_planner_config": "LIN", - #} - } - ompl_planning_yaml = load_yaml( - "motoman_common_moveit", "config/ompl_planning.yaml" - ) - ompl_planning_pipeline_config["ompl"].update(ompl_planning_yaml) - # WARNING default_planner_request_adapters/FixStartStateCollision might cause jumps if the robot is in a slight collision at start, deactivating it for now - # see https://github.com/ros-planning/moveit/issues/2268 - pilz_planning_yaml = load_yaml( - "motoman_common_moveit", "config/pilz_planning.yaml" - ) - pilz_planning_pipeline_config["pilz"].update(pilz_planning_yaml) - - pilz_capabilities_yaml = load_yaml( - "motoman_common_moveit", "config/pilz_capabilities.yaml" - ) - move_group_config.update(pilz_capabilities_yaml) - - pilz_limits_yaml = load_yaml( - "motoman_common_moveit", "config/pilz_cartesian_limits.yaml" - ) - robot_description_planning_config = { - "robot_description_planning" : pilz_limits_yaml - } - joint_limits_yaml = load_yaml( - "motoman_common_moveit", "config/joint_limits.yaml" - ) - - stomp_planning_pipeline_config = { - "stomp": { - "planning_plugin": "stomp_moveit/StompPlanner", - } - } - - stomp_planning_yaml = load_yaml( - "motoman_common_moveit", "config/stomp_planning.yaml" - ) - stomp_planning_pipeline_config["stomp"].update(stomp_planning_yaml) - - robot_description_planning_config["robot_description_planning"].update(joint_limits_yaml) - - # Trajectory Execution Functionality - moveit_simple_controllers_yaml = load_yaml( - "motoman_common_moveit", "config/motoman_controllers.yaml" - ) - moveit_controllers = { - "moveit_simple_controller_manager": moveit_simple_controllers_yaml, - "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", - } - - trajectory_execution = { - "moveit_manage_controllers": False, - "trajectory_execution.allowed_execution_duration_scaling": 100.0, - "trajectory_execution.allowed_goal_duration_margin": 0.5, - "trajectory_execution.allowed_start_tolerance": 0.01, - } - - planning_scene_monitor_parameters = { - "publish_planning_scene": True, - "publish_geometry_updates": True, - "publish_state_updates": True, - "publish_transforms_updates": True, - } - - # MoveIt2 - moveit_node = Node( - package="moveit_ros_move_group", - executable="move_group", - output="screen", - parameters=[ - robot_description, - robot_description_semantic, - kinematics_yaml, - move_group_config, - ompl_planning_pipeline_config, - pilz_planning_pipeline_config, - stomp_planning_pipeline_config, - robot_description_planning_config, - trajectory_execution, - moveit_controllers, - planning_scene_monitor_parameters, - ], - ) - - # Rviz2 - rviz_config = PathJoinSubstitution( - [FindPackageShare(moveit_config_package), "rviz", rviz_file]) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - #output="log", - output={ - 'stdout': 'log', - 'stderr': 'log' - }, - arguments=["-d", rviz_config], - parameters=[ - robot_description, - robot_description_semantic, - ompl_planning_pipeline_config, - kinematics_yaml, - ], - ) - - return LaunchDescription( - declared_arguments + [ - robot_state_publisher_node, - moveit_node, - rviz_node, - ] - ) diff --git a/motoman_common_moveit/launch/bringup_with_ros2_control.launch.py b/motoman_common_moveit/launch/bringup_with_ros2_control.launch.py deleted file mode 100644 index 2c1694cd..00000000 --- a/motoman_common_moveit/launch/bringup_with_ros2_control.launch.py +++ /dev/null @@ -1,404 +0,0 @@ -# Copyright 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import os -import yaml -import sys -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution -from launch.conditions import IfCondition, UnlessCondition -from launch_ros.actions import Node -from launch.actions import ExecuteProcess -from ament_index_python.packages import get_package_share_directory -import xacro -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) - - try: - with open(absolute_file_path, "r") as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -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) - - 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 None - - -def generate_launch_description(): - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "robot_name", - default_value="motoman", - description="Name of the robot or application for unique identification.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value="", - description="Prefix of the joint names, useful for \ - multi-robot setup. If changed than also joint names in the controllers' configuration \ - have to be updated.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "joints_with_suffix", - default_value="false", - description="Whether the joint names have a suffix, \ - i.e. [joint_1_s, joint_2_l, joint_3_u, joint_4_r, joint_5_b, joint_6_t]", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_package", - default_value = "motoman_gp250_support", - description="Description package with robot URDF/xacro files.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_macro_file", - default_value = "gp250_macro.xacro", - description="URDF/XACRO description file with of the robot or application. \ - The expected location of the file is '/urdf/'.", - ) - ) - - declared_arguments.append( - DeclareLaunchArgument( - "configuration_package", - default_value="motoman_ros2_control_support", - description="The package with the yaml configurations", - ) - ) - - declared_arguments.append( - DeclareLaunchArgument( - "controllers_file", - default_value="motoman_6dof_controllers.yaml", - description="YAML file with the controllers configuration. \ - The expected location of the file is '/config/'.", - ) - ) - - declared_arguments.append( - DeclareLaunchArgument( - "use_mock_hardware", - default_value="true", - description="Start robot with fake hardware mirroring command to its states.", - ) - ) - - declared_arguments.append( - DeclareLaunchArgument( - "moveit_config_package", - default_value="motoman_common_moveit", - description="Package with semantic robot description needed for MoveIt2.", - ) - ) - - declared_arguments.append( - DeclareLaunchArgument( - "moveit_controllers_file", - default_value="moveit_controllers.yaml", - description="YAML file with the controllers configuration. \ - The expected location of the file is '/config/'.", - ) - ) - - declared_arguments.append( - DeclareLaunchArgument( - "semantic_description_file", - default_value="common_motoman.srdf.xacro", - description="Semantic robot description file (SRDF/XACRO) with of the robot or \ - application. The expected location of the file is '/srdf/'.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "rviz_file", - default_value="moveit.rviz", - description="Rviz2 configuration file of the visualization. \ - The expected location of the file is '/rviz/'.", - ) - ) - - declared_arguments.append( - DeclareLaunchArgument( - "activate_ros2_control", - default_value="true", - description="Decide if this file should also start ros2_control stack and activate\ - controllers. This is useful for testing, but nor advised in production." - ) - ) - - # initialize arguments - robot_name = LaunchConfiguration("robot_name") - prefix = LaunchConfiguration("prefix") - joints_with_suffix = LaunchConfiguration("joints_with_suffix") - description_package = LaunchConfiguration("description_package") - description_macro_file = LaunchConfiguration("description_macro_file") - configuration_package = LaunchConfiguration("configuration_package") - controllers_file = LaunchConfiguration("controllers_file") - - use_mock_hardware = LaunchConfiguration("use_mock_hardware") - - moveit_config_package = LaunchConfiguration("moveit_config_package") - moveit_controllers_file = LaunchConfiguration("moveit_controllers_file") - semantic_description_file = LaunchConfiguration("semantic_description_file") - rviz_file = LaunchConfiguration("rviz_file") - - activate_ros2_control = LaunchConfiguration("activate_ros2_control") - - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare("motoman_ros2_control_support"), "urdf", "common_motoman.xacro"] - ), - " ", - "robot_name:=", - robot_name, - " ", - "prefix:=", - prefix, - " ", - "joints_with_suffix:=", - joints_with_suffix, - " ", - "description_package:=", - description_package, - " ", - "description_macro_file:=", - description_macro_file, - " ", - "use_mock_hardware:=", - use_mock_hardware, - " ", - ] - ) - robot_description = {"robot_description": robot_description_content} - - # Publish TF - robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - name="robot_state_publisher_node", - output="both", - parameters=[robot_description] - ) - - # ros2_control_node - robot_controllers = PathJoinSubstitution( - [FindPackageShare(configuration_package), "config", controllers_file] - ) - ros2_control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_description, robot_controllers], - output={ - "stdout": "screen", - "stderr": "screen", - }, - remappings=[ - ('/position_trajectory_controller/follow_joint_trajectory/_action/feedback', '/follow_joint_trajectory/_action/feedback'), - ('/position_trajectory_controller/follow_joint_trajectory/_action/status', '/follow_joint_trajectory/_action/status'), - ('/position_trajectory_controller/follow_joint_trajectory/_action/cancel_goal', '/follow_joint_trajectory/_action/cancel_goal'), - ('/position_trajectory_controller/follow_joint_trajectory/_action/get_result', '/follow_joint_trajectory/_action/get_result'), - ('/position_trajectory_controller/follow_joint_trajectory/_action/send_goal', '/follow_joint_trajectory/_action/send_goal'), - ], - ) - - # Spawn controllers - load_and_activate_controllers = [] - for controller in ["position_trajectory_controller", "joint_state_broadcaster"]: - load_and_activate_controllers += [ - ExecuteProcess( - cmd=[f"ros2 run controller_manager spawner {controller}"], - shell=True, - output="screen", - condition=IfCondition(activate_ros2_control), - ) - ] - - # MoveIt2 Configuration - robot_description_semantic_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare(moveit_config_package), "srdf", semantic_description_file] - ), - " ", - "robot_name:=", - robot_name, - " ", - "prefix:=", - prefix, - ] - ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_content - } - - kinematics_yaml = load_yaml( - "motoman_common_moveit", "config/kinematics.yaml" - ) - move_group_config = { - "planning_pipelines": ["ompl", "pilz", "stomp"], - "capabilities": [], - } - # Planning Functionality - ompl_planning_pipeline_config = { - "ompl": { - "planning_plugin": "ompl_interface/OMPLPlanner", - "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStatePathConstraints""", - "start_state_max_bounds_error": 0.1, - } - } - - pilz_planning_pipeline_config = { - "pilz": { - "planning_plugin": "pilz_industrial_motion_planner/CommandPlanner", - "default_planner_config": "PTP" - }, - #"pilz_lin": { - # "planning_plugin": "pilz_industrial_motion_planner/CommandPlanner", - # "default_planner_config": "LIN", - #} - } - ompl_planning_yaml = load_yaml( - "motoman_common_moveit", "config/ompl_planning.yaml" - ) - ompl_planning_pipeline_config["ompl"].update(ompl_planning_yaml) - # WARNING default_planner_request_adapters/FixStartStateCollision might cause jumps if the robot is in a slight collision at start, deactivating it for now - # see https://github.com/ros-planning/moveit/issues/2268 - pilz_planning_yaml = load_yaml( - "motoman_common_moveit", "config/pilz_planning.yaml" - ) - pilz_planning_pipeline_config["pilz"].update(pilz_planning_yaml) - - pilz_capabilities_yaml = load_yaml( - "motoman_common_moveit", "config/pilz_capabilities.yaml" - ) - move_group_config.update(pilz_capabilities_yaml) - - pilz_limits_yaml = load_yaml( - "motoman_common_moveit", "config/pilz_cartesian_limits.yaml" - ) - robot_description_planning_config = { - "robot_description_planning" : pilz_limits_yaml - } - joint_limits_yaml = load_yaml( - "motoman_common_moveit", "config/joint_limits.yaml" - ) - - stomp_planning_pipeline_config = { - "stomp": { - "planning_plugin": "stomp_moveit/StompPlanner", - } - } - - stomp_planning_yaml = load_yaml( - "motoman_common_moveit", "config/stomp_planning.yaml" - ) - stomp_planning_pipeline_config["stomp"].update(stomp_planning_yaml) - - robot_description_planning_config["robot_description_planning"].update(joint_limits_yaml) - - # Trajectory Execution Functionality - moveit_controllers = PathJoinSubstitution( - [FindPackageShare(moveit_config_package), "config", moveit_controllers_file] - ) - - trajectory_execution = { - "moveit_manage_controllers": False, - "trajectory_execution.allowed_execution_duration_scaling": 100.0, - "trajectory_execution.allowed_goal_duration_margin": 0.5, - "trajectory_execution.allowed_start_tolerance": 0.01, - } - - planning_scene_monitor_parameters = { - "publish_planning_scene": True, - "publish_geometry_updates": True, - "publish_state_updates": True, - "publish_transforms_updates": True, - } - - # MoveIt2 - moveit_node = Node( - package="moveit_ros_move_group", - executable="move_group", - output="screen", - parameters=[ - robot_description, - robot_description_semantic, - kinematics_yaml, - move_group_config, - ompl_planning_pipeline_config, - pilz_planning_pipeline_config, - stomp_planning_pipeline_config, - robot_description_planning_config, - trajectory_execution, - moveit_controllers, - planning_scene_monitor_parameters, - ], - ) - - # # Rviz2 - rviz_config = PathJoinSubstitution( - [FindPackageShare(moveit_config_package), "rviz", rviz_file]) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - #output="log", - output={ - 'stdout': 'log', - 'stderr': 'log' - }, - arguments=["-d", rviz_config], - parameters=[ - robot_description, - robot_description_semantic, - ompl_planning_pipeline_config, - kinematics_yaml, - ], - ) - - return LaunchDescription( - declared_arguments + [ - robot_state_publisher_node, - ros2_control_node, - moveit_node, - rviz_node, - ] - + load_and_activate_controllers - ) diff --git a/motoman_common_moveit/launch/bringup_with_ros2_control.launch.xml b/motoman_common_moveit/launch/bringup_with_ros2_control.launch.xml index fa145bb5..d1816352 100644 --- a/motoman_common_moveit/launch/bringup_with_ros2_control.launch.xml +++ b/motoman_common_moveit/launch/bringup_with_ros2_control.launch.xml @@ -53,9 +53,6 @@ Source of this file are templates in https://github.com/StoglRobotics/ros_team_w - @@ -126,7 +123,6 @@ Source of this file are templates in https://github.com/StoglRobotics/ros_team_w - @@ -142,6 +138,7 @@ Source of this file are templates in https://github.com/StoglRobotics/ros_team_w +