Add Stomp and AnytimePathShortening (#47)

* Add Stomp and AnytimePathShortening planner config info * Fix bad doc link --------- Co-authored-by: Sebastian Castro <[email protected]>
PickNikRobotics · Jun 26, 2023 · 523ffcb · 523ffcb
1 parent 7d4bcfb
commit 523ffcb
Show file tree

Hide file tree

Showing 3 changed files with 29 additions and 0 deletions.
diff --git a/src/picknik_ur_base_config/config/base_config.yaml b/src/picknik_ur_base_config/config/base_config.yaml
@@ -125,6 +125,9 @@ moveit_params:
   ompl_planning:
     package: "picknik_ur_base_config"
     path: "config/moveit/ompl_planning.yaml"
+  stomp_planning:
+    package: "picknik_ur_base_config"
+    path: "config/moveit/stomp_planning.yaml"
   kinematics:
     package: "picknik_ur_base_config"
     path: "config/moveit/pick_ik_kinematics.yaml"

diff --git a/src/picknik_ur_base_config/config/moveit/ompl_planning.yaml b/src/picknik_ur_base_config/config/moveit/ompl_planning.yaml
@@ -1,4 +1,11 @@
 planner_configs:
+  APSConfigDefault:
+    type: geometric::AnytimePathShortening
+    shortcut: 1  # Attempt to shortcut all new solution paths
+    hybridize: 1  # Compute hybrid solution trajectories
+    max_hybrid_paths: 32  # Number of hybrid paths generated per iteration
+    num_planners: 16  # The number of default planners to use for planning
+    planners: "RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect"  # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]"
   SBLkConfigDefault:
     type: geometric::SBL
     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
@@ -55,6 +62,7 @@ planner_configs:
 manipulator:
   default_planner_config: RRTConnectkConfigDefault
   planner_configs:
+    - APSConfigDefault
     - SBLkConfigDefault
     - ESTkConfigDefault
     - LBKPIECEkConfigDefault

diff --git a/src/picknik_ur_base_config/config/moveit/stomp_planning.yaml b/src/picknik_ur_base_config/config/moveit/stomp_planning.yaml
@@ -0,0 +1,18 @@
+planning_plugin: stomp_moveit/StompPlanner
+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/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.8
+  control_cost_weight: 0.1
+  delta_t: 0.1