From 18d1a869e32968eb998cf0dfb7f3c5da444d44d1 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 10 Jun 2024 20:24:02 +0000 Subject: [PATCH] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- include/dynoplan/dbastar/heuristics.hpp | 14 ++--- .../dynoplan/optimization/croco_models.hpp | 6 +-- src/dbastar/dbastar.cpp | 13 +++-- src/dbrrt/dbrrt.cpp | 17 +++--- src/idbastar/idbastar.cpp | 4 +- src/motion_primitives/main_primitives.cpp | 3 +- src/motion_primitives/motion_primitives.cpp | 4 +- src/ompl/robots.cpp | 4 +- src/ompl/sst.cpp | 6 ++- src/optimization/croco_models.cpp | 54 +++++++++---------- src/optimization/ocp.cpp | 40 +++++++++----- src/tdbastar/tdbastar.cpp | 13 +++-- 12 files changed, 104 insertions(+), 74 deletions(-) diff --git a/include/dynoplan/dbastar/heuristics.hpp b/include/dynoplan/dbastar/heuristics.hpp index 20155ac..b5ee02a 100644 --- a/include/dynoplan/dbastar/heuristics.hpp +++ b/include/dynoplan/dbastar/heuristics.hpp @@ -126,7 +126,7 @@ struct Heu_euclidean : Heu_fun { return robot->lower_bound_time(x, goal); } - virtual ~Heu_euclidean() override{}; + virtual ~Heu_euclidean() override {}; }; struct Heu_blind : Heu_fun { @@ -138,7 +138,7 @@ struct Heu_blind : Heu_fun { return 0; } - virtual ~Heu_blind() override{}; + virtual ~Heu_blind() override {}; }; struct Heu_roadmap : Heu_fun { @@ -181,12 +181,12 @@ struct Heu_roadmap : Heu_fun { return std::max(vel_h, pos_h); } - virtual ~Heu_roadmap() override{ + virtual ~Heu_roadmap() override { - // robot->getSpaceInformation()->freeState(__x_zero_vel); + // robot->getSpaceInformation()->freeState(__x_zero_vel); - // TODO: memory leak - // I have to delete more stuff!!! + // TODO: memory leak + // I have to delete more stuff!!! }; }; @@ -213,7 +213,7 @@ template struct Heu_roadmap_bwd : Heu_fun { } } - virtual ~Heu_roadmap_bwd() override{}; + virtual ~Heu_roadmap_bwd() override {}; }; void build_heuristic_distance_new( diff --git a/include/dynoplan/optimization/croco_models.hpp b/include/dynoplan/optimization/croco_models.hpp index 1fdfba8..f5718f6 100644 --- a/include/dynoplan/optimization/croco_models.hpp +++ b/include/dynoplan/optimization/croco_models.hpp @@ -117,7 +117,7 @@ struct StateCrocoDyno : crocoddyl::StateAbstractTpl { bool use_pin = true; std::shared_ptr state; explicit StateCrocoDyno(const std::shared_ptr &state) - : StateAbstractTpl(state->nx, state->ndx), state(state){}; + : StateAbstractTpl(state->nx, state->ndx), state(state) {}; virtual ~StateCrocoDyno() {} @@ -276,7 +276,7 @@ struct Dynamics { Eigen::VectorXd x_ub; Eigen::VectorXd x_weightb; - virtual ~Dynamics(){}; + virtual ~Dynamics() {}; }; struct Cost { @@ -479,7 +479,7 @@ struct Dynamics_free_time { Eigen::VectorXd x_ub; Eigen::VectorXd x_weightb; - virtual ~Dynamics_free_time(){}; + virtual ~Dynamics_free_time() {}; }; struct Quaternion_cost : Cost { diff --git a/src/dbastar/dbastar.cpp b/src/dbastar/dbastar.cpp index 8fde1cd..668c037 100644 --- a/src/dbastar/dbastar.cpp +++ b/src/dbastar/dbastar.cpp @@ -288,8 +288,8 @@ void from_solution_to_yaml_and_traj(dynobench::Model_robot &robot, } DYNO_CHECK_LEQ(take_num_actions, motion.actions.size(), AT); if (out) { - *out << space6 + "# " << "take_num_actions " << take_num_actions - << std::endl; + *out << space6 + "# " + << "take_num_actions " << take_num_actions << std::endl; } for (size_t k = 0; k < take_num_actions; ++k) { @@ -703,19 +703,22 @@ void dbastar(const dynobench::Problem &problem, Options_dbastar options_dbastar, if (static_cast(time_bench.expands) >= options_dbastar.max_expands) { status = Terminate_status::MAX_EXPANDS; - std::cout << "BREAK search:" << "MAX_EXPANDS" << std::endl; + std::cout << "BREAK search:" + << "MAX_EXPANDS" << std::endl; return true; } if (watch.elapsed_ms() > options_dbastar.search_timelimit) { status = Terminate_status::MAX_TIME; - std::cout << "BREAK search:" << "MAX_TIME" << std::endl; + std::cout << "BREAK search:" + << "MAX_TIME" << std::endl; return true; } if (open.empty()) { status = Terminate_status::EMPTY_QUEUE; - std::cout << "BREAK search:" << "EMPTY_QUEUE" << std::endl; + std::cout << "BREAK search:" + << "EMPTY_QUEUE" << std::endl; return true; } diff --git a/src/dbrrt/dbrrt.cpp b/src/dbrrt/dbrrt.cpp index 434b0ae..264deda 100644 --- a/src/dbrrt/dbrrt.cpp +++ b/src/dbrrt/dbrrt.cpp @@ -497,14 +497,16 @@ void dbrrtConnect(const dynobench::Problem &problem, auto stop_search = [&] { if (static_cast(time_bench.expands) >= options_dbrrt.max_expands) { status = Terminate_status::MAX_EXPANDS; - std::cout << "BREAK search:" << "MAX_EXPANDS" << std::endl; + std::cout << "BREAK search:" + << "MAX_EXPANDS" << std::endl; return true; } if (watch.elapsed_ms() > options_dbrrt.timelimit) { status = Terminate_status::MAX_TIME; - std::cout << "BREAK search:" << "MAX_TIME" << std::endl; + std::cout << "BREAK search:" + << "MAX_TIME" << std::endl; return true; } return false; @@ -1049,14 +1051,16 @@ void dbrrt(const dynobench::Problem &problem, auto stop_search = [&] { if (static_cast(time_bench.expands) >= options_dbrrt.max_expands) { status = Terminate_status::MAX_EXPANDS; - std::cout << "BREAK search:" << "MAX_EXPANDS" << std::endl; + std::cout << "BREAK search:" + << "MAX_EXPANDS" << std::endl; return true; } if (watch.elapsed_ms() > options_dbrrt.timelimit) { status = Terminate_status::MAX_TIME; - std::cout << "BREAK search:" << "MAX_TIME" << std::endl; + std::cout << "BREAK search:" + << "MAX_TIME" << std::endl; return true; } return false; @@ -1116,8 +1120,9 @@ void dbrrt(const dynobench::Problem &problem, if (options_dbrrt.ao_rrt && near_node->gScore + near_node->hScore > options_dbrrt.best_cost_prune_factor * cost_bound) { - std::cout << "warning! " << "cost of near is above bound -- " - << near_node->gScore << " " << cost_bound << std::endl; + std::cout << "warning! " + << "cost of near is above bound -- " << near_node->gScore << " " + << cost_bound << std::endl; continue; } diff --git a/src/idbastar/idbastar.cpp b/src/idbastar/idbastar.cpp index 17846f5..0cb8e50 100644 --- a/src/idbastar/idbastar.cpp +++ b/src/idbastar/idbastar.cpp @@ -280,8 +280,8 @@ void idbA(const dynobench::Problem &problem, std::make_move_iterator(motions_out.end())); std::cout << "Afer insert " << motions.size() << std::endl; - std::cout << "Warning: " << "I am inserting at the beginning" - << std::endl; + std::cout << "Warning: " + << "I am inserting at the beginning" << std::endl; std::cout << "i update the idx of the motions " << std::endl; // Very important to update the idx of the motions diff --git a/src/motion_primitives/main_primitives.cpp b/src/motion_primitives/main_primitives.cpp index 71f4637..e4e8212 100644 --- a/src/motion_primitives/main_primitives.cpp +++ b/src/motion_primitives/main_primitives.cpp @@ -379,7 +379,8 @@ int main(int argc, const char *argv[]) { } CSTR_(trajectories.data.size()); trajectories.compute_stats(out_file.c_str()); - std::cout << "writing stats to " << "/tmp/tmp_stats.yaml" << std::endl; + std::cout << "writing stats to " + << "/tmp/tmp_stats.yaml" << std::endl; std::filesystem::copy(out_file, "/tmp/tmp_stats.yaml", std::filesystem::copy_options::overwrite_existing); } diff --git a/src/motion_primitives/motion_primitives.cpp b/src/motion_primitives/motion_primitives.cpp index 6c674f3..faee48d 100644 --- a/src/motion_primitives/motion_primitives.cpp +++ b/src/motion_primitives/motion_primitives.cpp @@ -334,8 +334,8 @@ void split_motion_primitives(const dynobench::Trajectories &in, } dynobench::Trajectory new_traj; - std::cout << "i: " << i << " " << "start: " << start - << " length:" << length << std::endl; + std::cout << "i: " << i << " " + << "start: " << start << " length:" << length << std::endl; new_traj.states = std::vector{ traj.states.begin() + start, traj.states.begin() + start + length + 1}; diff --git a/src/ompl/robots.cpp b/src/ompl/robots.cpp index 1f85981..905700e 100644 --- a/src/ompl/robots.cpp +++ b/src/ompl/robots.cpp @@ -3114,8 +3114,8 @@ void load_motion_primitives_new(const std::string &motionsFile, CSTR_(add_noise_first_state); if (add_noise_first_state) { - std::cout << "WARNING:" << "adding noise to first and last state" - << std::endl; + std::cout << "WARNING:" + << "adding noise to first and last state" << std::endl; const double noise = 1e-7; for (auto &t : trajs.data) { t.states.front() += diff --git a/src/ompl/sst.cpp b/src/ompl/sst.cpp index ddad3e8..0471bd5 100644 --- a/src/ompl/sst.cpp +++ b/src/ompl/sst.cpp @@ -422,8 +422,10 @@ void solve_sst(const dynobench::Problem &problem, auto &state = states.at(i); auto &edge = edges.at(i); out << "- " << std::endl; - out << " " << "x: " << state.format(FMT) << std::endl; - out << " " << "e: "; + out << " " + << "x: " << state.format(FMT) << std::endl; + out << " " + << "e: "; print_vec(edge.data(), edge.size(), out); } } diff --git a/src/optimization/croco_models.cpp b/src/optimization/croco_models.cpp index 4d3bcb3..fc83c05 100644 --- a/src/optimization/croco_models.cpp +++ b/src/optimization/croco_models.cpp @@ -165,21 +165,21 @@ void check_input_calc(Eigen::Ref xnext, const Eigen::Ref &u, size_t nx, size_t nu) { if (static_cast(x.size()) != nx) { - throw_pretty( - "Invalid argument: " << "x has wrong dimension (it should be " + - std::to_string(nx) + ")"); + throw_pretty("Invalid argument: " + << "x has wrong dimension (it should be " + + std::to_string(nx) + ")"); } if (static_cast(u.size()) != nu) { - throw_pretty( - "Invalid argument: " << "x has wrong dimension (it should be " + - std::to_string(nu) + ")"); + throw_pretty("Invalid argument: " + << "x has wrong dimension (it should be " + + std::to_string(nu) + ")"); } if (static_cast(xnext.size()) != nx) { - throw_pretty( - "Invalid argument: " << "xnext has wrong dimension (it should be " + - std::to_string(nx) + ")"); + throw_pretty("Invalid argument: " + << "xnext has wrong dimension (it should be " + + std::to_string(nx) + ")"); } }; @@ -190,38 +190,38 @@ void check_input_calcdiff(Eigen::Ref Fx, size_t nu) { if (static_cast(x.size()) != nx) { - throw_pretty( - "Invalid argument: " << "x has wrong dimension (it should be " + - std::to_string(nx) + ")"); + throw_pretty("Invalid argument: " + << "x has wrong dimension (it should be " + + std::to_string(nx) + ")"); } if (static_cast(u.size()) != nu) { - throw_pretty( - "Invalid argument: " << "u has wrong dimension (it should be " + - std::to_string(nu) + ")"); + throw_pretty("Invalid argument: " + << "u has wrong dimension (it should be " + + std::to_string(nu) + ")"); } if (static_cast(Fx.cols()) != nx) { - throw_pretty( - "Invalid argument: " << "Fx has wrong dimension (it should be " + - std::to_string(nx) + ")"); + throw_pretty("Invalid argument: " + << "Fx has wrong dimension (it should be " + + std::to_string(nx) + ")"); } if (static_cast(Fx.rows()) != nx) { - throw_pretty( - "Invalid argument: " << "Fx has wrong dimension (it should be " + - std::to_string(nx) + ")"); + throw_pretty("Invalid argument: " + << "Fx has wrong dimension (it should be " + + std::to_string(nx) + ")"); } if (static_cast(Fu.cols()) != nu) { - throw_pretty( - "Invalid argument: " << "Fu has wrong dimension (it should be " + - std::to_string(nx) + ")"); + throw_pretty("Invalid argument: " + << "Fu has wrong dimension (it should be " + + std::to_string(nx) + ")"); } if (static_cast(Fu.rows()) != nx) { - throw_pretty( - "Invalid argument: " << "Fu has wrong dimension (it should be " + - std::to_string(nx) + ")"); + throw_pretty("Invalid argument: " + << "Fu has wrong dimension (it should be " + + std::to_string(nx) + ")"); } } diff --git a/src/optimization/ocp.cpp b/src/optimization/ocp.cpp index 4621a77..c0e923a 100644 --- a/src/optimization/ocp.cpp +++ b/src/optimization/ocp.cpp @@ -614,7 +614,8 @@ void fix_problem_quaternion(Eigen::VectorXd &start, Eigen::VectorXd &goal, double d2 = (xs_init.front().segment<4>(3) + start.segment<4>(3)).norm(); if (d2 < d1) { - std::cout << "WARNING: " << "i flip the start state" << std::endl; + std::cout << "WARNING: " + << "i flip the start state" << std::endl; xs_init.front().segment<4>(3) *= -1.; } @@ -1080,8 +1081,8 @@ void __trajectory_optimization( SOLVER solver = static_cast(options_trajopt_local.solver_id); if (modify_to_match_goal_start) { - std::cout << "WARNING: " << "i modify last state to match goal" - << std::endl; + std::cout << "WARNING: " + << "i modify last state to match goal" << std::endl; xs_init.back() = goal; xs_init.front() = start; } @@ -1648,14 +1649,16 @@ void __trajectory_optimization( if (is_last) { finished = true; - std::cout << "finished: " << "is_last=TRUE" << std::endl; + std::cout << "finished: " + << "is_last=TRUE" << std::endl; } counter++; if (counter > options_trajopt_local.max_mpc_iterations) { finished = true; - std::cout << "finished: " << "max mpc iterations" << std::endl; + std::cout << "finished: " + << "max mpc iterations" << std::endl; } } std::cout << "Total TIME: " << total_time << std::endl; @@ -2085,7 +2088,9 @@ void trajectory_optimization(const dynobench::Problem &problem, CSTR_(time_ddp_total); if (!opti_out.success) { - std::cout << "warning" << " " << "not success" << std::endl; + std::cout << "warning" + << " " + << "not success" << std::endl; do_mpcc = false; break; } @@ -2110,7 +2115,9 @@ void trajectory_optimization(const dynobench::Problem &problem, time_ddp_total += std::stod(opti_out.data.at("ddp_time")); CSTR_(time_ddp_total); if (!opti_out.success) { - std::cout << "warning" << " " << "not success" << std::endl; + std::cout << "warning" + << " " + << "not success" << std::endl; break; } } @@ -2137,7 +2144,9 @@ void trajectory_optimization(const dynobench::Problem &problem, time_ddp_total += std::stod(opti_out.data.at("ddp_time")); CSTR_(time_ddp_total); if (!opti_out.success) { - std::cout << "warning" << " " << "not success" << std::endl; + std::cout << "warning" + << " " + << "not success" << std::endl; do_free_time = false; } @@ -2185,7 +2194,9 @@ void trajectory_optimization(const dynobench::Problem &problem, time_ddp_total += std::stod(opti_out.data.at("ddp_time")); CSTR_(time_ddp_total); if (!opti_out.success) { - std::cout << "warning" << " " << "fail first step" << std::endl; + std::cout << "warning" + << " " + << "fail first step" << std::endl; do_free_time = false; } @@ -2363,7 +2374,8 @@ void trajectory_optimization(const dynobench::Problem &problem, CSTR_(time_ddp_total); if (!opti_out.success) { - std::cout << "warning:" << "not success" << std::endl; + std::cout << "warning:" + << "not success" << std::endl; do_final_repair_step = false; } @@ -2398,7 +2410,9 @@ void trajectory_optimization(const dynobench::Problem &problem, CSTR_(time_ddp_total); if (!opti_out.success) { - std::cout << "warning" << " " << "infeasible" << std::endl; + std::cout << "warning" + << " " + << "infeasible" << std::endl; do_final_repair_step = false; } @@ -2509,7 +2523,9 @@ void trajectory_optimization(const dynobench::Problem &problem, // solve without bounds -- if (!opti_out.success) { - std::cout << "warning" << " " << "not success" << std::endl; + std::cout << "warning" + << " " + << "not success" << std::endl; do_opti_with_real_bounds = false; } diff --git a/src/tdbastar/tdbastar.cpp b/src/tdbastar/tdbastar.cpp index 41460bf..efc2d87 100644 --- a/src/tdbastar/tdbastar.cpp +++ b/src/tdbastar/tdbastar.cpp @@ -276,8 +276,8 @@ void from_solution_to_yaml_and_traj(dynobench::Model_robot &robot, } DYNO_CHECK_LEQ(take_num_actions, motion.actions.size(), AT); if (out) { - *out << space6 + "# " << "take_num_actions " << take_num_actions - << std::endl; + *out << space6 + "# " + << "take_num_actions " << take_num_actions << std::endl; } for (size_t k = 0; k < take_num_actions; ++k) { @@ -675,19 +675,22 @@ void tdbastar( if (static_cast(time_bench.expands) >= options_tdbastar.max_expands) { status = Terminate_status::MAX_EXPANDS; - std::cout << "BREAK search:" << "MAX_EXPANDS" << std::endl; + std::cout << "BREAK search:" + << "MAX_EXPANDS" << std::endl; return true; } if (watch.elapsed_ms() > options_tdbastar.search_timelimit) { status = Terminate_status::MAX_TIME; - std::cout << "BREAK search:" << "MAX_TIME" << std::endl; + std::cout << "BREAK search:" + << "MAX_TIME" << std::endl; return true; } if (open.empty()) { status = Terminate_status::EMPTY_QUEUE; - std::cout << "BREAK search:" << "EMPTY_QUEUE" << std::endl; + std::cout << "BREAK search:" + << "EMPTY_QUEUE" << std::endl; return true; }