add non optimal version of the baselines, to check what is better

benchmark/config/algs/geo_iros.yaml

@@ -0,0 +1,6 @@
+reference: geo_v0 # I import the weight_goal parameter from idbastar_v0
+default:
+  iterative_rrt: True
+  max_trials_rrt: 20
+  planner: "rrt"
+  solver_id: 1

benchmark/config/algs/sst_iros.yaml

@@ -0,0 +1,4 @@
+reference: "sst_v0"
+# params for SST
+default:
+  planner: "rrt"

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
@@ -25,9 +25,9 @@ problems:
   # - unicycle2_v0/narrow_passage
   # # # # # # # # # #
   # - car1_v0/kink_0
-  # - car1_v0/bugtrap_0
-  # - car1_v0/bugtrap_double
-  # - car1_v0/narrow_passage
+  - car1_v0/bugtrap_0
+  - car1_v0/bugtrap_double
+  - car1_v0/narrow_passage
   # - car1_v0/parallelpark_0
   # - car1_v0/parallelpark_0
   # # # # # # # #

benchmark/config/compare_iros.yaml

@@ -87,7 +87,7 @@ algs:
   # Idbastar
   # - idbastar_v0
   - idbastar_iros
-  # # - geo_v0
+  - geo_iros
   # # - sst_v0
   - dbrrt_v0
   # # - dbrrt_v0DEV

benchmark/config/tmp.yaml

@@ -0,0 +1,127 @@
+trials:  5
+timelimit: 60
+n_cores: 4 # -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_iros
+  # - geo_v0
+  - sst_v0
+  - sst_iros
+  # - 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!!

include/dynoplan/dbastar/dbastar.hpp

