made progress towards papers. can run two benchmarks now.

benchmark/config/algs/idbastar_iros.yaml

@@ -10,6 +10,10 @@ unicycle2_v0:
   default:
     motionsFile: ../dynomotions/unicycle2_v0__ispso__2023_04_03__15_36_01.bin.im.bin.im.bin.small5000.msgpack
 
+unicycle1_v2:
+  defaults:
+    motionsFile: ../dynomotions/tmp_motions_unicycle1_v2.bin.sp.bin.small5000.msgpack
+
 car1_v0:
   default:
     motionsFile: ../dynomotions/car1_v0_all.bin.sp.bin.small5000.msgpack

benchmark/config/compare.yaml

@@ -10,10 +10,10 @@ n_cores: 8 # -1=auto
 problems:
   # # # # # #
   # - unicycle1_v0/parallelpark_0
-  # - unicycle1_v0/bugtrap_0
+  - unicycle1_v0/bugtrap_0
   # - unicycle1_v0/kink_0
-  # - unicycle1_v0/bugtrap_double
-  # - unicycle1_v0/narrow_passage
+  - unicycle1_v0/bugtrap_double
+  - unicycle1_v0/narrow_passage
   # # # # # #
   # - unicycle1_v1/kink_0
   # - unicycle1_v2/wall_0
@@ -83,7 +83,7 @@ problems:
   # - quadrotor_v1/recovery_with_obs
   # - quadrotor_v1/recovery_with_obs4
   # - quadrotor_v1/window_double
-  - quadrotor_v1/window_double2
+  # - quadrotor_v1/window_double2
   # - quadrotor_v1/bugtrap3
 
 algs:

benchmark/config/compare_iros.yaml

@@ -0,0 +1,126 @@
+trials:  10
+timelimit: 60
+n_cores: 8 # -1=auto
+
+
+problems:
+  # # # # # #
+  # - unicycle1_v0/parallelpark_0
+  # - unicycle1_v0/bugtrap_0
+  # - unicycle1_v0/kink_0
+  - unicycle1_v0/bugtrap_double
+  - unicycle1_v0/narrow_passage
+  # # # # # #
+  # - unicycle1_v1/kink_0
+  # - unicycle1_v2/wall_0
+  # # # # # # # # #
+  # - unicycle2_v0/kink_0
+  # - unicycle2_v0/parallelpark_0
+  # - unicycle2_v0/bugtrap_0
+  - unicycle2_v0/bugtrap_double
+  - unicycle2_v0/narrow_passage
+  # # # # # # # # # #
+  # - car1_v0/kink_0
+  # - car1_v0/bugtrap_0
+  - car1_v0/bugtrap_double
+  - car1_v0/narrow_passage
+  # - car1_v0/parallelpark_0
+  # - car1_v0/parallelpark_0
+  # # # # # # # #
+  # - quad2d_v0/empty_0
+  # - quad2d_v0/empty_1
+  # - quad2d_v0/quad_obs_columf
+  # - quad2d_v0/quad_bugtrap
+  - quad2d_v0/quad_bugtrap_double
+  - quad2d_v0/narrow_passage
+  # - quad2d_v0/quad2d_recovery_wo_obs
+  # - quad2d_v0/quad2d_recovery_obs
+  - quad2d_v0/quad2d_recovery_obs_double
+  # - quad2d_v0/fall_through
+  # # # # # # # # #
+  # - quad2dpole_v0/move_with_down
+  # - quad2dpole_v0/move_with_up
+  # - quad2dpole_v0/up
+  # - quad2dpole_v0/down
+  # - quad2dpole_v0/up_obs
+  # - quad2dpole_v0/window_easy
+  # - quad2dpole_v0/window
+  # - quad2dpole_v0/window_hard
+  # - quad2dpole_v0/window_hard2
+  - quad2dpole_v0/up_obs2
+  - quad2dpole_v0/window_hard2
+  # # # # # # # # #
+  # - acrobot_v0/swing_up_empty
+  # - acrobot_v0/swing_up_obs
+  # - acrobot_v0/swing_up_obs_hard
+  # - acrobot_v0/swing_down_easy
+  # - acrobot_v0/swing_down
+  # # # # # # # # #
+  # - quadrotor_v0/empty_0_easy
+  # - quadrotor_v0/empty_1_easy
+  # - quadrotor_v0/window
+  # - quadrotor_v0/recovery
+  # - quadrotor_v0/recovery_with_obs
+  # - quadrotor_v0/recovery_with_obs2
+  - quadrotor_v0/recovery_with_obs3
+  - quadrotor_v0/recovery_with_obs4
+  # - quadrotor_v0/quad_one_obs
+  - quadrotor_v0/window_double
+  # - quadrotor_v0/bugtrap
+  # - quadrotor_v0/bugtrap2
+  - quadrotor_v0/bugtrap3
+  # - quadrotor_v0/narrow_recovery # NOT working
+  # # # # # # # # # #
+  # - quadrotor_v1/empty_0_easy
+  # - quadrotor_v1/empty_1_easy
+  # - quadrotor_v1/window
+  # - quadrotor_v1/recovery
+  # - quadrotor_v1/quad_one_obs
+  # - quadrotor_v1/recovery_with_obs
+  - quadrotor_v1/recovery_with_obs4
+  # - quadrotor_v1/window_double
+  - quadrotor_v1/window_double2
+  - quadrotor_v1/bugtrap3
+
+algs:
+
+  # Idbastar
+  # - idbastar_v0
+  - idbastar_iros
+  # # - geo_v0
+  # # - sst_v0
+  - dbrrt_v0
+  # # - dbrrt_v0DEV
+  - dbrrt_v1
+  - dbrrt_v2
+  # - dbaorrt_v0
+  # - dbaorrt_v1
+
+
+  # VISUAL STUFF
+  # - idbastar_v0_vid_car
+
+
+  # - idbastar_tmp
+
+  # SST*
+  # - sst_tmp
+  # - sst_v1
+  # - sst_v2
+  # - sst_v3
+  # - sst_v4
+  # - sst_v1t
+  # - sst_v2t
+  # - sst_v3t
+  #
+  #
+  #
+  # # GEO
+  # - geo_v1
+  # # #
+  # #
+  # #
+  # # - "idbastar_v0_mpcc"
+  # # - "idbastar_v0_search"
+  # # - "idbastar_v0_heu1"
+  # # -  "idbastar_v1" # NOT USEFUL YET!!

