Add Anytime Path Shortening

Author: amalnanavati

ada_moveit/config/ompl_planning.yaml

@@ -11,6 +11,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()
@@ -68,6 +75,7 @@ jaco_arm:
   enforce_constrained_state_space: true
   projection_evaluator: joints(j2n6s200_joint_1,j2n6s200_joint_2)
   planner_configs:
+    - AnytimePathShortening
     - SBLkConfigDefault
     - ESTkConfigDefault
     - LBKPIECEkConfigDefault