Enable AnytimePathShortening and fix benchmarks (#167)

moveit · Apr 18, 2023 · b292ca0 · b292ca0
1 parent 471e35e
commit b292ca0
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 10 deletions.
diff --git a/benchmarking/launch/run_benchmark.launch.py b/benchmarking/launch/run_benchmark.launch.py
@@ -21,10 +21,10 @@ def load_yaml(package_name, file_path):
 def generate_launch_description():
 
     moveit_ros_benchmarks_config = (
-        ParameterBuilder("moveit_ros_benchmarks")
+        ParameterBuilder("moveit_resources_benchmarking")
         .yaml(
             parameter_namespace="benchmark_config",
-            file_path="run_benchmark.yaml",
+            file_path="config/run_benchmark.yaml",
         )
         .to_dict()
     )
@@ -34,8 +34,8 @@ def generate_launch_description():
         .robot_description(file_path="config/panda.urdf.xacro")
         .planning_pipelines("ompl", ["ompl", "chomp", "pilz_industrial_motion_planner"])
         .moveit_cpp(
-            file_path=get_package_share_directory("moveit_ros_benchmarks")
-            + "/moveit_cpp.yaml"
+            file_path=get_package_share_directory("moveit_resources_benchmarking")
+            + "/config/moveit_cpp.yaml"
         )
         .to_moveit_configs()
     )
@@ -61,7 +61,7 @@ def generate_launch_description():
 
     sqlite_database = (
         get_package_share_directory("moveit_resources_benchmarking")
-        + "/panda_test_db.sqlite"
+        + "/databases/panda_test_db.sqlite"
     )
 
     warehouse_ros_config = {

diff --git a/fanuc_moveit_config/config/ompl_planning.yaml b/fanuc_moveit_config/config/ompl_planning.yaml
@@ -12,11 +12,11 @@ start_state_max_bounds_error: 0.1
 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]"
+    shortcut: 1  # Attempt to shortcut all new solution paths
+    hybridize: 1  # Compute hybrid solution trajectories
+    max_hybrid_paths: 36  # Number of hybrid paths generated per iteration
+    num_planners: 8  # The number of default planners to use for planning
+    planners: "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]"
   SBL:
     type: geometric::SBL
     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()

diff --git a/panda_moveit_config/config/ompl_planning.yaml b/panda_moveit_config/config/ompl_planning.yaml
@@ -10,6 +10,13 @@ request_adapters: >-
   default_planner_request_adapters/FixStartStatePathConstraints
 start_state_max_bounds_error: 0.1
 planner_configs:
+  APSConfigDefault:
+    type: geometric::AnytimePathShortening
+    shortcut: 1  # Attempt to shortcut all new solution paths
+    hybridize: 1  # Compute hybrid solution trajectories
+    max_hybrid_paths: 36  # Number of hybrid paths generated per iteration
+    num_planners: 8  # The number of default planners to use for planning
+    planners: "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()
@@ -136,6 +143,7 @@ planner_configs:
 
 panda_arm:
   planner_configs:
+    - APSConfigDefault
     - SBLkConfigDefault
     - ESTkConfigDefault
     - LBKPIECEkConfigDefault
@@ -162,6 +170,7 @@ panda_arm:
     - TrajOptDefault
 panda_arm_hand:
   planner_configs:
+    - APSConfigDefault
     - SBLkConfigDefault
     - ESTkConfigDefault
     - LBKPIECEkConfigDefault
@@ -188,6 +197,7 @@ panda_arm_hand:
     - TrajOptDefault
 hand:
   planner_configs:
+    - APSConfigDefault
     - SBLkConfigDefault
     - ESTkConfigDefault
     - LBKPIECEkConfigDefault