benchmark/config/compare_selected_idbastar.yaml

@@ -4,27 +4,30 @@ n_cores: 8 # -1=auto
 
 
 problems:
-  - acrobot/swing_up_empty
-  - acrobot/swing_up_obs_hard
-  - car_first_order_with_1_trailers_0/kink_0
-  - car_first_order_with_1_trailers_0/parallelpark_0
-  - quad2d/fall_through
-  - quad2d/quad_bugtrap
-  - quad2dpole/up_obs
-  - quad2dpole/window_hard
-  - quadrotor_0/recovery
-  - quadrotor_0/recovery_with_obs
-  - quadrotor_ompl/quad_one_obs
-  - quadrotor_ompl/window
-  - unicycle_first_order_0/bugtrap_0
-  - unicycle_first_order_2/wall_0
-  - unicycle_second_order_0/bugtrap_0
-  - unicycle_second_order_0/parallelpark_0
+  # - acrobot_v0/swing_up_empty
+  # - acrobot_v0/swing_up_obs_hard
+  # - car1_v0/kink_0
+  # - car1_v0/parallelpark_0
+  # - quad2d_v0/fall_through
+  # - quad2d_v0/quad_bugtrap
+  # - quad2dpole_v0/up_obs
+  # - quad2dpole_v0/window_hard
+  # - quadrotor_v0/recovery
+  # - quadrotor_v0/recovery_with_obs
+  # - quadrotor_v1/quad_one_obs
+  # - quadrotor_v1/window
+  - unicycle1_v0/bugtrap_0
+  # - unicycle1_v2/wall_0
+  - unicycle2_v0/bugtrap_0
+  # - unicycle2_v0/parallelpark_0
+
 
 algs:
   - idbastar_iros
   - dbrrt_v0
   - dbrrt_v1
   - dbrrt_v2
+  - geo_v0
+  - sst_v0
 
 

include/dynoplan/ompl/rrt_to.hpp

@@ -82,4 +82,15 @@ void solve_ompl_geometric(const dynobench::Problem &problem,
                           dynobench::Trajectory &traj_out,
                           dynobench::Info_out &info_out_omplgeo);
 
+
+void solve_ompl_geometric_iterative_rrt(const dynobench::Problem &problem,
+                          const Options_geo &options_geo,
+                          const Options_trajopt &options_trajopt,
+                          dynobench::Trajectory &traj_out,
+                          dynobench::Info_out &info_out_omplgeo);
+
+
+
+
+
 } // namespace dynoplan

src/ompl/main_rrt_to.cpp

@@ -57,6 +57,9 @@ int main(int argc, char *argv[]) {
   solve_ompl_geometric(problem, options_geo, options_trajopt, traj_out,
                        info_out_omplgeo);
 
+  // solve_ompl_geometric_iterative_rrt( problem, options_geo, options_trajopt, traj_out,
+  //                      info_out_omplgeo);
+
   std::cout << "*** info_out_omplgeo   *** " << std::endl;
   info_out_omplgeo.print(std::cout);
   std::cout << "***" << std::endl;