@@ -295,7 +295,8 @@ struct Expander {
 //
 void plot_search_tree(std::vector<AStarNode *> nodes,
                       std::vector<Motion> &motions,
-                      dynobench::Model_robot &robot, const char *filename);
+                      dynobench::Model_robot &robot, const char *filename,
+                      bool fwd = true);
 
 void from_solution_to_yaml_and_traj(dynobench::Model_robot &robot,
                                     const std::vector<Motion> &motions,

include/dynoplan/ompl/rrt_to.hpp

@@ -29,6 +29,8 @@ struct Options_geo {
   double range = -1;
   std::string outFile = "out.yaml";
   bool geo_use_nigh = false;
+  bool iterative_rrt = false;
+  int max_trials_rrt = 10; // only used in iterative rrt
 
   void add_options(po::options_description &desc) {
 
@@ -38,6 +40,8 @@ struct Options_geo {
     set_from_boostop(desc, VAR_WITH_NAME(planner));
     set_from_boostop(desc, VAR_WITH_NAME(timelimit));
     set_from_boostop(desc, VAR_WITH_NAME(outFile));
+    set_from_boostop(desc, VAR_WITH_NAME(iterative_rrt));
+    set_from_boostop(desc, VAR_WITH_NAME(max_trials_rrt));
   }
 
   void read_from_yaml(const char *file) {
@@ -62,6 +66,8 @@ struct Options_geo {
     set_from_yaml(node, VAR_WITH_NAME(planner));
     set_from_yaml(node, VAR_WITH_NAME(timelimit));
     set_from_yaml(node, VAR_WITH_NAME(outFile));
+    set_from_yaml(node, VAR_WITH_NAME(iterative_rrt));
+    set_from_yaml(node, VAR_WITH_NAME(max_trials_rrt));
   }
 
   void print(std::ostream &out, const std::string &be = "",
@@ -82,15 +88,10 @@ 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);
-
-
-
-
+                                        const Options_geo &options_geo,
+                                        const Options_trajopt &options_trajopt,
+                                        dynobench::Trajectory &traj_out,
+                                        dynobench::Info_out &info_out_omplgeo);
 
 } // namespace dynoplan

plot_fwd_bwd.py

@@ -0,0 +1,120 @@
+import yaml
+import matplotlib.pyplot as plt
+
+import sys  # noqa
+sys.path.append("/home/quim/code/dynoplan/dynobench/dynobench/utils/viewer")  # noqa
+
+
+from quad2d_viewer import Quad2dViewer
+
+
+fwd_file = "figs_iros/quad2d_bb/db_rrt_tree_fwd_0.yaml"
+bwd_file = "figs_iros/quad2d_bb/db_rrt_tree_bwd_0.yaml"
+
+
+filename_env = "dynobench/envs/quad2d_v0/quad_bugtrap_double.yaml"
+
+with open(filename_env) as env_file:
+    env = yaml.safe_load(env_file)
+
+
+viewer = Quad2dViewer()
+
+# load the data
+with open(fwd_file, 'r') as stream:
+    fwd_data = yaml.load(stream, Loader=yaml.CLoader)
+
+with open(bwd_file, 'r') as stream:
+    bwd_data = yaml.load(stream, Loader=yaml.CLoader)
+
+
+# plot 100 nodes of each tree
+
+
+fig, ax = plt.subplots()
+
+
+
+for e in fwd_data["edges"][:38]:
+    traj = e["traj"]
+    X = [x[0] for x in traj]
+    Y = [x[1] for x in traj]
+    ax.plot(X, Y,"g",alpha=.4)
+
+for e in bwd_data["edges"][:45]:
+    traj = e["traj"]
+    X = [x[0] for x in traj]
+    Y = [x[1] for x in traj]
+    ax.plot(X, Y,"r",alpha=.4)
+
+
+
+viewer.view_problem(ax, env, env_name=filename_env)
+plt.show()
+
+
+fig, ax = plt.subplots()
+
+
+
+for e in fwd_data["edges"]:
+    traj = e["traj"]
+    X = [x[0] for x in traj]
+    Y = [x[1] for x in traj]
+    ax.plot(X, Y,"g",alpha=.4)
+
+for e in bwd_data["edges"]:
+    traj = e["traj"]
+    X = [x[0] for x in traj]
+    Y = [x[1] for x in traj]
+    ax.plot(X, Y,"r",alpha=.4)
+
+
+
+viewer.view_problem(ax, env, env_name=filename_env)
+plt.show()
+
+
+
+#
+
+filename_traj = "figs_iros/quad2d_bb/db_rrt_traj_0.yaml"
+with open(filename_traj) as f:
+    result = yaml.load(f,Loader=yaml.CLoader)
+
+X = [X[0] for X in result["states"]]
+Y = [X[1] for X in result["states"]]
+
+fig, ax = plt.subplots()
+
+ax.plot(X, Y, "b", alpha=1)
+viewer.view_problem(ax, env, env_name=filename_env)
+
+plt.show()
+
+filename_traj_opt = "figs_iros/quad2d_bb/run_7_out.yaml"
+
+with open(filename_traj_opt) as f:
+    result = yaml.load(f,Loader=yaml.CLoader)
+
+fig, ax = plt.subplots()
+
+ax.plot(X, Y, "gray", alpha=.8)
+viewer.view_problem(ax, env, env_name=filename_env)
+
+
+Xopt = [X[0] for X in result["trajs_opt"][0]["states"]]
+Yopt = [X[1] for X in result["trajs_opt"][0]["states"]]
+
+# init_guess = "/tmp/dynoplan/init_guess_smooth.yaml"
+# with open(init_guess) as f:
+#     guess = yaml.load(f,Loader=yaml.CLoader)
+#
+# ax.plot([X[0] for X in guess["states"]], [X[1] for X in guess["states"]], "r", alpha=.8)
+
+ax.plot(Xopt, Yopt, "b", alpha=1)
+
+
+plt.show()
+
+

src/dbastar/dbastar.cpp

@@ -69,7 +69,7 @@ bool compareAStarNode::operator()(const AStarNode *a,
 
 void plot_search_tree(std::vector<AStarNode *> nodes,
                       std::vector<Motion> &motions,
-                      dynobench::Model_robot &robot, const char *filename) {
+                      dynobench::Model_robot &robot, const char *filename, bool fwd) {
 
   std::cout << "plotting search tree to: " << filename << std::endl;
   std::ofstream out(filename);
@@ -117,7 +117,7 @@ void plot_search_tree(std::vector<AStarNode *> nodes,
       dynobench::Trajectory traj;
       traj.states = lazy_traj.motion->traj.states;
       traj.actions = lazy_traj.motion->traj.actions;
-      lazy_traj.compute(traj);
+      lazy_traj.compute(traj,fwd);
       out << indent4 << "traj:" << std::endl;
       for (auto &s : traj.states) {
         out << indent6 << "- " << s.format(dynobench::FMT) << std::endl;