-
Notifications
You must be signed in to change notification settings - Fork 11
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge remote-tracking branch 'origin/main' into tmp-dev
- Loading branch information
Showing
10 changed files
with
745 additions
and
57 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Submodule dynobench
updated
59 files
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,139 @@ | ||
| #include <algorithm> | ||
| #include <boost/program_options.hpp> | ||
| #include <chrono> | ||
| #include <fstream> | ||
| #include <iostream> | ||
| #include <iterator> | ||
| #include <yaml-cpp/yaml.h> | ||
| #include "dynobench/multirobot_trajectory.hpp" | ||
|
|
||
| // OMPL headers | ||
|
|
||
| #include <dynoplan/optimization/ocp.hpp> | ||
|
|
||
|
|
||
| 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<int> 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<int>(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<int> 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<int>(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<std::string>(&envFile)->required())( | ||
| "init,i", po::value<std::string>(&initFile)->required())( | ||
| "out,o", po::value<std::string>(&outFile)->required()) | ||
| ( | ||
| "base,b", po::value<std::string>(&dynobench_base)->required()) | ||
|
|
||
| ( | ||
| "sum,s", po::value<bool>(&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); | ||
| } |
Oops, something went wrong.