diff --git a/benchmark/config/algs/geo_iros.yaml b/benchmark/config/algs/geo_iros.yaml new file mode 100644 index 0000000..facc70b --- /dev/null +++ b/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 diff --git a/benchmark/config/algs/sst_iros.yaml b/benchmark/config/algs/sst_iros.yaml new file mode 100644 index 0000000..b0ac1c3 --- /dev/null +++ b/benchmark/config/algs/sst_iros.yaml @@ -0,0 +1,4 @@ +reference: "sst_v0" +# params for SST +default: + planner: "rrt" diff --git a/benchmark/config/compare.yaml b/benchmark/config/compare.yaml index 3d3f584..2258d50 100644 --- a/benchmark/config/compare.yaml +++ b/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 # # # # # # # # diff --git a/benchmark/config/compare_iros.yaml b/benchmark/config/compare_iros.yaml index 9779033..1462538 100644 --- a/benchmark/config/compare_iros.yaml +++ b/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 diff --git a/benchmark/config/tmp.yaml b/benchmark/config/tmp.yaml new file mode 100644 index 0000000..60ac098 --- /dev/null +++ b/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!! diff --git a/include/dynoplan/dbastar/dbastar.hpp b/include/dynoplan/dbastar/dbastar.hpp index 2fec703..5da9bb0 100644 --- a/include/dynoplan/dbastar/dbastar.hpp +++ b/include/dynoplan/dbastar/dbastar.hpp @@ -295,7 +295,8 @@ struct Expander { // void plot_search_tree(std::vector nodes, std::vector &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 &motions, diff --git a/include/dynoplan/ompl/rrt_to.hpp b/include/dynoplan/ompl/rrt_to.hpp index 844d1dc..8f1d64f 100644 --- a/include/dynoplan/ompl/rrt_to.hpp +++ b/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 diff --git a/plot_fwd_bwd.py b/plot_fwd_bwd.py new file mode 100644 index 0000000..49b2d5e --- /dev/null +++ b/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() + + diff --git a/src/dbastar/dbastar.cpp b/src/dbastar/dbastar.cpp index cc3a361..fc2c70a 100644 --- a/src/dbastar/dbastar.cpp +++ b/src/dbastar/dbastar.cpp @@ -69,7 +69,7 @@ bool compareAStarNode::operator()(const AStarNode *a, void plot_search_tree(std::vector nodes, std::vector &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 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; diff --git a/src/dbrrt/dbrrt.cpp b/src/dbrrt/dbrrt.cpp index 7b5f31a..06e8d90 100644 --- a/src/dbrrt/dbrrt.cpp +++ b/src/dbrrt/dbrrt.cpp @@ -277,8 +277,7 @@ void reverse_motions(std::vector &motions_rev, } else if (startsWith(robot.name, "acrobot")) { // Don't do anything for the acrobot - } - else { + } else { std::string msg = "robot name " + robot.name + "not supported"; ERROR_WITH_INFO(msg); } @@ -748,6 +747,34 @@ void dbrrtConnect(const dynobench::Problem &problem, std::cout << "sizeTN_rev: " << T_nrev->size() << std::endl; std::cout << "time_bench:" << std::endl; + + std::string random_id = gen_random(6); + + if (options_dbrrt.debug) { + std::cout << "saving trajectories to disk " << std::endl; + std::cout << "random id: " << random_id << std::endl; + + auto fileout_name = + "/tmp/dynoplan/db_rrt_time_bench_" + random_id + ".yaml"; + + std::cout << "saving expanded trajs to " << fileout_name << std::endl; + create_dir_if_necessary(fileout_name); + + std::ofstream fileout(fileout_name); + + fileout << "chosen_trajs_fwd:" << std::endl; + for (auto &traj : chosen_trajs_fwd) { + fileout << " - " << std::endl; + traj.to_yaml_format(fileout, " "); + } + + fileout << "chosen_trajs_bwd:" << std::endl; + for (auto &traj : chosen_trajs_bwd) { + fileout << " - " << std::endl; + traj.to_yaml_format(fileout, " "); + } + } + time_bench.write(std::cout); if (solution_bwd && solution_fwd) { @@ -1479,7 +1506,7 @@ void dbrrtConnectOrig(const dynobench::Problem &problem, plot_search_tree(active_nodes_bwd, motions_rev, *robot, ("/tmp/dynoplan/db_rrt_tree_bwd_" + std::to_string(info_out.trajs_raw.size()) + ".yaml") - .c_str()); + .c_str(),false); if (info_out.solved_raw) { traj_out.to_yaml_format("/tmp/dynoplan/db_rrt_traj_" + diff --git a/src/ompl/main_rrt_to.cpp b/src/ompl/main_rrt_to.cpp index 069868d..54ca2cf 100644 --- a/src/ompl/main_rrt_to.cpp +++ b/src/ompl/main_rrt_to.cpp @@ -54,10 +54,19 @@ int main(int argc, char *argv[]) { dynobench::Info_out info_out_omplgeo; dynobench::Trajectory traj_out; - solve_ompl_geometric(problem, options_geo, options_trajopt, traj_out, - info_out_omplgeo); + if (options_geo.iterative_rrt) { + solve_ompl_geometric_iterative_rrt(problem, options_geo, options_trajopt, + traj_out, info_out_omplgeo); + } else { + solve_ompl_geometric(problem, options_geo, options_trajopt, traj_out, + info_out_omplgeo); + } + + // 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, + // solve_ompl_geometric_iterative_rrt( problem, options_geo, options_trajopt, + // traj_out, // info_out_omplgeo); std::cout << "*** info_out_omplgeo *** " << std::endl; diff --git a/src/ompl/rrt_to.cpp b/src/ompl/rrt_to.cpp index 9087582..3c0e9d2 100644 --- a/src/ompl/rrt_to.cpp +++ b/src/ompl/rrt_to.cpp @@ -127,7 +127,6 @@ void solve_ompl_geometric(const dynobench::Problem &problem, pdef->setIntermediateSolutionCallback( [&](const ob::Planner *, const std::vector &states, const ob::Cost cost) { - info_out_omplgeo.solved_raw = true; dynobench::Trajectory traj; dynobench::Trajectory traj_geo; @@ -149,7 +148,6 @@ void solve_ompl_geometric(const dynobench::Problem &problem, traj_geo.states.push_back(x); } traj_geo.states.push_back(traj_geo.goal); - traj_geo.feasible = false; double time = 0; traj_geo.times.resize(traj_geo.states.size()); @@ -316,7 +314,7 @@ void solve_ompl_geometric_iterative_rrt(const dynobench::Problem &problem, dynobench::Trajectory &traj_out, dynobench::Info_out &info_out_omplgeo) { - int max_trials = 100; + int max_trials = options_geo.max_trials_rrt; bool terminate = false; int trial = 0; @@ -349,6 +347,9 @@ void solve_ompl_geometric_iterative_rrt(const dynobench::Problem &problem, TerminateCriteria terminate_criteria = RUNNING; int max_planning_seconds = 10; + + double counter_time_ms = 0; + while (!terminate) { // RUN RRT. @@ -410,26 +411,38 @@ void solve_ompl_geometric_iterative_rrt(const dynobench::Problem &problem, pdef->print(std::cout); // attempt to solve the problem within timelimit - ob::PlannerStatus solved; + ob::PlannerStatus solved_raw; - solved = planner->ob::Planner::solve(max_planning_seconds); + auto tic = std::chrono::steady_clock::now(); + solved_raw = planner->ob::Planner::solve(max_planning_seconds); + auto toc = std::chrono::steady_clock::now(); + + counter_time_ms += + std::chrono::duration_cast(toc - tic) + .count(); dynobench::Trajectory traj_opt; - if (solved) { + if (solved_raw) { + + info_out_omplgeo.solved_raw = true; ob::PathPtr path = pdef->getSolutionPath(); std::cout << "Found solution:" << std::endl; - std::cout << "print as path geometric" << std::endl; + std::cout << "Print as path geometric" << std::endl; path->as()->print(std::cout); auto states = path->as()->getStates(); dynobench::Trajectory traj_geo; + traj_geo.time_stamp = counter_time_ms; + state_to_eigen(traj_geo.start, si, startState); state_to_eigen(traj_geo.goal, si, goalState); traj_geo.states.push_back(traj_geo.start); + traj_geo.feasible = true; // TODO: feasible here means it was a success, + // not that it fulfils the dynamics constraints. auto __states = states; @@ -442,8 +455,34 @@ void solve_ompl_geometric_iterative_rrt(const dynobench::Problem &problem, } // run trajectory optimization + double time = 0; + + traj_geo.times.resize(traj_geo.states.size()); + traj_geo.times(0) = 0.; + + // compute the times. + for (size_t i = 0; i < traj_geo.states.size() - 1; i++) { + auto &x = traj_geo.states.at(i); + auto &y = traj_geo.states.at(i + 1); + time += robot->diff_model->lower_bound_time(x, y); + traj_geo.times(i + 1) = time; + } + + traj_geo.cost = traj_geo.times.tail(1)(0); + + // add the control + + for (size_t i = 0; i < traj_geo.states.size() - 1; i++) { + traj_geo.actions.push_back(robot->diff_model->u_0); + } + traj_geo.to_yaml_format(std::cout); + std::cout << "traj geo " << std::endl; + + info_out_omplgeo.trajs_raw.push_back(traj_geo); + traj_geo.to_yaml_format("/tmp/dynoplan/traj_out.yaml"); + Result_opti result; // convert path to eigen @@ -459,17 +498,26 @@ void solve_ompl_geometric_iterative_rrt(const dynobench::Problem &problem, trajectory_optimization(problem, traj_geo, options_trajopt, traj_opt, result); + + counter_time_ms += + watch.elapsed_ms() - std::stof(result.data.at("time_ddp_total")); + + traj_opt.time_stamp = counter_time_ms; + double raw_time = watch.elapsed_ms(); std::cout << "*** Optimization done ***" << std::endl; std::cout << "traj opt" << std::endl; traj_opt.to_yaml_format(std::cout); + info_out_omplgeo.trajs_opt.push_back(traj_opt); if (traj_opt.feasible) { // solved! terminate = true; terminate_criteria = SOLVED; + info_out_omplgeo.solved = true; + info_out_omplgeo.cost = traj_opt.cost; } } @@ -497,4 +545,6 @@ void solve_ompl_geometric_iterative_rrt(const dynobench::Problem &problem, << terminate_criteria_str(terminate_criteria) << std::endl; } +// set info_out_omplgeo + } // namespace dynoplan diff --git a/src/ompl/sst.cpp b/src/ompl/sst.cpp index be0ba63..873c845 100644 --- a/src/ompl/sst.cpp +++ b/src/ompl/sst.cpp @@ -36,11 +36,27 @@ struct SST_public_interface : public oc::SST { ompl::base::Cost get_prevSolutionCost_() const { return prevSolutionCost_; } }; +struct RRTc_public_interface : public oc::RRT { + + RRTc_public_interface(const oc::SpaceInformationPtr &si) : oc::RRT(si) {} + + ~RRTc_public_interface() = default; + + void setNearestNeighbors(const std::string &name, + const std::shared_ptr &robot) { + auto t = nigh_factory( + name, robot, [](oc::RRT::Motion *m) { return m->state; }); + nn_.reset(t); + } +}; + void solve_sst(const dynobench::Problem &problem, const Options_sst &options_ompl_sst, const Options_trajopt &options_trajopt, dynobench::Trajectory &traj_out, dynobench::Info_out &info_out_omplsst) { + // TODO: separate into two implementations: + // SST vs RRT. std::string random_id = gen_random(6); @@ -99,8 +115,11 @@ void solve_sst(const dynobench::Problem &problem, auto robot_type = problem.robotType; if (options_ompl_sst.planner == "rrt") { - auto rrt = new oc::RRT(si); + auto rrt = new RRTc_public_interface(si); + // koc::RRT(si); rrt->setGoalBias(options_ompl_sst.goal_bias); + if (options_ompl_sst.sst_use_nigh) + rrt->setNearestNeighbors(problem.robotType, robot); planner.reset(rrt); } else if (options_ompl_sst.planner == "sst") { // auto sst =fnew oc::SST(si); @@ -313,7 +332,12 @@ void solve_sst(const dynobench::Problem &problem, ob::PlannerStatus solved; // for (int i = 0; i < 3; ++i) { + + auto tic = std::chrono::steady_clock::now(); solved = planner->ob::Planner::solve(options_ompl_sst.timelimit); + auto toc = std::chrono::steady_clock::now(); + double time_elapsed_ms = + std::chrono::duration(toc - tic).count(); // solved = planner->ob::Planner::solve( // [&] { @@ -331,26 +355,29 @@ void solve_sst(const dynobench::Problem &problem, CSTR_(solved); - { + if (options_ompl_sst.planner == "rrt") { dynobench::Trajectory traj_sst; std::vector solutions = pdef->getSolutions(); CSTR_(solutions.size()); if (solutions.size()) { + + info_out_omplsst.solved_raw = true; + // TODO : why I am doint this here? Clean the code!! ompl::base::PlannerSolution sol = solutions.front(); auto sol_control = sol.path_->as(); + sol_control->interpolate(); // this interpolate with the dt + std::vector states = sol_control->getStates(); + // std::cout << "last state is " << std::endl; + // si->printState(states.back(), std::cout); // - std::cout << "last state is " << std::endl; - si->printState(states.back(), std::cout); - - double distance_to_goal = si->distance(states.back(), goalState); - CSTR_(distance_to_goal); - - sol_control->print(std::cout); - sol_control->interpolate(); - sol_control->print(std::cout); + // double distance_to_goal = si->distance(states.back(), goalState); + // CSTR_(distance_to_goal); + // + // sol_control->print(std::cout); + // sol_control->print(std::cout); Eigen::VectorXd x; for (size_t i = 0; i < sol_control->getStateCount(); ++i) { @@ -366,6 +393,9 @@ void solve_sst(const dynobench::Problem &problem, traj_sst.actions.push_back(u); } + traj_sst.feasible = true; + traj_sst.time_stamp = time_elapsed_ms; + traj_sst.check(robot->diff_model); { @@ -373,6 +403,28 @@ void solve_sst(const dynobench::Problem &problem, random_id + ".yaml"); traj_sst.to_yaml_format(out); } + + // run trajectory optimization + // + // + // + + Result_opti result; + dynobench::Trajectory traj_opt; + std::cout << "*** Sart optimization ***" << std::endl; + + // NOTE: I don't consider the time spent in optimization, + // as it was not part of planning time (i.e. this is a lower bound + // on true cost) + Stopwatch sw; + trajectory_optimization(problem, traj_sst, options_trajopt, traj_opt, + result); + traj_sst.time_stamp = time_elapsed_ms; + + info_out_omplsst.trajs_opt.push_back(traj_opt); + info_out_omplsst.trajs_raw.push_back(traj_sst); + info_out_omplsst.cost = traj_opt.cost; + info_out_omplsst.solved = true; } } @@ -446,9 +498,9 @@ void solve_sst(const dynobench::Problem &problem, // << std::endl; if (info_out_omplsst.solved) { - std::cout << "Found solution:" << std::endl; + std::cout << "Kino RRT -- Found solution" << std::endl; } else { - std::cout << "No solution found" << std::endl; + std::cout << "Kino RRT -- No solution found" << std::endl; } } } // namespace dynoplan diff --git a/test/rrt_to/test_rrt_to_0.cpp b/test/rrt_to/test_rrt_to_0.cpp index 28fa439..ff4ffc5 100644 --- a/test/rrt_to/test_rrt_to_0.cpp +++ b/test/rrt_to/test_rrt_to_0.cpp @@ -53,6 +53,29 @@ BOOST_AUTO_TEST_CASE(parallel_park_1) { BOOST_TEST(info_out_omplgeo.cost < 5.); } +BOOST_AUTO_TEST_CASE(t_bugtrap1) { + + Options_geo options_geo; + Options_trajopt options_trajopt; + options_geo.planner = "rrt"; + options_geo.timelimit = 3; + options_geo.max_trials_rrt = 1; + + options_trajopt.solver_id = static_cast(SOLVER::time_search_traj_opt); + + Trajectory traj_out; + Info_out info_out_omplgeo; + + Problem problem(DYNOBENCH_BASE + + std::string("envs/unicycle1_v0/bugtrap_0.yaml")); + problem.models_base_path = DYNOBENCH_BASE + std::string("models/"); + + solve_ompl_geometric_iterative_rrt(problem, options_geo, options_trajopt, + traj_out, info_out_omplgeo); + BOOST_TEST(info_out_omplgeo.solved == true); + BOOST_TEST(info_out_omplgeo.cost < 100.); +} + // TODO: // BOOST_AUTO_TEST_CASE(test_bugtrap_heu) { // diff --git a/test/sst/test_sst_0.cpp b/test/sst/test_sst_0.cpp index 7102c64..5c7a561 100644 --- a/test/sst/test_sst_0.cpp +++ b/test/sst/test_sst_0.cpp @@ -53,6 +53,28 @@ BOOST_AUTO_TEST_CASE(parallel_park_1) { BOOST_TEST(info_out_omplgeo.cost <= 20); } +BOOST_AUTO_TEST_CASE(t_parallel_park_1) { + + Options_sst options_ompl_sst; + Options_trajopt options_trajopt; + Trajectory traj_out; + Info_out info_out_omplgeo; + + options_ompl_sst.timelimit = 5; + options_ompl_sst.sst_use_nigh = true; + options_ompl_sst.planner = "rrt"; + + Problem problem(DYNOBENCH_BASE + + std::string("envs/unicycle1_v0/parallelpark_0.yaml")); + problem.models_base_path = DYNOBENCH_BASE + std::string("models/"); + + solve_sst(problem, options_ompl_sst, options_trajopt, traj_out, + info_out_omplgeo); + + BOOST_TEST(info_out_omplgeo.solved == true); + BOOST_TEST(info_out_omplgeo.cost <= 20); +} + // BOOST_AUTO_TEST_CASE(test_bugtrap_heu) { // // Problem problem(DYNOBENCH_BASE +