diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml index 4daab01..21c185d 100644 --- a/.github/workflows/cmake.yml +++ b/.github/workflows/cmake.yml @@ -24,7 +24,7 @@ jobs: submodules: recursive - name: Install system dependencies - run: sudo apt update && sudo apt install libboost-all-dev libfcl-dev cmake libeigen3-dev libyaml-cpp-dev liblz4-dev python3-pip texlive latexmk texlive-latex-extra texlive-science -y + run: sudo apt update && sudo apt install libboost-all-dev libfcl-dev cmake libeigen3-dev libyaml-cpp-dev liblz4-dev python3-pip texlive latexmk texlive-latex-extra texlive-science coinor-libipopt-dev -y - name: install crocoddyl run: | @@ -38,9 +38,13 @@ jobs: - name: Install latest ompl run: | + # sudo apt install libompl-dev -y + cd mkdir __local git clone https://github.com/ompl/ompl - cd ompl && mkdir build && cd build && cmake .. -DCMAKE_INSTALL_PREFIX=~/__local && make install -j8 + cd ompl + git checkout e2994e5 + mkdir build && cd build && cmake .. -DCMAKE_INSTALL_PREFIX=~/__local && make install -j4 - name: Configure CMake # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. @@ -52,7 +56,7 @@ jobs: run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} - name: Test - continue-on-error: true + # continue-on-error: true working-directory: ${{github.workspace}}/build # Execute tests defined by the CMake configuration. # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail @@ -62,7 +66,7 @@ jobs: - name: Check pip run: pip3 install numpy matplotlib scipy pyyaml pandas - - name: Python - working-directory: ${{github.workspace}}/build - run: | - python3 ../benchmark/test.py + # - name: Python + # working-directory: ${{github.workspace}}/build + # run: | + # python3 ../benchmark/test.py diff --git a/.gitmodules b/.gitmodules index 8d84412..c1d4527 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,6 @@ [submodule "dynobench"] path = dynobench - url = git@github.com:quimortiz/dynobench + url = git@github.com:quimortiz/dynobench [submodule "deps/nigh"] path = deps/nigh url = https://github.com/UNC-Robotics/nigh.git diff --git a/CMakeLists.txt b/CMakeLists.txt index 894a840..50d7df0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,7 @@ project( # find_package(pinocchio REQUIRED) find_package(Boost REQUIRED COMPONENTS program_options serialization stacktrace_basic) -find_package(ompl 1.6 REQUIRED) +find_package(ompl REQUIRED) include(FindThreads) @@ -57,6 +57,8 @@ add_executable(from_boost_to_msgpack add_executable(main_sst ./src/ompl/main_sst.cpp) add_executable(main_optimization ./src/optimization/main_optimization.cpp) +add_executable(main_multirobot_optimization + ./src/optimization/main_multirobot_optimization.cpp) add_executable(main_dbastar ./src/dbastar/main_dbastar.cpp) add_executable(main_idbastar ./src/idbastar/main_idbastar.cpp) @@ -103,6 +105,12 @@ target_include_directories( $ PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src) +target_include_directories( + main_multirobot_optimization + PUBLIC $ + $ + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src) + target_include_directories( sst PUBLIC $ ${OMPL_INCLUDE_DIRS} @@ -224,7 +232,7 @@ target_link_libraries( optimization PUBLIC Eigen3::Eigen dynobench::dynobench PRIVATE ${CROCODDYL_LIBRARIES} fcl yaml-cpp Boost::program_options - Boost::serialization) + Boost::serialization libipopt.so) target_link_libraries( main_optimization @@ -237,5 +245,16 @@ target_link_libraries( Boost::program_options Boost::serialization) +target_link_libraries( + main_multirobot_optimization + PRIVATE ${CROCODDYL_LIBRARIES} + optimization + Eigen3::Eigen + dynobench::dynobench + fcl + yaml-cpp + Boost::program_options + Boost::serialization) + enable_testing() add_subdirectory(test) diff --git a/README.md b/README.md index cd5500a..4774af8 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,15 @@ # Dynoplan 🦖 + +

+

+ +

