From 9bd50ecb739998684ee17613cf920edd79216c94 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Wed, 13 Nov 2024 19:12:32 -0800 Subject: [PATCH 1/3] Add Anytime Path Shortening --- ada_moveit/config/ompl_planning.yaml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/ada_moveit/config/ompl_planning.yaml b/ada_moveit/config/ompl_planning.yaml index f4cc886..16b0a0a 100644 --- a/ada_moveit/config/ompl_planning.yaml +++ b/ada_moveit/config/ompl_planning.yaml @@ -8,6 +8,13 @@ request_adapters: >- default_planner_request_adapters/FixWorkspaceBounds # Based on Kinova's parameters: https://github.com/Kinovarobotics/kinova-ros/blob/noetic-devel/kinova_moveit/robot_configs/j2n6s300_moveit_config/config/ompl_planning.yaml planner_configs: + AnytimePathShortening: + type: geometric::AnytimePathShortening + shortcut: 1 # Attempt to shortcut all new solution paths + hybridize: 1 # 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: "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() @@ -65,6 +72,7 @@ jaco_arm: enforce_constrained_state_space: true projection_evaluator: joints(j2n6s200_joint_1,j2n6s200_joint_2) planner_configs: + - AnytimePathShortening - SBLkConfigDefault - ESTkConfigDefault - LBKPIECEkConfigDefault From 130109d6bce5b6bc1939cb05e848d358b33fc2f8 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Mon, 2 Dec 2024 13:17:51 -0800 Subject: [PATCH 2/3] Tune parameters --- ada_moveit/config/ompl_planning.yaml | 9 ++++----- ada_moveit/launch/demo.launch.py | 4 +++- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/ada_moveit/config/ompl_planning.yaml b/ada_moveit/config/ompl_planning.yaml index 16b0a0a..2191d2a 100644 --- a/ada_moveit/config/ompl_planning.yaml +++ b/ada_moveit/config/ompl_planning.yaml @@ -12,9 +12,9 @@ planner_configs: type: geometric::AnytimePathShortening shortcut: 1 # Attempt to shortcut all new solution paths hybridize: 1 # Compute hybrid solution trajectories - max_hybrid_paths: 24 # Number of hybrid paths generated per iteration + max_hybrid_paths: 8 # Number of hybrid paths generated per iteration num_planners: 4 # The number of default planners to use for planning - planners: "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]" + planners: "RRTConnect[intermediate_states=0 range=0.05]" # 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() @@ -46,7 +46,8 @@ planner_configs: 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() + intermediate_states: 0 # Whether to add intermediate states to the plan, default 0 + range: 0.05 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() RRTstarkConfigDefault: type: geometric::RRTstar range: 3.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() (in practice, for ADA, this default is ~7) @@ -85,8 +86,6 @@ jaco_arm: - PRMkConfigDefault - PRMstarkConfigDefault hand: - enforce_constrained_state_space: true - projection_evaluator: joints(j2n6s200_joint_1,j2n6s200_joint_2) planner_configs: - SBLkConfigDefault - ESTkConfigDefault diff --git a/ada_moveit/launch/demo.launch.py b/ada_moveit/launch/demo.launch.py index c0793b2..97608b6 100644 --- a/ada_moveit/launch/demo.launch.py +++ b/ada_moveit/launch/demo.launch.py @@ -250,7 +250,9 @@ def generate_launch_description(): package="controller_manager", executable="ros2_control_node", parameters=[robot_description, robot_controllers], - arguments=["--ros-args", "--log-level", log_level], + # Commented out the log-level since the joint state publisher logs every joint read + # when on debug level + arguments=["--ros-args"],#, "--log-level", log_level], ) ) From 5ea90f5d4bd82364d168de26ec8d31a7609e7ceb Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Mon, 2 Dec 2024 20:34:26 -0800 Subject: [PATCH 3/3] Further tune range --- ada_moveit/config/ompl_planning.yaml | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/ada_moveit/config/ompl_planning.yaml b/ada_moveit/config/ompl_planning.yaml index 2191d2a..e402927 100644 --- a/ada_moveit/config/ompl_planning.yaml +++ b/ada_moveit/config/ompl_planning.yaml @@ -8,13 +8,15 @@ request_adapters: >- default_planner_request_adapters/FixWorkspaceBounds # Based on Kinova's parameters: https://github.com/Kinovarobotics/kinova-ros/blob/noetic-devel/kinova_moveit/robot_configs/j2n6s300_moveit_config/config/ompl_planning.yaml planner_configs: + # NOTE: In practice, MoveIt2 hybridizes and shortcuts by default: + # https://github.com/moveit/moveit2/blob/e7872ebf92146bb80930ec98cf6c181e8384e99c/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp#L81 AnytimePathShortening: type: geometric::AnytimePathShortening shortcut: 1 # Attempt to shortcut all new solution paths hybridize: 1 # Compute hybrid solution trajectories max_hybrid_paths: 8 # Number of hybrid paths generated per iteration num_planners: 4 # The number of default planners to use for planning - planners: "RRTConnect[intermediate_states=0 range=0.05]" # 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]" + planners: "RRTConnect[range=0.01]" # 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() @@ -47,7 +49,7 @@ planner_configs: RRTConnectkConfigDefault: type: geometric::RRTConnect intermediate_states: 0 # Whether to add intermediate states to the plan, default 0 - range: 0.05 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + range: 0.01 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() RRTstarkConfigDefault: type: geometric::RRTstar range: 3.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() (in practice, for ADA, this default is ~7)