From a2ce5dfb49f262e1dcdd3f75730b30978598aa5e Mon Sep 17 00:00:00 2001 From: Nibanovic Date: Fri, 5 Apr 2024 15:12:46 +0200 Subject: [PATCH] separate ompl_planning.yaml config, refactor config --- .../src/myrobot_moveit/config/move_group.yaml | 79 +++++++++++++++++++ .../config/{all.yaml => ompl_planning.yaml} | 79 ------------------- .../config_old/joint_limits.yaml | 40 ---------- .../myrobot_moveit/config_old/kinematics.yaml | 5 -- .../config_old/moveit_controllers.yaml | 24 ------ .../config_old/ompl_planning.yaml | 69 ---------------- .../myrobot_moveit/launch/moveit.launch.py | 21 +++-- 7 files changed, 88 insertions(+), 229 deletions(-) create mode 100644 myrobot_ws/src/myrobot_moveit/config/move_group.yaml rename myrobot_ws/src/myrobot_moveit/config/{all.yaml => ompl_planning.yaml} (64%) delete mode 100644 myrobot_ws/src/myrobot_moveit/config_old/joint_limits.yaml delete mode 100644 myrobot_ws/src/myrobot_moveit/config_old/kinematics.yaml delete mode 100644 myrobot_ws/src/myrobot_moveit/config_old/moveit_controllers.yaml delete mode 100644 myrobot_ws/src/myrobot_moveit/config_old/ompl_planning.yaml diff --git a/myrobot_ws/src/myrobot_moveit/config/move_group.yaml b/myrobot_ws/src/myrobot_moveit/config/move_group.yaml new file mode 100644 index 00000000..0264c560 --- /dev/null +++ b/myrobot_ws/src/myrobot_moveit/config/move_group.yaml @@ -0,0 +1,79 @@ +/**: + ros__parameters: +#-------- Moveit controllers + moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + moveit_simple_controller_manager: + controller_names: + - joint_trajectory_controller # add or remove controllers as necessary + + joint_trajectory_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + +#-------- kinematics + robot_description_kinematics: + myrobot_manipulator: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 + +#-------- joint limits + robot_description_planning: + 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: + joint1: + has_velocity_limits: true + max_velocity: 1.7453292519943295 + has_acceleration_limits: true + max_acceleration: 5.0 + joint2: + has_velocity_limits: true + max_velocity: 1.5707963267948966 + has_acceleration_limits: true + max_acceleration: 5.0 + joint3: + has_velocity_limits: true + max_velocity: 1.5707963267948966 + has_acceleration_limits: true + max_acceleration: 5.0 + joint4: + has_velocity_limits: true + max_velocity: 2.9670597283903604 + has_acceleration_limits: true + max_acceleration: 5.0 + joint5: + has_velocity_limits: true + max_velocity: 2.0943951023931953 + has_acceleration_limits: true + max_acceleration: 5.0 + joint6: + has_velocity_limits: true + max_velocity: 3.3161255787892263 + has_acceleration_limits: true + max_acceleration: 5.0 + +#-------- other + moveit_manage_controllers: false + trajectory_execution: + allowed_execution_duration_scaling: 1.2 + allowed_goal_duration_margin: 0.5 + allowed_start_tolerance: 0.01 + capabilities: move_group/MoveGroupExecuteTrajectoryAction + publish_planning_scene: True + publish_geometry_updates: True + publish_state_updates: True + publish_transforms_updates: True + + \ No newline at end of file diff --git a/myrobot_ws/src/myrobot_moveit/config/all.yaml b/myrobot_ws/src/myrobot_moveit/config/ompl_planning.yaml similarity index 64% rename from myrobot_ws/src/myrobot_moveit/config/all.yaml rename to myrobot_ws/src/myrobot_moveit/config/ompl_planning.yaml index 12256ddb..d198a254 100644 --- a/myrobot_ws/src/myrobot_moveit/config/all.yaml +++ b/myrobot_ws/src/myrobot_moveit/config/ompl_planning.yaml @@ -1,84 +1,5 @@ /**: ros__parameters: - # from launch file - capabilities: move_group/MoveGroupExecuteTrajectoryAction - publish_planning_scene: True - publish_geometry_updates: True - publish_state_updates: True - publish_transforms_updates: True - - # kinematics - robot_description_kinematics: - myrobot_manipulator: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin - kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 3 - - # joint limits - robot_description_planning: - 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: - joint1: - has_velocity_limits: true - max_velocity: 1.7453292519943295 - has_acceleration_limits: true - max_acceleration: 5.0 - joint2: - has_velocity_limits: true - max_velocity: 1.5707963267948966 - has_acceleration_limits: true - max_acceleration: 5.0 - joint3: - has_velocity_limits: true - max_velocity: 1.5707963267948966 - has_acceleration_limits: true - max_acceleration: 5.0 - joint4: - has_velocity_limits: true - max_velocity: 2.9670597283903604 - has_acceleration_limits: true - max_acceleration: 5.0 - joint5: - has_velocity_limits: true - max_velocity: 2.0943951023931953 - has_acceleration_limits: true - max_acceleration: 5.0 - joint6: - has_velocity_limits: true - max_velocity: 3.3161255787892263 - has_acceleration_limits: true - max_acceleration: 5.0 - - # Moveit Controllers - moveit_manage_controllers: false - trajectory_execution: - allowed_execution_duration_scaling: 1.2 - allowed_goal_duration_margin: 0.5 - allowed_start_tolerance: 0.01 - - moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager - moveit_simple_controller_manager: - controller_names: - - joint_trajectory_controller - - joint_trajectory_controller: - type: FollowJointTrajectory - action_ns: follow_joint_trajectory - default: true - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - - # OMPL planning move_group: planning_plugins: - ompl_interface/OMPLPlanner diff --git a/myrobot_ws/src/myrobot_moveit/config_old/joint_limits.yaml b/myrobot_ws/src/myrobot_moveit/config_old/joint_limits.yaml deleted file mode 100644 index 92ff4b51..00000000 --- a/myrobot_ws/src/myrobot_moveit/config_old/joint_limits.yaml +++ /dev/null @@ -1,40 +0,0 @@ -# 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 - -# 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: 1.7453292519943295 - has_acceleration_limits: true - max_acceleration: 5.0 - joint2: - has_velocity_limits: true - max_velocity: 1.5707963267948966 - has_acceleration_limits: true - max_acceleration: 5.0 - joint3: - has_velocity_limits: true - max_velocity: 1.5707963267948966 - has_acceleration_limits: true - max_acceleration: 5.0 - joint4: - has_velocity_limits: true - max_velocity: 2.9670597283903604 - has_acceleration_limits: true - max_acceleration: 5.0 - joint5: - has_velocity_limits: true - max_velocity: 2.0943951023931953 - has_acceleration_limits: true - max_acceleration: 5.0 - joint6: - has_velocity_limits: true - max_velocity: 3.3161255787892263 - has_acceleration_limits: true - max_acceleration: 5.0 \ No newline at end of file diff --git a/myrobot_ws/src/myrobot_moveit/config_old/kinematics.yaml b/myrobot_ws/src/myrobot_moveit/config_old/kinematics.yaml deleted file mode 100644 index 59e9d690..00000000 --- a/myrobot_ws/src/myrobot_moveit/config_old/kinematics.yaml +++ /dev/null @@ -1,5 +0,0 @@ -myrobot_manipulator: - 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/myrobot_ws/src/myrobot_moveit/config_old/moveit_controllers.yaml b/myrobot_ws/src/myrobot_moveit/config_old/moveit_controllers.yaml deleted file mode 100644 index d2b3042f..00000000 --- a/myrobot_ws/src/myrobot_moveit/config_old/moveit_controllers.yaml +++ /dev/null @@ -1,24 +0,0 @@ -# MoveIt uses this configuration for controller management -moveit_manage_controllers: false -trajectory_execution: - allowed_execution_duration_scaling: 1.2 - allowed_goal_duration_margin: 0.5 - allowed_start_tolerance: 0.01 - -moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager - -moveit_simple_controller_manager: - controller_names: - - joint_trajectory_controller - - joint_trajectory_controller: - type: FollowJointTrajectory - action_ns: follow_joint_trajectory - default: true - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 \ No newline at end of file diff --git a/myrobot_ws/src/myrobot_moveit/config_old/ompl_planning.yaml b/myrobot_ws/src/myrobot_moveit/config_old/ompl_planning.yaml deleted file mode 100644 index 645cf70c..00000000 --- a/myrobot_ws/src/myrobot_moveit/config_old/ompl_planning.yaml +++ /dev/null @@ -1,69 +0,0 @@ -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 -myrobot_manipulator: - planner_configs: - SBLkConfigDefault: - type: geometric::SBL - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - ESTkConfigDefault: - 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 - LBKPIECEkConfigDefault: - 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 - BKPIECEkConfigDefault: - 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 - KPIECEkConfigDefault: - 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 - RRTkConfigDefault: - 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 - RRTConnectkConfigDefault: - type: geometric::RRTConnect - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - RRTstarkConfigDefault: - 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 - TRRTkConfigDefault: - 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 expression. default: 0.0 set in setup() - PRMkConfigDefault: - type: geometric::PRM - max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 - PRMstarkConfigDefault: - type: geometric::PRMstar - ##Note: commenting the following line lets moveit chose RRTConnect as default planner rather than LBKPIECE - #projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint) - longest_valid_segment_fraction: 0.01 \ No newline at end of file diff --git a/myrobot_ws/src/myrobot_moveit/launch/moveit.launch.py b/myrobot_ws/src/myrobot_moveit/launch/moveit.launch.py index c1c0f344..ba31e8ad 100644 --- a/myrobot_ws/src/myrobot_moveit/launch/moveit.launch.py +++ b/myrobot_ws/src/myrobot_moveit/launch/moveit.launch.py @@ -1,6 +1,3 @@ -import os -import yaml - from launch.actions import DeclareLaunchArgument from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node @@ -43,8 +40,6 @@ def launch_setup(context, *args, **kwargs): " ", ] ) - robot_description = {"robot_description": robot_description_content.perform(context)} - # SRDF robot_description_semantic_content = Command( [ @@ -53,14 +48,13 @@ def launch_setup(context, *args, **kwargs): PathJoinSubstitution( [FindPackageShare(moveit_package), "srdf", semantic_description_file] ), - ] ) - robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content.perform(context)} - other_configs = PathJoinSubstitution( - [FindPackageShare(moveit_package), "config", "all.yaml"] - ) + robot_description = {"robot_description": robot_description_content.perform(context)} + robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content.perform(context)} + planning_config = PathJoinSubstitution([FindPackageShare(moveit_package), "config", "ompl_planning.yaml"]) + move_group_config = PathJoinSubstitution([FindPackageShare(moveit_package), "config", "move_group.yaml"]) # -------------------------------------------------------# # Move Group Node # @@ -73,7 +67,8 @@ def launch_setup(context, *args, **kwargs): parameters=[ robot_description, robot_description_semantic, - other_configs, + planning_config, + move_group_config, {"use_sim_time": use_sim_time}, ], ) @@ -90,7 +85,9 @@ def launch_setup(context, *args, **kwargs): parameters=[ robot_description, robot_description_semantic, - other_configs + planning_config, + move_group_config, + {"use_sim_time": use_sim_time}, ], )