diff --git a/trajopt/include/trajopt/collision_terms.hpp b/trajopt/include/trajopt/collision_terms.hpp index 9a0a8570..f0ee862d 100644 --- a/trajopt/include/trajopt/collision_terms.hpp +++ b/trajopt/include/trajopt/collision_terms.hpp @@ -1,4 +1,5 @@ #pragma once +#include #include #include #include @@ -24,7 +25,7 @@ using ContactResultVectorConstPtr = std::shared_ptr + namespace trajopt { // collision_terms.hpp -enum class CollisionExpressionEvaluatorType; +enum class CollisionExpressionEvaluatorType : std::uint8_t; struct LinkGradientResults; struct GradientResults; struct CollisionEvaluator; @@ -25,7 +27,7 @@ struct JointPosTermInfo; struct JointVelTermInfo; struct JointAccTermInfo; struct JointJerkTermInfo; -enum class CollisionEvaluatorType; +enum class CollisionEvaluatorType : std::uint8_t; struct CollisionTermInfo; struct TotalTimeTermInfo; struct AvoidSingularityTermInfo; diff --git a/trajopt/include/trajopt/json_marshal.hpp b/trajopt/include/trajopt/json_marshal.hpp index a5676e8c..cb78227a 100644 --- a/trajopt/include/trajopt/json_marshal.hpp +++ b/trajopt/include/trajopt/json_marshal.hpp @@ -10,7 +10,7 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace Json { class Value; -} +} // namespace Json namespace json_marshal { diff --git a/trajopt/include/trajopt/problem_description.hpp b/trajopt/include/trajopt/problem_description.hpp index 5e6563a8..ce2b25c4 100644 --- a/trajopt/include/trajopt/problem_description.hpp +++ b/trajopt/include/trajopt/problem_description.hpp @@ -16,7 +16,7 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace sco { struct OptResults; -} +} // namespace sco namespace trajopt { @@ -169,7 +169,7 @@ struct InitInfo In all cases the dt column (if present) is appended the selected method is defined. */ - enum Type + enum Type : std::uint8_t { STATIONARY, JOINT_INTERPOLATED, @@ -581,7 +581,7 @@ struct JointJerkTermInfo : public TermInfo JointJerkTermInfo() : TermInfo(TermType::TT_COST | TermType::TT_CNT) {} }; -enum class CollisionEvaluatorType +enum class CollisionEvaluatorType : std::uint8_t { SINGLE_TIMESTEP = 0, DISCRETE_CONTINUOUS = 1, diff --git a/trajopt/src/collision_terms.cpp b/trajopt/src/collision_terms.cpp index bef3f631..c720a932 100644 --- a/trajopt/src/collision_terms.cpp +++ b/trajopt/src/collision_terms.cpp @@ -809,7 +809,7 @@ void CollisionEvaluator::removeInvalidContactResults(tesseract_collision::Contac const std::array& pair_data) const { auto end = std::remove_if( - contact_results.begin(), contact_results.end(), [=, &pair_data](const tesseract_collision::ContactResult& r) { + contact_results.begin(), contact_results.end(), [this, &pair_data](const tesseract_collision::ContactResult& r) { switch (evaluator_type_) { case CollisionExpressionEvaluatorType::START_FREE_END_FREE: @@ -1010,7 +1010,7 @@ void SingleTimestepCollisionEvaluator::Plot(const std::shared_ptrgetPairSafetyMarginData(link1, link2)[0]; }; @@ -1256,7 +1256,7 @@ void DiscreteCollisionEvaluator::Plot(const std::shared_ptrgetPairSafetyMarginData(link1, link2)[0]; }; @@ -1508,7 +1508,7 @@ void CastCollisionEvaluator::Plot(const std::shared_ptrgetPairSafetyMarginData(link1, link2)[0]; }; diff --git a/trajopt/src/file_write_callback.cpp b/trajopt/src/file_write_callback.cpp index 3011614e..00fc1ac3 100644 --- a/trajopt/src/file_write_callback.cpp +++ b/trajopt/src/file_write_callback.cpp @@ -88,9 +88,9 @@ void WriteFile(const std::shared_ptr& file, *file << ',' << constraint; } - *file << std::endl; + *file << '\n'; } - *file << std::endl; + *file << '\n' << std::flush; } // namespace trajopt std::function WriteCallback(std::shared_ptr file, @@ -133,7 +133,7 @@ std::function WriteCallback(std::shared_p *file << ',' << cnt->name(); } - *file << std::endl; + *file << '\n' << std::flush; // return callback function return [file, capture0 = prob->GetKin(), &capture1 = prob->GetVars()](auto&&, auto&& PH2) { diff --git a/trajopt/src/problem_description.cpp b/trajopt/src/problem_description.cpp index c9c5d6e1..4f8f59ea 100644 --- a/trajopt/src/problem_description.cpp +++ b/trajopt/src/problem_description.cpp @@ -243,7 +243,7 @@ void ProblemConstructionInfo::readInitInfo(const Json::Value& v) for (int i = 0; i < n_steps; ++i) { DblVec row; - json_marshal::fromJsonArray(vdata[i], row, static_cast(n_dof)); + json_marshal::fromJsonArray(vdata[i], row, n_dof); init_info.data.row(i) = trajopt_common::toVectorXd(row); } } diff --git a/trajopt_common/CMakeLists.txt b/trajopt_common/CMakeLists.txt index caf931e9..db92a77b 100644 --- a/trajopt_common/CMakeLists.txt +++ b/trajopt_common/CMakeLists.txt @@ -26,7 +26,6 @@ set(UTILS_SOURCE_FILES src/clock.cpp src/collision_types.cpp src/collision_utils.cpp - src/config.cpp src/logging.cpp src/utils.cpp) diff --git a/trajopt_common/include/trajopt_common/logging.hpp b/trajopt_common/include/trajopt_common/logging.hpp index b01eba14..ee7e3926 100644 --- a/trajopt_common/include/trajopt_common/logging.hpp +++ b/trajopt_common/include/trajopt_common/logging.hpp @@ -1,12 +1,13 @@ #pragma once #include +#include TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_common { -enum LogLevel +enum LogLevel : std::uint8_t { LevelFatal = 0, LevelError = 1, diff --git a/trajopt_common/include/trajopt_common/math.hpp b/trajopt_common/include/trajopt_common/math.hpp deleted file mode 100644 index 2b9a8bd1..00000000 --- a/trajopt_common/include/trajopt_common/math.hpp +++ /dev/null @@ -1,10 +0,0 @@ -#pragma once -#include -TRAJOPT_IGNORE_WARNINGS_PUSH -#include -TRAJOPT_IGNORE_WARNINGS_POP - -namespace trajopt_common -{ -inline float randf() { return (float)rand() / (float)RAND_MAX; } -} // namespace trajopt_common diff --git a/trajopt_common/src/config.cpp b/trajopt_common/src/config.cpp index 558974fa..56ea3560 100644 --- a/trajopt_common/src/config.cpp +++ b/trajopt_common/src/config.cpp @@ -19,8 +19,8 @@ void CommandParser::read(int argc, char* argv[]) po::store(po::command_line_parser(argc, argv).options(od).run(), vm); if (vm.count("help") != 0U) { - std::cout << "usage: " << argv[0] << " [options]" << std::endl; - std::cout << od << std::endl; + std::cout << "usage: " << argv[0] << " [options]" << '\n'; + std::cout << od << '\n'; exit(0); } po::notify(vm); diff --git a/trajopt_common/src/logging.cpp b/trajopt_common/src/logging.cpp index b027dd13..d89894c5 100644 --- a/trajopt_common/src/logging.cpp +++ b/trajopt_common/src/logging.cpp @@ -14,7 +14,7 @@ int LoggingInit() { const char* VALID_THRESH_VALUES = "FATAL ERROR WARN INFO DEBUG TRACE"; - char* lvlc = getenv("TRAJOPT_LOG_THRESH"); + const char* lvlc = getenv("TRAJOPT_LOG_THRESH"); std::string lvlstr; if (lvlc == nullptr) { diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h index 2b963813..263553a8 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h @@ -46,7 +46,7 @@ struct CartPosInfo using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - enum class Type + enum class Type : std::uint8_t { TARGET_ACTIVE, SOURCE_ACTIVE, diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/fwd.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/fwd.h index 6f5b5240..90250890 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/fwd.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/fwd.h @@ -1,12 +1,14 @@ #ifndef TRAJOPT_SQP_FWD_H #define TRAJOPT_SQP_FWD_H +#include + namespace trajopt_sqp { // types.h -enum class ConstraintType; -enum class CostPenaltyType; -enum class SQPStatus; +enum class ConstraintType : std::uint8_t; +enum class CostPenaltyType : std::uint8_t; +enum class SQPStatus : std::uint8_t; struct SQPParameters; struct SQPResults; @@ -19,7 +21,7 @@ struct QuadExprs; class QPProblem; // qp_solver.h -enum class QPSolverStatus; +enum class QPSolverStatus : std::uint8_t; class QPSolver; // osqp_eigen_solver.h diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/ifopt_qp_problem.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/ifopt_qp_problem.h index f674aeaa..017a2737 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/ifopt_qp_problem.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/ifopt_qp_problem.h @@ -33,7 +33,7 @@ namespace ifopt { class Problem; -} +} // namespace ifopt namespace trajopt_sqp { diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/osqp_eigen_solver.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/osqp_eigen_solver.h index 65099478..c8c68aad 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/osqp_eigen_solver.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/osqp_eigen_solver.h @@ -31,7 +31,7 @@ namespace OsqpEigen { class Solver; -} +} // namespace OsqpEigen namespace trajopt_sqp { diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_problem.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_problem.h index bef3f1f6..07dbca1b 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_problem.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_problem.h @@ -12,7 +12,7 @@ class ConstraintSet; namespace trajopt_sqp { -enum class CostPenaltyType; +enum class CostPenaltyType : std::uint8_t; /** @brief QP Problem Base */ class QPProblem diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_solver.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_solver.h index 60625865..11df026c 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_solver.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_solver.h @@ -32,7 +32,7 @@ namespace trajopt_sqp { -enum class QPSolverStatus +enum class QPSolverStatus : std::uint8_t { UNITIALIZED, INITIALIZED, diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/types.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/types.h index a4985182..1ba279ac 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/types.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/types.h @@ -30,13 +30,13 @@ namespace trajopt_sqp { -enum class ConstraintType +enum class ConstraintType : std::uint8_t { EQ, INEQ }; -enum class CostPenaltyType +enum class CostPenaltyType : std::uint8_t { SQUARED, ABSOLUTE, @@ -157,7 +157,7 @@ struct SQPResults /** * @brief Status codes for the SQP Optimization */ -enum class SQPStatus +enum class SQPStatus : std::uint8_t { RUNNING, /**< Optimization is currently running */ NLP_CONVERGED, /**< NLP Successfully converged */ diff --git a/trajopt_optimizers/trajopt_sqp/src/callbacks/collision_plotter.cpp b/trajopt_optimizers/trajopt_sqp/src/callbacks/collision_plotter.cpp index 46d60138..9ebd260d 100644 --- a/trajopt_optimizers/trajopt_sqp/src/callbacks/collision_plotter.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/callbacks/collision_plotter.cpp @@ -39,7 +39,7 @@ CollisionPlottingCallback::CollisionPlottingCallback(std::shared_ptr(cnt) << ", "; - std::cout << std::endl; - std::cout << "box_size_: " << box_size_.transpose().format(format) << std::endl; // NOLINT - std::cout << "constraint_merit_coeff_: " << constraint_merit_coeff_.transpose().format(format) << std::endl; + std::cout << '\n'; + std::cout << "box_size_: " << box_size_.transpose().format(format) << '\n'; // NOLINT + std::cout << "constraint_merit_coeff_: " << constraint_merit_coeff_.transpose().format(format) << '\n'; - std::cout << "Hessian:\n" << hessian_.toDense().format(format) << std::endl; - std::cout << "Gradient: " << gradient_.transpose().format(format) << std::endl; - std::cout << "Constraint Matrix:\n" << constraint_matrix_.toDense().format(format) << std::endl; - std::cout << "bounds_lower: " << bounds_lower_.transpose().format(format) << std::endl; - std::cout << "bounds_upper: " << bounds_upper_.transpose().format(format) << std::endl; - std::cout << "NLP values: " << nlp_->GetVariableValues().transpose().format(format) << std::endl; + std::cout << "Hessian:\n" << hessian_.toDense().format(format) << '\n'; + std::cout << "Gradient: " << gradient_.transpose().format(format) << '\n'; + std::cout << "Constraint Matrix:\n" << constraint_matrix_.toDense().format(format) << '\n'; + std::cout << "bounds_lower: " << bounds_lower_.transpose().format(format) << '\n'; + std::cout << "bounds_upper: " << bounds_upper_.transpose().format(format) << '\n'; + std::cout << "NLP values: " << nlp_->GetVariableValues().transpose().format(format) << '\n'; } } // namespace trajopt_sqp diff --git a/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp b/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp index 01f846a3..8d6c3a77 100644 --- a/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp @@ -81,55 +81,55 @@ bool OSQPEigenSolver::solve() { Eigen::IOFormat format(5); const auto* osqp_data = solver_->data()->getData(); - std::cout << "OSQP Number of Variables:" << osqp_data->n << std::endl; - std::cout << "OSQP Number of Constraints:" << osqp_data->m << std::endl; + std::cout << "OSQP Number of Variables:" << osqp_data->n << '\n'; + std::cout << "OSQP Number of Constraints:" << osqp_data->m << '\n'; Eigen::Map> P_p_vec(osqp_data->P->p, osqp_data->P->n + 1); Eigen::Map> P_i_vec(osqp_data->P->i, osqp_data->P->nzmax); Eigen::Map> P_x_vec(osqp_data->P->x, osqp_data->P->nzmax); - std::cout << "OSQP Hessian:" << std::endl; - std::cout << " nzmax:" << osqp_data->P->nzmax << std::endl; - std::cout << " nz:" << osqp_data->P->nz << std::endl; - std::cout << " m:" << osqp_data->P->m << std::endl; - std::cout << " n:" << osqp_data->P->n << std::endl; - std::cout << " p:" << P_p_vec.transpose().format(format) << std::endl; - std::cout << " i:" << P_i_vec.transpose().format(format) << std::endl; - std::cout << " x:" << P_x_vec.transpose().format(format) << std::endl; + std::cout << "OSQP Hessian:" << '\n'; + std::cout << " nzmax:" << osqp_data->P->nzmax << '\n'; + std::cout << " nz:" << osqp_data->P->nz << '\n'; + std::cout << " m:" << osqp_data->P->m << '\n'; + std::cout << " n:" << osqp_data->P->n << '\n'; + std::cout << " p:" << P_p_vec.transpose().format(format) << '\n'; + std::cout << " i:" << P_i_vec.transpose().format(format) << '\n'; + std::cout << " x:" << P_x_vec.transpose().format(format) << '\n'; Eigen::Map q_vec(osqp_data->q, osqp_data->n); - std::cout << "OSQP Gradient: " << q_vec.transpose().format(format) << std::endl; + std::cout << "OSQP Gradient: " << q_vec.transpose().format(format) << '\n'; Eigen::Map> A_p_vec(osqp_data->A->p, osqp_data->A->n + 1); Eigen::Map> A_i_vec(osqp_data->A->i, osqp_data->A->nzmax); Eigen::Map> A_x_vec(osqp_data->A->x, osqp_data->A->nzmax); - std::cout << "OSQP Constraint Matrix:" << std::endl; - std::cout << " nzmax:" << osqp_data->A->nzmax << std::endl; - std::cout << " m:" << osqp_data->A->m << std::endl; - std::cout << " n:" << osqp_data->A->n << std::endl; - std::cout << " p:" << A_p_vec.transpose().format(format) << std::endl; - std::cout << " i:" << A_i_vec.transpose().format(format) << std::endl; - std::cout << " x:" << A_x_vec.transpose().format(format) << std::endl; + std::cout << "OSQP Constraint Matrix:" << '\n'; + std::cout << " nzmax:" << osqp_data->A->nzmax << '\n'; + std::cout << " m:" << osqp_data->A->m << '\n'; + std::cout << " n:" << osqp_data->A->n << '\n'; + std::cout << " p:" << A_p_vec.transpose().format(format) << '\n'; + std::cout << " i:" << A_i_vec.transpose().format(format) << '\n'; + std::cout << " x:" << A_x_vec.transpose().format(format) << '\n'; Eigen::Map> l_vec(osqp_data->l, osqp_data->m); Eigen::Map> u_vec(osqp_data->u, osqp_data->m); - std::cout << "OSQP Lower Bounds: " << l_vec.transpose().format(format) << std::endl; - std::cout << "OSQP Upper Bounds: " << u_vec.transpose().format(format) << std::endl; + std::cout << "OSQP Lower Bounds: " << l_vec.transpose().format(format) << '\n'; + std::cout << "OSQP Upper Bounds: " << u_vec.transpose().format(format) << '\n'; - std::cout << "OSQP Variable Names: " << std::endl; + std::cout << "OSQP Variable Names: " << '\n'; } /** @todo Need to check if this is what we want in the new version */ const bool solved = solver_->solve(); const auto status = static_cast(solver_->workspace()->info->status_val); if (OSQP_COMPARE_DEBUG_MODE) - std::cout << "OSQP Status Value: " << solver_->workspace()->info->status_val << std::endl; + std::cout << "OSQP Status Value: " << solver_->workspace()->info->status_val << '\n'; if (solved || status == OSQP_SOLVED_INACCURATE) { if (OSQP_COMPARE_DEBUG_MODE) { Eigen::IOFormat format(5); - std::cout << "OSQP Solution: " << solver_->getSolution().transpose().format(format) << std::endl; + std::cout << "OSQP Solution: " << solver_->getSolution().transpose().format(format) << '\n'; } return true; } @@ -140,18 +140,18 @@ bool OSQPEigenSolver::solve() if (solver_->workspace()->info->status_val == -3) { Eigen::Map primal_certificate(solver_->workspace()->delta_x, num_cnts_, 1); - std::cout << "OSQP Status: " << solver_->workspace()->info->status << std::endl; + std::cout << "OSQP Status: " << solver_->workspace()->info->status << '\n'; std::cout << "\n---------------------------------------\n"; - std::cout << std::scientific << "Primal Certificate (v): " << primal_certificate.transpose() << std::endl; + std::cout << std::scientific << "Primal Certificate (v): " << primal_certificate.transpose() << '\n'; double first = bounds_lower_.transpose() * primal_certificate; double second = bounds_upper_.transpose() * primal_certificate; std::cout << "A.transpose() * v = 0\n" - << "l.transpose() * v = " << first << " u.transpose() * v = " << second << std::endl; + << "l.transpose() * v = " << first << " u.transpose() * v = " << second << '\n'; std::cout << "l.transpose() * v + u.transpose() * v = " << first + second << " < 0\n"; - std::cout << "Bounds_lower: " << bounds_lower_.transpose() << std::endl; - std::cout << "Bounds_upper: " << bounds_upper_.transpose() << std::endl; + std::cout << "Bounds_lower: " << bounds_lower_.transpose() << '\n'; + std::cout << "Bounds_upper: " << bounds_upper_.transpose() << '\n'; std::cout << std::fixed; std::cout << "\n---------------------------------------\n"; } @@ -160,14 +160,14 @@ bool OSQPEigenSolver::solve() if (solver_->workspace()->info->status_val == -4) // NOLINT { Eigen::Map dual_certificate(solver_->workspace()->delta_y, num_vars_, 1); - std::cout << "OSQP Status: " << solver_->workspace()->info->status << std::endl; + std::cout << "OSQP Status: " << solver_->workspace()->info->status << '\n'; std::cout << "\n---------------------------------------\n"; - std::cout << "Dual Certificate (x): " << dual_certificate.transpose() << std::endl; // NOLINT + std::cout << "Dual Certificate (x): " << dual_certificate.transpose() << '\n'; // NOLINT // TODO: Find a way to get the hessian here // std::cout << "P*x = " << (qp_problem_->getHessian() * dual_certificate).transpose() << " = 0" << - // std::endl; - std::cout << "q.transpose() * x = " << gradient_.transpose() * dual_certificate << " < 0" << std::endl; + // '\n'; + std::cout << "q.transpose() * x = " << gradient_.transpose() * dual_certificate << " < 0" << '\n'; std::cout << std::fixed; std::cout << "\n---------------------------------------\n"; } diff --git a/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp b/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp index cae334d8..a4c5af13 100644 --- a/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp @@ -517,42 +517,42 @@ void TrajOptQPProblem::Implementation::print() const { Eigen::IOFormat format(3); - std::cout << "-------------- QPProblem::print() --------------" << std::endl; - std::cout << "Num NLP Vars: " << getNumNLPVars() << std::endl; - std::cout << "Num QP Vars: " << num_qp_vars_ << std::endl; - std::cout << "Num NLP Constraints: " << num_qp_cnts_ << std::endl; + std::cout << "-------------- QPProblem::print() --------------" << '\n'; + std::cout << "Num NLP Vars: " << getNumNLPVars() << '\n'; + std::cout << "Num QP Vars: " << num_qp_vars_ << '\n'; + std::cout << "Num NLP Constraints: " << num_qp_cnts_ << '\n'; std::cout << "Detected Constraint Type: "; for (const auto& cnt : constraint_types_) std::cout << static_cast(cnt) << ", "; - std::cout << std::endl; - std::cout << "Box Size: " << box_size_.transpose().format(format) << std::endl; // NOLINT - std::cout << "Constraint Merit Coeff: " << constraint_merit_coeff_.transpose().format(format) << std::endl; + std::cout << '\n'; + std::cout << "Box Size: " << box_size_.transpose().format(format) << '\n'; // NOLINT + std::cout << "Constraint Merit Coeff: " << constraint_merit_coeff_.transpose().format(format) << '\n'; - std::cout << "Hessian:\n" << hessian_.toDense().format(format) << std::endl; - std::cout << "Gradient: " << gradient_.transpose().format(format) << std::endl; - std::cout << "Constraint Matrix:\n" << constraint_matrix_.toDense().format(format) << std::endl; + std::cout << "Hessian:\n" << hessian_.toDense().format(format) << '\n'; + std::cout << "Gradient: " << gradient_.transpose().format(format) << '\n'; + std::cout << "Constraint Matrix:\n" << constraint_matrix_.toDense().format(format) << '\n'; std::cout << "Constraint Lower Bounds: " << bounds_lower_.head(getNumNLPConstraints() + hinge_constraints_.GetRows()).transpose().format(format) - << std::endl; + << '\n'; std::cout << "Constraint Upper Bounds: " << bounds_upper_.head(getNumNLPConstraints() + hinge_constraints_.GetRows()).transpose().format(format) - << std::endl; + << '\n'; std::cout << "Variable Lower Bounds: " << bounds_lower_.tail(bounds_lower_.rows() - getNumNLPConstraints() - hinge_constraints_.GetRows()) .transpose() .format(format) - << std::endl; + << '\n'; std::cout << "Variable Upper Bounds: " << bounds_upper_.tail(bounds_upper_.rows() - getNumNLPConstraints() - hinge_constraints_.GetRows()) .transpose() .format(format) - << std::endl; - std::cout << "All Lower Bounds: " << bounds_lower_.transpose().format(format) << std::endl; - std::cout << "All Upper Bounds: " << bounds_upper_.transpose().format(format) << std::endl; - std::cout << "NLP values: " << std::endl; + << '\n'; + std::cout << "All Lower Bounds: " << bounds_lower_.transpose().format(format) << '\n'; + std::cout << "All Upper Bounds: " << bounds_upper_.transpose().format(format) << '\n'; + std::cout << "NLP values: " << '\n'; for (const auto& v_set : variables_->GetComponents()) - std::cout << v_set->GetValues().transpose().format(format) << std::endl; + std::cout << v_set->GetValues().transpose().format(format) << '\n'; } Eigen::Index TrajOptQPProblem::Implementation::getNumNLPVars() const { return variables_->GetRows(); } diff --git a/trajopt_optimizers/trajopt_sqp/src/types.cpp b/trajopt_optimizers/trajopt_sqp/src/types.cpp index e28f6b20..462dde27 100644 --- a/trajopt_optimizers/trajopt_sqp/src/types.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/types.cpp @@ -51,39 +51,39 @@ SQPResults::SQPResults(Eigen::Index num_vars, Eigen::Index num_cnts, Eigen::Inde void SQPResults::print() const { Eigen::IOFormat format(3); - std::cout << "-------------- SQPResults::print() --------------" << std::endl; - std::cout << "best_exact_merit: " << best_exact_merit << std::endl; - std::cout << "new_exact_merit: " << new_exact_merit << std::endl; - std::cout << "best_approx_merit: " << best_approx_merit << std::endl; - std::cout << "new_approx_merit: " << new_approx_merit << std::endl; + std::cout << "-------------- SQPResults::print() --------------" << '\n'; + std::cout << "best_exact_merit: " << best_exact_merit << '\n'; + std::cout << "new_exact_merit: " << new_exact_merit << '\n'; + std::cout << "best_approx_merit: " << best_approx_merit << '\n'; + std::cout << "new_approx_merit: " << new_approx_merit << '\n'; // NOLINTNEXTLINE - std::cout << "best_var_vals: " << best_var_vals.transpose().format(format) << std::endl; - std::cout << "new_var_vals: " << new_var_vals.transpose().format(format) << std::endl; + std::cout << "best_var_vals: " << best_var_vals.transpose().format(format) << '\n'; + std::cout << "new_var_vals: " << new_var_vals.transpose().format(format) << '\n'; - std::cout << "approx_merit_improve: " << approx_merit_improve << std::endl; - std::cout << "exact_merit_improve: " << exact_merit_improve << std::endl; - std::cout << "merit_improve_ratio: " << merit_improve_ratio << std::endl; + std::cout << "approx_merit_improve: " << approx_merit_improve << '\n'; + std::cout << "exact_merit_improve: " << exact_merit_improve << '\n'; + std::cout << "merit_improve_ratio: " << merit_improve_ratio << '\n'; - std::cout << "box_size: " << box_size.transpose().format(format) << std::endl; - std::cout << "merit_error_coeffs: " << merit_error_coeffs.transpose().format(format) << std::endl; + std::cout << "box_size: " << box_size.transpose().format(format) << '\n'; + std::cout << "merit_error_coeffs: " << merit_error_coeffs.transpose().format(format) << '\n'; - std::cout << "best_constraint_violations: " << best_constraint_violations.transpose().format(format) << std::endl; - std::cout << "new_constraint_violations: " << new_constraint_violations.transpose().format(format) << std::endl; + std::cout << "best_constraint_violations: " << best_constraint_violations.transpose().format(format) << '\n'; + std::cout << "new_constraint_violations: " << new_constraint_violations.transpose().format(format) << '\n'; std::cout << "best_approx_constraint_violations: " << best_approx_constraint_violations.transpose().format(format) - << std::endl; + << '\n'; std::cout << "new_approx_constraint_violations: " << new_approx_constraint_violations.transpose().format(format) - << std::endl; + << '\n'; - std::cout << "best_costs: " << best_costs.transpose().format(format) << std::endl; - std::cout << "new_costs: " << new_costs.transpose().format(format) << std::endl; - std::cout << "best_approx_costs: " << best_approx_costs.transpose().format(format) << std::endl; - std::cout << "new_approx_costs: " << new_approx_costs.transpose().format(format) << std::endl; + std::cout << "best_costs: " << best_costs.transpose().format(format) << '\n'; + std::cout << "new_costs: " << new_costs.transpose().format(format) << '\n'; + std::cout << "best_approx_costs: " << best_approx_costs.transpose().format(format) << '\n'; + std::cout << "new_approx_costs: " << new_approx_costs.transpose().format(format) << '\n'; - std::cout << "penalty_iteration: " << penalty_iteration << std::endl; - std::cout << "convexify_iteration: " << convexify_iteration << std::endl; - std::cout << "trust_region_iteration: " << trust_region_iteration << std::endl; - std::cout << "overall_iteration: " << overall_iteration << std::endl; + std::cout << "penalty_iteration: " << penalty_iteration << '\n'; + std::cout << "convexify_iteration: " << convexify_iteration << '\n'; + std::cout << "trust_region_iteration: " << trust_region_iteration << '\n'; + std::cout << "overall_iteration: " << overall_iteration << '\n'; } } // namespace trajopt_sqp diff --git a/trajopt_sco/include/trajopt_sco/modeling_utils.hpp b/trajopt_sco/include/trajopt_sco/modeling_utils.hpp index 4e54d067..7add5d2a 100644 --- a/trajopt_sco/include/trajopt_sco/modeling_utils.hpp +++ b/trajopt_sco/include/trajopt_sco/modeling_utils.hpp @@ -12,7 +12,7 @@ using numerical derivatives or user-defined analytic derivatives. namespace sco { -enum PenaltyType +enum PenaltyType : std::uint8_t { SQUARED, ABS, diff --git a/trajopt_sco/include/trajopt_sco/optimizers.hpp b/trajopt_sco/include/trajopt_sco/optimizers.hpp index 0642a261..d8f8ef63 100644 --- a/trajopt_sco/include/trajopt_sco/optimizers.hpp +++ b/trajopt_sco/include/trajopt_sco/optimizers.hpp @@ -2,6 +2,7 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH #include +#include #include #include TRAJOPT_IGNORE_WARNINGS_POP @@ -14,7 +15,7 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace sco { -enum OptStatus +enum OptStatus : std::uint8_t { OPT_CONVERGED, OPT_SCO_ITERATION_LIMIT, // hit iteration limit before convergence diff --git a/trajopt_sco/include/trajopt_sco/sco_common.hpp b/trajopt_sco/include/trajopt_sco/sco_common.hpp index 6fcf62fb..9bfb574a 100644 --- a/trajopt_sco/include/trajopt_sco/sco_common.hpp +++ b/trajopt_sco/include/trajopt_sco/sco_common.hpp @@ -2,6 +2,7 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH #include +#include #include #include TRAJOPT_IGNORE_WARNINGS_POP diff --git a/trajopt_sco/include/trajopt_sco/solver_interface.hpp b/trajopt_sco/include/trajopt_sco/solver_interface.hpp index f1e8d59c..d594536e 100644 --- a/trajopt_sco/include/trajopt_sco/solver_interface.hpp +++ b/trajopt_sco/include/trajopt_sco/solver_interface.hpp @@ -3,6 +3,7 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH #include +#include #include #include #include @@ -14,7 +15,7 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace Json { class Value; -} +} // namespace Json /** @file solver_interface.hpp @@ -28,7 +29,7 @@ backends. namespace sco { -enum ConstraintType +enum ConstraintType : std::uint8_t { EQ, INEQ @@ -36,7 +37,7 @@ enum ConstraintType using ConstraintTypeVector = std::vector; -enum CvxOptStatus +enum CvxOptStatus : std::uint8_t { CVX_SOLVED, CVX_INFEASIBLE, @@ -225,7 +226,7 @@ std::ostream& operator<<(std::ostream&, const QuadExpr&); class ModelType { public: - enum Value + enum Value : std::uint8_t { GUROBI, OSQP, diff --git a/trajopt_sco/src/bpmpd_caller.cpp b/trajopt_sco/src/bpmpd_caller.cpp index 8a5d434f..89547f3a 100644 --- a/trajopt_sco/src/bpmpd_caller.cpp +++ b/trajopt_sco/src/bpmpd_caller.cpp @@ -40,7 +40,7 @@ int main(int /*argc*/, char** /*argv*/) if (err != 0) { std::cerr << "error going to BPMPD working dir\n"; - std::cerr << strerror(err) << std::endl; + std::cerr << strerror(err) << '\n'; abort(); } // int counter=0; @@ -56,7 +56,7 @@ int main(int /*argc*/, char** /*argv*/) bo.dual.resize(static_cast(bi.m) + static_cast(bi.n)); bo.status.resize(static_cast(bi.m) + static_cast(bi.n)); -#define DBG(expr) // cerr << #expr << ": " << CSTR(expr) << std::endl +#define DBG(expr) // cerr << #expr << ": " << CSTR(expr) << '\n' DBG(bi.m); DBG(bi.n); DBG(bi.nz); diff --git a/trajopt_sco/src/optimizers.cpp b/trajopt_sco/src/optimizers.cpp index 45d92c60..31f67115 100644 --- a/trajopt_sco/src/optimizers.cpp +++ b/trajopt_sco/src/optimizers.cpp @@ -20,12 +20,12 @@ const bool SUPER_DEBUG_MODE = false; std::ostream& operator<<(std::ostream& o, const OptResults& r) { - o << "Optimization results:" << std::endl - << "status: " << statusToString(r.status) << std::endl - << "cost values: " << trajopt_common::Str(r.cost_vals) << std::endl - << "constraint violations: " << trajopt_common::Str(r.cnt_viols) << std::endl - << "n func evals: " << r.n_func_evals << std::endl - << "n qp solves: " << r.n_qp_solves << std::endl; + o << "Optimization results:" << '\n' + << "status: " << statusToString(r.status) << '\n' + << "cost values: " << trajopt_common::Str(r.cost_vals) << '\n' + << "constraint violations: " << trajopt_common::Str(r.cnt_viols) << '\n' + << "n func evals: " << r.n_func_evals << '\n' + << "n qp solves: " << r.n_qp_solves << '\n'; return o; } diff --git a/trajopt_sco/src/osqp_interface.cpp b/trajopt_sco/src/osqp_interface.cpp index 87121dab..9701e254 100644 --- a/trajopt_sco/src/osqp_interface.cpp +++ b/trajopt_sco/src/osqp_interface.cpp @@ -283,46 +283,46 @@ CvxOptStatus OSQPModel::optimize() } catch (std::exception& e) { - std::cout << "OSQP Setup failed with error: " << e.what() << std::endl; + std::cout << "OSQP Setup failed with error: " << e.what() << '\n'; return CVX_FAILED; } if (OSQP_COMPARE_DEBUG_MODE) { Eigen::IOFormat format(5); - std::cout << "OSQP Number of Variables:" << osqp_data_.n << std::endl; - std::cout << "OSQP Number of Constraints:" << osqp_data_.m << std::endl; + std::cout << "OSQP Number of Variables:" << osqp_data_.n << '\n'; + std::cout << "OSQP Number of Constraints:" << osqp_data_.m << '\n'; Eigen::Map> P_p_vec(osqp_data_.P->p, osqp_data_.P->n + 1); Eigen::Map> P_i_vec(osqp_data_.P->i, osqp_data_.P->nzmax); Eigen::Map> P_x_vec(osqp_data_.P->x, osqp_data_.P->nzmax); - std::cout << "OSQP Hessian:" << std::endl; - std::cout << " nzmax:" << osqp_data_.P->nzmax << std::endl; - std::cout << " nz:" << osqp_data_.P->nz << std::endl; - std::cout << " m:" << osqp_data_.P->m << std::endl; - std::cout << " n:" << osqp_data_.P->n << std::endl; - std::cout << " p:" << P_p_vec.transpose().format(format) << std::endl; - std::cout << " i:" << P_i_vec.transpose().format(format) << std::endl; - std::cout << " x:" << P_x_vec.transpose().format(format) << std::endl; + std::cout << "OSQP Hessian:" << '\n'; + std::cout << " nzmax:" << osqp_data_.P->nzmax << '\n'; + std::cout << " nz:" << osqp_data_.P->nz << '\n'; + std::cout << " m:" << osqp_data_.P->m << '\n'; + std::cout << " n:" << osqp_data_.P->n << '\n'; + std::cout << " p:" << P_p_vec.transpose().format(format) << '\n'; + std::cout << " i:" << P_i_vec.transpose().format(format) << '\n'; + std::cout << " x:" << P_x_vec.transpose().format(format) << '\n'; Eigen::Map q_vec(osqp_data_.q, osqp_data_.n); - std::cout << "OSQP Gradient: " << q_vec.transpose().format(format) << std::endl; + std::cout << "OSQP Gradient: " << q_vec.transpose().format(format) << '\n'; Eigen::Map> A_p_vec(osqp_data_.A->p, osqp_data_.A->n + 1); Eigen::Map> A_i_vec(osqp_data_.A->i, osqp_data_.A->nzmax); Eigen::Map> A_x_vec(osqp_data_.A->x, osqp_data_.A->nzmax); - std::cout << "OSQP Constraint Matrix:" << std::endl; - std::cout << " nzmax:" << osqp_data_.A->nzmax << std::endl; - std::cout << " m:" << osqp_data_.A->m << std::endl; - std::cout << " n:" << osqp_data_.A->n << std::endl; - std::cout << " p:" << A_p_vec.transpose().format(format) << std::endl; - std::cout << " i:" << A_i_vec.transpose().format(format) << std::endl; - std::cout << " x:" << A_x_vec.transpose().format(format) << std::endl; + std::cout << "OSQP Constraint Matrix:" << '\n'; + std::cout << " nzmax:" << osqp_data_.A->nzmax << '\n'; + std::cout << " m:" << osqp_data_.A->m << '\n'; + std::cout << " n:" << osqp_data_.A->n << '\n'; + std::cout << " p:" << A_p_vec.transpose().format(format) << '\n'; + std::cout << " i:" << A_i_vec.transpose().format(format) << '\n'; + std::cout << " x:" << A_x_vec.transpose().format(format) << '\n'; Eigen::Map> l_vec(osqp_data_.l, osqp_data_.m); Eigen::Map> u_vec(osqp_data_.u, osqp_data_.m); - std::cout << "OSQP Lower Bounds: " << l_vec.transpose().format(format) << std::endl; - std::cout << "OSQP Upper Bounds: " << u_vec.transpose().format(format) << std::endl; + std::cout << "OSQP Lower Bounds: " << l_vec.transpose().format(format) << '\n'; + std::cout << "OSQP Upper Bounds: " << u_vec.transpose().format(format) << '\n'; std::vector vars_names(vars_.size()); for (const auto& var : vars_) @@ -331,7 +331,7 @@ CvxOptStatus OSQPModel::optimize() std::cout << "OSQP Variable Names: "; for (const auto& var_name : vars_names) std::cout << var_name << ","; - std::cout << std::endl; + std::cout << '\n'; } // Solve Problem @@ -347,8 +347,8 @@ CvxOptStatus OSQPModel::optimize() { Eigen::IOFormat format(5); Eigen::Map solution_vec(solution_.data(), static_cast(solution_.size())); - std::cout << "OSQP Status Value: " << status << std::endl; - std::cout << "OSQP Solution: " << solution_vec.transpose().format(format) << std::endl; + std::cout << "OSQP Status Value: " << status << '\n'; + std::cout << "OSQP Solution: " << solution_vec.transpose().format(format) << '\n'; } if (status == OSQP_SOLVED || status == OSQP_SOLVED_INACCURATE) diff --git a/trajopt_sco/src/solver_interface.cpp b/trajopt_sco/src/solver_interface.cpp index c37c13dd..51b6c8fa 100644 --- a/trajopt_sco/src/solver_interface.cpp +++ b/trajopt_sco/src/solver_interface.cpp @@ -216,7 +216,7 @@ std::ostream& operator<<(std::ostream& os, const ModelType& cs) if (cs_ivalue_ > ModelType::MODEL_NAMES_.size()) { std::stringstream conversion_error; - conversion_error << "Error converting ModelType to string - " << "enum value is " << cs_ivalue_ << std::endl; + conversion_error << "Error converting ModelType to string - " << "enum value is " << cs_ivalue_ << '\n'; throw std::runtime_error(conversion_error.str()); } os << ModelType::MODEL_NAMES_[cs_ivalue_]; @@ -301,7 +301,7 @@ Model::Ptr createModel(ModelType model_type, const ModelConfig::ConstPtr& model_ extern Model::Ptr createqpOASESModel(); #endif - char* solver_env = getenv("TRAJOPT_CONVEX_SOLVER"); + const char* solver_env = getenv("TRAJOPT_CONVEX_SOLVER"); ModelType solver = model_type; @@ -358,7 +358,7 @@ Model::Ptr createModel(ModelType model_type, const ModelConfig::ConstPtr& model_ return createqpOASESModel(); #endif std::stringstream solver_instatiation_error; - solver_instatiation_error << "Failed to create solver: unknown solver " << solver << std::endl; + solver_instatiation_error << "Failed to create solver: unknown solver " << solver << '\n'; PRINT_AND_THROW(solver_instatiation_error.str()); return {}; }