+ Dynoplan is a small library for solving kinodynamic motion planning problems, as defined in [Dynobench](https://github.com/quimortiz/dynobench) :t-rex:. It implements 3 different algorithms: Trajectory Optimization with geometric initial guess (RRT*-TO), Sample based Motion Planning (SST*), and Iterative Search and Optimization (iDb-A*).

@@ -7,9 +17,6 @@ Dynoplan is a small library for solving kinodynamic motion planning problems, as

- - - The first version [kinodynamic-motion-planning-benchmark](https://github.com/imrCLab/kinodynamic-motion-planning-benchmark) is now deprecated. ## Robots and Problem Description @@ -98,48 +105,37 @@ https://drive.google.com/file/d/1OLuw5XICTueoZuleXOuD6vNh3PCWfHif/view?usp=drive ## How to generate motion primitives for new systems +We will show how to generate motion primitives for the `integrator1_2d_v0` -Step one: Implement the Dynamics in Dynobench, Following the tutorial for the `Integrator2_2d` in the `README` +* Step one: Implement the Dynamics in Dynobench, following the tutorial for the `Integrator2_2d` in the `README` (in this case `integrator1_2d_v0` is already implemented) +* Step two: Solve Optimization Problems with Random Start and Goals ``` - - - - -``` - -Step two: Solve Optimization Problems with Random Start and Goals - -``` - ./main_primitives --mode_gen_id 0 --dynamics integrator1_2d_v0 --models_base_path ../dynobench/models/ --max_num_primitives 200 --out_file /tmp/my_motions.bin ``` -Primitives will be store in `/tmp/my_motions.bin` and `/tmp/my_motions.bin.yaml` - +Primitives will be store in `/tmp/my_motions.bin` and `/tmp/my_motions.bin.yaml`. You can pass options to the solver for trajectory optimization. -Step Three: Improve the quality of the primitives +* Step Three: Improve the cost of the primitives ``` ./main_primitives --mode_gen_id 1 --dynamics integrator1_2d_v0 --models_base_path ../dynobench/models/ --max_num_primitives 200 --in_file /tmp/my_motions.bin --solver_id 1 - ``` -By default, primitives are stored in `/tmp/my_motions.bin.im.bin` and `/tmp/my_motions.bin.im.bin.yaml` +By default, primitives are stored in `/tmp/my_motions.bin.im.bin` and `/tmp/my_motions.bin.im.bin.yaml`. You can pass options to the solver for trajectory optimization. -Step Fours: Randomnly cut primitives +* Step Fours: Randomnly cut primitives ``` -m4 main_primitives && ./main_primitives --mode_gen_id 2 --in_file /tmp/my_motions.bin.im.bin --max_num_primitives -1 --max_splits 1 --max_length_cut 50 --min_length_cut 5 --dynamics integrator1_2d_v0 --models_base_path ../dynobench/models/ +./main_primitives --mode_gen_id 2 --in_file /tmp/my_motions.bin.im.bin --max_num_primitives -1 --max_splits 1 --max_length_cut 50 --min_length_cut 5 --dynamics integrator1_2d_v0 --models_base_path ../dynobench/models/ ``` By default, primitives will be stored in `/tmp/my_motions.bin.im.bin.sp.bin` and `/tmp/my_motions.bin.im.bin.sp.bin.yaml` - Done! -`main_primitives` provide more utils, such as conversion between formats, computing statistics, generating primitives with random rollouts, sorting primitives and resampling of primitives +Additionally, `main_primitives` provide more useful functionality, such as conversion between formats, computing statistics, generating primitives with random rollouts, sorting primitives and resampling of primitives. ## Benchmark diff --git a/dynobench b/dynobench index b384b98..138e710 160000 --- a/dynobench +++ b/dynobench @@ -1 +1 @@ -Subproject commit b384b9814f81d598c40839f28dc3a0304e28c71b +Subproject commit 138e71007433380a8a40212c17a10b1dee6d94a7 diff --git a/src/optimization/generate_ocp.cpp b/src/optimization/generate_ocp.cpp index ba823e7..711cc17 100644 --- a/src/optimization/generate_ocp.cpp +++ b/src/optimization/generate_ocp.cpp @@ -1,6 +1,7 @@ #include "dynoplan/optimization/generate_ocp.hpp" +#include "dynobench/joint_robot.hpp" namespace dynoplan { @@ -100,6 +101,40 @@ generate_problem(const Generate_params &gen_args, std::vector> feats_run; + if (gen_args.model_robot->name == "joint_robot") { + + auto ptr_derived = std::dynamic_pointer_cast( + gen_args.model_robot); + + std::vector goal_times = ptr_derived->goal_times; + + print_vec(goal_times.data(), goal_times.size()); + CSTR_(gen_args.N); + + + if (goal_times.size()) { + Eigen::VectorXd weights = Eigen::VectorXd::Zero(nx); + for (size_t j = 0; j < goal_times.size(); j++) { + if (goal_times.at(j) <= t + 1) { + size_t start_index = std::accumulate( + ptr_derived->nxs.begin(), ptr_derived->nxs.begin() + j, 0); + size_t nx = ptr_derived->nxs.at(j); + weights.segment(start_index, nx).setOnes(); + } + } + + if (weights.sum() > 1e-12) { + std::cout << "warning, adding special goal cost" << std::endl; + ptr state_feature = mk( + gen_args.model_robot, nx, nu, + gen_args.penalty * options_trajopt.weight_goal * weights, + gen_args.goal); + + feats_run.emplace_back(state_feature); + } + } + } + if (control_mode == Control_Mode::free_time_linear) { if (t > 0) feats_run.emplace_back(mk(nx, nu)); diff --git a/src/optimization/main_multirobot_optimization.cpp b/src/optimization/main_multirobot_optimization.cpp new file mode 100644 index 0000000..901eead --- /dev/null +++ b/src/optimization/main_multirobot_optimization.cpp @@ -0,0 +1,139 @@ +#include +#include +#include +#include +#include +#include +#include +#include "dynobench/multirobot_trajectory.hpp" + +// OMPL headers + +#include + + +bool execute_optimizationMultiRobot(const std::string &env_file, + const std::string &initial_guess_file, + const std::string &output_file, + const std::string &dynobench_base, + bool sum_robots_cost) { + + using namespace dynoplan; + using namespace dynobench; + + Options_trajopt options_trajopt; + Problem problem(env_file); + + MultiRobotTrajectory init_guess_multi_robot; + init_guess_multi_robot.read_from_yaml(initial_guess_file.c_str()); + + + + std::vector goal_times(init_guess_multi_robot.trajectories.size()); + + std::transform(init_guess_multi_robot.trajectories.begin(), + init_guess_multi_robot.trajectories.end(), goal_times.begin(), + [](const Trajectory &traj) { return traj.states.size(); }); + + Trajectory init_guess; + + std::cout << "goal times are " << std::endl; + + for (auto &t : goal_times) { + std::cout << t << std::endl; + } + + if (sum_robots_cost) { + std::cout + << "warning: new approach where each robot tries to reach the goal fast" + << std::endl; + problem.goal_times = goal_times; + } + + else { + std::cout + << "warning: old apprach, robots will reach the goals at the same time " + << std::endl; + } + + options_trajopt.solver_id = 1; // static_cast(SOLVER::traj_opt); + options_trajopt.control_bounds = 1; + options_trajopt.use_warmstart = 1; + options_trajopt.weight_goal = 100; + options_trajopt.max_iter = 50; + problem.models_base_path = dynobench_base + std::string("models/"); + + Result_opti result; + Trajectory sol; + + dynobench::Trajectory init_guess_joint = + init_guess_multi_robot.transform_to_joint_trajectory(); + init_guess.to_yaml_format("/tmp/check2.yaml"); + + trajectory_optimization(problem, init_guess_joint, options_trajopt, sol, + result); + if (!result.feasible) { + std::cout << "optimization infeasible" << std::endl; + return false; + } + + std::cout << "optimization done! " << std::endl; + std::vector index_time_goals; + + if (problem.goal_times.size()) { + index_time_goals = sol.multi_robot_index_goal; + } else { + size_t num_robots = init_guess_multi_robot.get_num_robots(); + index_time_goals = std::vector(num_robots, sol.states.size()); + } + + MultiRobotTrajectory multi_out = from_joint_to_indiv_trajectory( + sol, init_guess_multi_robot.get_nxs(), init_guess_multi_robot.get_nus(), + index_time_goals); + + multi_out.to_yaml_format("/tmp/check5.yaml"); + multi_out.to_yaml_format(output_file.c_str()); + + + return true; + +} + + +int main(int argc, char *argv[]) { + + namespace po = boost::program_options; + po::options_description desc("Allowed options"); + std::string envFile; + std::string initFile; + std::string outFile; + std::string dynobench_base; + bool sum_robots_cost = true; + desc.add_options()("help", "produce help message")( + "env,e", po::value(&envFile)->required())( + "init,i", po::value(&initFile)->required())( + "out,o", po::value(&outFile)->required()) +( + "base,b", po::value(&dynobench_base)->required()) + + ( + "sum,s", po::value(&sum_robots_cost)->required()); + + try { + po::variables_map vm; + po::store(po::parse_command_line(argc, argv, desc), vm); + po::notify(vm); + + if (vm.count("help") != 0u) { + std::cout << desc << "\n"; + return 0; + } + } catch (po::error &e) { + std::cerr << e.what() << std::endl << std::endl; + std::cerr << desc << std::endl; + return 1; + } + + execute_optimizationMultiRobot(envFile, initFile, outFile, + dynobench_base, sum_robots_cost); +} diff --git a/src/optimization/ocp.cpp b/src/optimization/ocp.cpp index 8a2a376..e04152d 100644 --- a/src/optimization/ocp.cpp +++ b/src/optimization/ocp.cpp @@ -13,6 +13,7 @@ #include "crocoddyl/core/utils/timer.hpp" #include "dynobench/general_utils.hpp" +#include "dynobench/joint_robot.hpp" #include "dynobench/robot_models.hpp" #include "dynoplan/optimization/croco_models.hpp" @@ -193,9 +194,7 @@ void check_problem_with_finite_diff( const std::vector &us) { std::cout << "Checking with finite diff " << std::endl; options.use_finite_diff = true; - // options.disturbance = 1e-4; - // options.disturbance = 1e-5; - options.disturbance = 1e-6; + options.disturbance = 1e-5; std::cout << "gen problem " << STR_(AT) << std::endl; size_t nx, nu; ptr problem_fdiff = @@ -1294,7 +1293,7 @@ void __trajectory_optimization( CSTR_(success); if (__free_time_mode) { - // TODO: Use Trajectory Class as interface!! + Trajectory traj_resample = traj.resample(model_robot); std::cout << "check traj after resample " << std::endl; @@ -1305,6 +1304,33 @@ void __trajectory_optimization( xs_out = traj_resample.states; us_out = traj_resample.actions; + if (problem.goal_times.size()) { + auto ptr_derived = + std::dynamic_pointer_cast(model_robot); + + std::cout << "warning: fix the terminal times for the subgoals " + << std::endl; + + CSTR_(traj.times.size()); + for (auto &g : ptr_derived->goal_times) { + std::cout << g << std::endl; + std::cout << traj.times[g - 1] << std::endl; + g = int((traj.times[g - 1] / ptr_derived->ref_dt) + 2); + } + + for (const auto &g : ptr_derived->goal_times) { + std::cout << "goal time " << g << std::endl; + } + + int max_goal_time = 0; + + for (auto &t : ptr_derived->goal_times) { + if (t > max_goal_time) { + max_goal_time = t; + } + } + DYNO_CHECK_EQ(max_goal_time, xs_out.size(), AT); + } } else { xs_out = _xs_out; us_out = _us_out; @@ -1333,7 +1359,6 @@ void __trajectory_optimization( opti_out.xs_out = xs_out; opti_out.us_out = us_out; opti_out.cost = us_out.size() * dt; - traj.states = xs_out; traj.actions = us_out; @@ -1419,19 +1444,31 @@ void trajectory_optimization(const dynobench::Problem &problem, double time_ddp_total = 0; Stopwatch watch; Options_trajopt options_trajopt_local = options_trajopt; + // std::string _base_path = "../../models/"; + + std::shared_ptr model_robot; + // if (problem.robotTypes.size() == 1) { + if (problem.robotTypes.size() == 1 && problem.goal_times.size() == 0) { + // robotType.empty()) { + model_robot = dynobench::robot_factory( + (problem.models_base_path + problem.robotType + ".yaml").c_str()); + } else { + model_robot = dynobench::joint_robot_factory(problem.robotTypes, + problem.models_base_path, + problem.p_lb, problem.p_ub); + } - std::shared_ptr model_robot = - dynobench::robot_factory( - (problem.models_base_path + problem.robotType + ".yaml").c_str(), - problem.p_lb, problem.p_ub); - - // std::shared_ptr model_robot = - // dynobench::robot_factory( - // (problem.models_base_path + problem.robotType + ".yaml").c_str()); + if (problem.goal_times.size()) { + auto ptr_derived = + std::dynamic_pointer_cast(model_robot); + CHECK(ptr_derived, "multiple goal times only work for joint robot"); + ptr_derived->goal_times = problem.goal_times; + traj.multi_robot_index_goal = ptr_derived->goal_times; + } load_env(*model_robot, problem); - size_t _nx = model_robot->nx; + size_t _nx = model_robot->nx; // state size_t _nu = model_robot->nu; Trajectory tmp_init_guess(init_guess), tmp_solution; @@ -1883,8 +1920,81 @@ void trajectory_optimization(const dynobench::Problem &problem, options_trajopt_local.debug_file_name = "/tmp/dynoplan/debug_file_trajopt_after_freetime_proxi.yaml"; +#if 0 + if (problem.goal_times.size() > 0) { + + std::cout << "warning: I have to adjust the goal times, if they were " + "provided in the multirobot sytems" + << std::endl; + + // Example: at the beginning they were + // 16 , 47 + // and num_actions was 46 + + // now num actions should be 60. + // so 47 should be: 61 + + // + + int max_goal_time = 0; + + for (auto &t : ptr_derived->goal_times) { + if (t > max_goal_time) { + max_goal_time = t; + } + } + + size_t num_new_actions = tmp_solution.actions.size(); + + for (auto &g : ptr_derived->goal_times) { + g = std::lround(double(g) / max_goal_time * (num_new_actions + 1)); + } + + // some checks + { + int max_goal_time = 0; + + for (auto &t : ptr_derived->goal_times) { + if (t > max_goal_time) { + max_goal_time = t; + } + } + CHECK_EQ(max_goal_time, num_new_actions + 1, AT); + } + } +#endif + + if (problem.goal_times.size()) { + std::cout << "new goal times are " << std::endl; + + auto ptr_derived = + std::dynamic_pointer_cast(model_robot); + CHECK(ptr_derived, "multiple goal times only work for joint robot"); + + for (auto &g : ptr_derived->goal_times) { + std::cout << g << " "; + } + std::cout << std::endl; + } + __trajectory_optimization(problem, model_robot, tmp_solution, options_trajopt_local, traj, opti_out); + + if (problem.goal_times.size()) { + auto ptr_derived = + std::dynamic_pointer_cast(model_robot); + CHECK(ptr_derived, "multiple goal times only work for joint robot"); + traj.multi_robot_index_goal = ptr_derived->goal_times; + + int max_index = 0; + for (auto &t : ptr_derived->goal_times) { + if (t > max_index) { + max_index = t; + } + } + DYNO_CHECK_EQ(max_index, traj.states.size(), AT); + } + time_ddp_total += std::stod(opti_out.data.at("ddp_time")); CSTR_(time_ddp_total); } @@ -1963,21 +2073,24 @@ void trajectory_optimization(const dynobench::Problem &problem, void Result_opti::write_yaml(std::ostream &out) { out << "feasible: " << feasible << std::endl; out << "success: " << success << std::endl; - out << "cost: " << cost << std::endl; + out << "cost: " << cost << std::endl; // TODO: why 2*cost is joint_robot? if (data.size()) { out << "info:" << std::endl; for (const auto &[k, v] : data) { out << " " << k << ": " << v << std::endl; } } - - out << "xs_out: " << std::endl; + // TODO: @QUIM @AKMARAL Clarify this!!! + out << "result:" << std::endl; + // out << "xs_out: " << std::endl; + out << " - states:" << std::endl; for (auto &x : xs_out) - out << " - " << x.format(FMT) << std::endl; + out << " - " << x.format(FMT) << std::endl; - out << "us_out: " << std::endl; + // out << "us_out: " << std::endl; + out << " actions:" << std::endl; for (auto &u : us_out) - out << " - " << u.format(FMT) << std::endl; + out << " - " << u.format(FMT) << std::endl; } void Result_opti::write_yaml_db(std::ostream &out) { diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 7cf554c..3ca59b8 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -6,9 +6,11 @@ find_package(Boost REQUIRED COMPONENTS unit_test_framework) add_executable( test_optimization - ./optimization/test_optimization_base.cpp - ./optimization/test_optimization_0.cpp ./optimization/test_optimization_1.cpp - ./optimization/test_optimization_cli.cpp) + ./optimization/test_optimization_base.cpp + ./optimization/test_optimization_0.cpp + ./optimization/test_optimization_1.cpp + ./optimization/test_optimization_cli.cpp + ./optimization/test_optimization_multirobot.cpp) add_executable( test_dbastar dbastar/test_dbastar_base.cpp dbastar/test_dbastar_0.cpp diff --git a/test/optimization/test_optimization_multirobot.cpp b/test/optimization/test_optimization_multirobot.cpp new file mode 100644 index 0000000..0b76f0a --- /dev/null +++ b/test/optimization/test_optimization_multirobot.cpp @@ -0,0 +1,380 @@ +#include "dynoplan/optimization/ocp.hpp" + +// #define BOOST_TEST_MODULE test module name +// #define BOOST_TEST_DYN_LINK +#include +#include + +#include "Eigen/Core" +#include + +// #include "collision_checker.hpp" +// save data without the cluster stuff + +#include +#include +#include +#include + +#include +#include + +#include "dynobench/motions.hpp" +#include "dynobench/multirobot_trajectory.hpp" +#include "dynobench/planar_rotor.hpp" +#include "dynobench/planar_rotor_pole.hpp" +#include +#include + +using namespace dynoplan; +using namespace dynobench; + +BOOST_AUTO_TEST_CASE(t_multi_robot_cli) { + + { + std::vector build_cmd_new = { + "make", + "../main_multirobot_optimization", + }; + + std::string build_cmd = ""; + + for (auto &s : build_cmd_new) { + build_cmd += s + " "; + } + int out = std::system(build_cmd.c_str()); + BOOST_TEST(out == 0); + } + + std::vector run_cmd_new = { + "../main_multirobot_optimization", + "--env", + "../../dynobench/envs/multirobot/straight.yaml", + "--init", + "../../dynobench/envs/multirobot/guess_indiv_straight.yaml", + "--base", + "../../dynobench/", + "--out", + "buu.yaml", + "--s", + "1", + ">", + "/tmp/db_log.txt"}; + + std::string cmd = ""; + + for (auto &s : run_cmd_new) { + cmd += s + " "; + } + + { + std::cout << "running:\n" << cmd << std::endl; + int out = std::system(cmd.c_str()); + BOOST_TEST(out == 0); + } + + // TODO: @akmaral, Can you add the other test in test_standalone? +} + +BOOST_AUTO_TEST_CASE(t_multi_robot) { + + // TODO: @akmaral, Can you add the other test in test_standalone? + + // 0: optimize the Max time of arrival. + // 1: optimize the sum of the time of arrival of all robots. + bool sum_robots_cost = 1; + std::string dynobench_base = "../../dynobench/"; + std::string env_file = dynobench_base + "envs/multirobot/straight.yaml"; + std::string initial_guess_file = + dynobench_base + "envs/multirobot/guess_indiv_straight.yaml"; + + Problem problem(env_file); + MultiRobotTrajectory init_guess_multi_robot; + init_guess_multi_robot.read_from_yaml(initial_guess_file.c_str()); + + std::vector goal_times(init_guess_multi_robot.trajectories.size()); + + std::transform(init_guess_multi_robot.trajectories.begin(), + init_guess_multi_robot.trajectories.end(), goal_times.begin(), + [](const Trajectory &traj) { return traj.states.size(); }); + + std::cout << "goal times are " << std::endl; + for (auto &t : goal_times) { + std::cout << t << std::endl; + } + + Trajectory init_guess; + if (sum_robots_cost) { + std::cout + << "warning: new approach where each robot tries to reach the goal fast" + << std::endl; + problem.goal_times = goal_times; + } + + else { + std::cout + << "warning: old apprach, robots will reach the goals at the same time " + << std::endl; + } + + Options_trajopt options_trajopt; + options_trajopt.solver_id = 1; + options_trajopt.control_bounds = 1; + options_trajopt.use_warmstart = 1; + options_trajopt.weight_goal = 100; + options_trajopt.max_iter = 50; + problem.models_base_path = dynobench_base + std::string("models/"); + + Result_opti result; + Trajectory sol; + + dynobench::Trajectory init_guess_joint = + init_guess_multi_robot.transform_to_joint_trajectory(); + init_guess.to_yaml_format("/tmp/check2.yaml"); + + trajectory_optimization(problem, init_guess_joint, options_trajopt, sol, + result); + + BOOST_TEST(result.feasible == 1); + + std::cout << "optimization done! " << std::endl; + std::vector index_time_goals; + + if (problem.goal_times.size()) { + index_time_goals = sol.multi_robot_index_goal; + } else { + size_t num_robots = init_guess_multi_robot.get_num_robots(); + index_time_goals = std::vector(num_robots, sol.states.size()); + } + + MultiRobotTrajectory multi_out = from_joint_to_indiv_trajectory( + sol, init_guess_multi_robot.get_nxs(), init_guess_multi_robot.get_nus(), + index_time_goals); + + multi_out.to_yaml_format("/tmp/test_multi.yaml"); +} + +BOOST_AUTO_TEST_CASE(t_multi_robot_swap2_trailer) { + + bool sum_robots_cost = 1; + std::string dynobench_base = "../../dynobench/"; + std::string env_file = + dynobench_base + "envs/multirobot/example/swap2_trailer.yaml"; + std::string initial_guess_file = + dynobench_base + "envs/multirobot/results/swap2_trailer_db.yaml"; + + Problem problem(env_file); + MultiRobotTrajectory init_guess_multi_robot; + init_guess_multi_robot.read_from_yaml(initial_guess_file.c_str()); + + std::vector goal_times(init_guess_multi_robot.trajectories.size()); + + std::transform(init_guess_multi_robot.trajectories.begin(), + init_guess_multi_robot.trajectories.end(), goal_times.begin(), + [](const Trajectory &traj) { return traj.states.size(); }); + + std::cout << "goal times are " << std::endl; + for (auto &t : goal_times) { + std::cout << t << std::endl; + } + + Trajectory init_guess; + if (sum_robots_cost) { + std::cout + << "warning: new approach where each robot tries to reach the goal fast" + << std::endl; + problem.goal_times = goal_times; + } + + else { + std::cout + << "warning: old apprach, robots will reach the goals at the same time " + << std::endl; + } + + Options_trajopt options_trajopt; + options_trajopt.solver_id = 1; + options_trajopt.control_bounds = 1; + options_trajopt.use_warmstart = 1; + options_trajopt.weight_goal = 100; + options_trajopt.max_iter = 50; + problem.models_base_path = dynobench_base + std::string("models/"); + + Result_opti result; + Trajectory sol; + + dynobench::Trajectory init_guess_joint = + init_guess_multi_robot.transform_to_joint_trajectory(); + init_guess.to_yaml_format("/tmp/check3.yaml"); + + trajectory_optimization(problem, init_guess_joint, options_trajopt, sol, + result); + + BOOST_TEST(result.feasible == 1); + + std::cout << "optimization done! " << std::endl; + std::vector index_time_goals; + + if (problem.goal_times.size()) { + index_time_goals = sol.multi_robot_index_goal; + } else { + size_t num_robots = init_guess_multi_robot.get_num_robots(); + index_time_goals = std::vector(num_robots, sol.states.size()); + } + + MultiRobotTrajectory multi_out = from_joint_to_indiv_trajectory( + sol, init_guess_multi_robot.get_nxs(), init_guess_multi_robot.get_nus(), + index_time_goals); + + multi_out.to_yaml_format("/tmp/test_multi_swap2_trailer.yaml"); +} + +BOOST_AUTO_TEST_CASE(t_multi_robot_swap4_unicycle) { + + bool sum_robots_cost = 1; + std::string dynobench_base = "../../dynobench/"; + std::string env_file = + dynobench_base + "envs/multirobot/example/swap4_unicycle.yaml"; + std::string initial_guess_file = + dynobench_base + "envs/multirobot/results/swap4_unicycle_db.yaml"; + + Problem problem(env_file); + MultiRobotTrajectory init_guess_multi_robot; + init_guess_multi_robot.read_from_yaml(initial_guess_file.c_str()); + + std::vector goal_times(init_guess_multi_robot.trajectories.size()); + + std::transform(init_guess_multi_robot.trajectories.begin(), + init_guess_multi_robot.trajectories.end(), goal_times.begin(), + [](const Trajectory &traj) { return traj.states.size(); }); + + std::cout << "goal times are " << std::endl; + for (auto &t : goal_times) { + std::cout << t << std::endl; + } + + Trajectory init_guess; + if (sum_robots_cost) { + std::cout + << "warning: new approach where each robot tries to reach the goal fast" + << std::endl; + problem.goal_times = goal_times; + } + + else { + std::cout + << "warning: old apprach, robots will reach the goals at the same time " + << std::endl; + } + + Options_trajopt options_trajopt; + options_trajopt.solver_id = 1; + options_trajopt.control_bounds = 1; + options_trajopt.use_warmstart = 1; + options_trajopt.weight_goal = 100; + options_trajopt.max_iter = 50; + problem.models_base_path = dynobench_base + std::string("models/"); + + Result_opti result; + Trajectory sol; + + dynobench::Trajectory init_guess_joint = + init_guess_multi_robot.transform_to_joint_trajectory(); + + init_guess_joint.to_yaml_format("/tmp/check4.yaml"); + + trajectory_optimization(problem, init_guess_joint, options_trajopt, sol, + result); + + BOOST_TEST(result.feasible == 1); + + std::cout << "optimization done! " << std::endl; + std::vector index_time_goals; + + if (problem.goal_times.size()) { + index_time_goals = sol.multi_robot_index_goal; + } else { + size_t num_robots = init_guess_multi_robot.get_num_robots(); + index_time_goals = std::vector(num_robots, sol.states.size()); + } + + MultiRobotTrajectory multi_out = from_joint_to_indiv_trajectory( + sol, init_guess_multi_robot.get_nxs(), init_guess_multi_robot.get_nus(), + index_time_goals); + + multi_out.to_yaml_format("/tmp/test_multi_swap4_unicycle.yaml"); +} + +BOOST_AUTO_TEST_CASE(t_multi_but_only_one) { + + bool sum_robots_cost = 1; + std::string dynobench_base = "../../dynobench/"; + std::string env_file = dynobench_base + "envs/multirobot/swap1_trailer.yaml"; + std::string initial_guess_file = + dynobench_base + "envs/multirobot/swap1_trailer_db.yaml"; + + Problem problem(env_file); + MultiRobotTrajectory init_guess_multi_robot; + init_guess_multi_robot.read_from_yaml(initial_guess_file.c_str()); + + std::vector goal_times(init_guess_multi_robot.trajectories.size()); + + std::transform(init_guess_multi_robot.trajectories.begin(), + init_guess_multi_robot.trajectories.end(), goal_times.begin(), + [](const Trajectory &traj) { return traj.states.size(); }); + + std::cout << "goal times are " << std::endl; + for (auto &t : goal_times) { + std::cout << t << std::endl; + } + + Trajectory init_guess; + if (sum_robots_cost) { + std::cout + << "warning: new approach where each robot tries to reach the goal fast" + << std::endl; + problem.goal_times = goal_times; + } + + else { + std::cout + << "warning: old apprach, robots will reach the goals at the same time " + << std::endl; + } + + Options_trajopt options_trajopt; + options_trajopt.solver_id = 1; + options_trajopt.control_bounds = 1; + options_trajopt.use_warmstart = 1; + options_trajopt.weight_goal = 100; + options_trajopt.max_iter = 50; + problem.models_base_path = dynobench_base + std::string("models/"); + + Result_opti result; + Trajectory sol; + + dynobench::Trajectory init_guess_joint = + init_guess_multi_robot.transform_to_joint_trajectory(); + init_guess_joint.to_yaml_format("/tmp/check4.yaml"); + + trajectory_optimization(problem, init_guess_joint, options_trajopt, sol, + result); + + BOOST_TEST(result.feasible == 1); + + std::cout << "optimization done! " << std::endl; + std::vector index_time_goals; + + if (problem.goal_times.size()) { + index_time_goals = sol.multi_robot_index_goal; + } else { + size_t num_robots = init_guess_multi_robot.get_num_robots(); + index_time_goals = std::vector(num_robots, sol.states.size()); + } + + MultiRobotTrajectory multi_out = from_joint_to_indiv_trajectory( + sol, init_guess_multi_robot.get_nxs(), init_guess_multi_robot.get_nus(), + index_time_goals); + + multi_out.to_yaml_format("/tmp/test_multi_swap1_unicycle.yaml"); +}