Merge branch 'iros-dev' of github.com:quimortiz/dynoplan into iros-dev

benchmark/config/algs/dbrrt_v0.yaml

@@ -20,14 +20,14 @@ unicycle1_v1:
   default:
     goal_region: 0.2
     delta: 0.2
-    motionsFile: ../dynomotions_full/tmp_motions_unicycle1_v1.bin.sp.bin.msgpack
+    motionsFile: ../dynomotions/tmp_motions_unicycle1_v1.bin.sp.bin.small5000.msgpack
     max_motions: 200
 #
 unicycle1_v2:
   default:
     goal_region: .2
     delta: .15
-    motionsFile: ../dynomotions_full/tmp_motions_unicycle1_v2.bin.sp.bin.msgpack
+    motionsFile: ../dynomotions/tmp_motions_unicycle1_v2.bin.sp.bin.small5000.msgpack
     max_motions: 200
     # soft_control_bounds: true
 #
@@ -112,7 +112,7 @@ quad3d_v0:
     # solver_id: 0
     weight_goal: 100 
     use_collision_shape: false
-    solver_id: 0
+    # solver_id: 0
     # use_mim_solvers: true
 
   # recovery_with_obs:

benchmark/config/algs/idbastar_iros.yaml

@@ -10,9 +10,14 @@ unicycle2_v0:
   default:
     motionsFile: ../dynomotions/unicycle2_v0__ispso__2023_04_03__15_36_01.bin.im.bin.im.bin.small5000.msgpack
 
+unicycle1_v1:
+  default:
+    motionsFile: ../dynomotions/tmp_motions_unicycle1_v1.bin.sp.bin.small5000.msgpack
+
 unicycle1_v2:
-  defaults:
+  default:
     motionsFile: ../dynomotions/tmp_motions_unicycle1_v2.bin.sp.bin.small5000.msgpack
+    max_motions_primitives: 1000
 
 car1_v0:
   default:

benchmark/config/compare.yaml

@@ -16,7 +16,7 @@ problems:
   # - unicycle1_v0/narrow_passage
   # # # # # #
   # - unicycle1_v1/kink_0
-  # - unicycle1_v2/wall_0
+  - unicycle1_v2/wall_0
   # # # # # # # # #
   # - unicycle2_v0/kink_0
   # - unicycle2_v0/parallelpark_0
@@ -94,9 +94,9 @@ algs:
   # # - geo_v0
   # # - sst_v0
   - dbrrt_v0
-  # # - dbrrt_v0DEV
-  - dbrrt_v1
-  - dbrrt_v2
+  # # # - dbrrt_v0DEV
+  # - dbrrt_v1
+  # - dbrrt_v2
   # - dbaorrt_v0
   # - dbaorrt_v1
 

benchmark/config/compare_selected_idbastar.yaml

@@ -27,7 +27,7 @@ algs:
   - idbastar_iros
   - dbrrt_v0
   - dbrrt_v1
-  - dbrrt_v2
+  # - dbrrt_v2
   # - geo_v0
   # - sst_v0
 

src/dbrrt/dbrrt.cpp

@@ -2369,7 +2369,12 @@ void idbrrt(const dynobench::Problem &problem,
   DYNO_CHECK_EQ(options_dbrrt.ao_rrt, false,
                 "ao rrt not supported in this mode");
 
-  double delta_factor = .99;
+
+  // NOTE: in current implementation i don't change the number 
+  // of motions because the RRT is always able to find a solution
+  double motion_factor = 1.05;
+
+  double delta_factor = .95;
   size_t it = 0;
 
   Stopwatch watch;
@@ -2380,7 +2385,12 @@ void idbrrt(const dynobench::Problem &problem,
   double time_search = 0;
   double time_opt = 0;
 
-  while (!finished && it < options_dbrrt.max_idb_it) {
+  auto tic = std::chrono::high_resolution_clock::now();
+  // int max_motions  = options_dbrrt.max_motions;
+
+
+  while (!finished) {
+
     if (it > 0) {
       options_dbrrt_local.delta *= delta_factor;
       options_dbrrt_local.goal_region *= delta_factor;
@@ -2456,6 +2466,19 @@ void idbrrt(const dynobench::Problem &problem,
     }
 
     it++;
+
+    double elapsed_time = std::chrono::duration_cast<std::chrono::milliseconds>(
+                              std::chrono::high_resolution_clock::now() - tic)
+                              .count();
+    if (elapsed_time / 1000. > options_dbrrt.timelimit) {
+      std::cout << "iDb-RRT: breaking because of time limit" << std::endl;
+      break;
+    }
+
+    if (it >= options_dbrrt.max_idb_it) {
+      std::cout << "iDb-RRT: breaking because of outter iterations" << std::endl;
+      break;
+    }
   }
 
   info_out.data.insert(

src/ompl/robots.cpp

@@ -3118,13 +3118,26 @@ void load_motion_primitives_new(const std::string &motionsFile,
     std::cout << "WARNING:"
               << "adding noise to first and last state" << std::endl;
     const double noise = 1e-7;
+    std::cout << "size " << trajs.data.size() << std::endl;
+    // TODO: unicycle1_v2 contains trajs of length 0.
+    // I shoudl delte this ones
     for (auto &t : trajs.data) {
+      if (t.states.size() == 0) {
+        std::cout << "WARNING: traj of length 0" << std::endl;
+        continue;
+      }
       t.states.front() +=
           noise * Eigen::VectorXd::Random(t.states.front().size());
 
       t.states.back() +=
           noise * Eigen::VectorXd::Random(t.states.back().size());
     }
+
+    // delte trajs of length 0 with erase-remove idiom
+    trajs.data.erase(
+        std::remove_if(trajs.data.begin(), trajs.data.end(),
+                       [](const auto &t) { return t.states.size() == 0; }),
+        trajs.data.end());
   }
 
   // TODO: robot should have "add noise function"