diff --git a/.clang-tidy b/.clang-tidy index 61600ae5..2d9e538d 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -6,6 +6,8 @@ Checks: > clang-analyzer-*, -clang-analyzer-cplusplus*, bugprone-*, + -bugprone-easily-swappable-parameters, + -bugprone-exception-escape, cppcoreguidelines-*, -cppcoreguidelines-macro-usage, -cppcoreguidelines-pro-type-static-cast-downcast, @@ -16,8 +18,10 @@ Checks: > -cppcoreguidelines-pro-bounds-constant-array-index, -cppcoreguidelines-avoid-magic-numbers, -cppcoreguidelines-non-private-member-variables-in-classes, + -cppcoreguidelines-owning-memory, misc-*, -misc-non-private-member-variables-in-classes, + -misc-no-recursion, modernize-*, -modernize-use-trailing-return-type, -modernize-use-nodiscard, @@ -27,7 +31,10 @@ Checks: > -readability-named-parameter, -readability-magic-numbers, -readability-isolate-declaration, - -readability-function-cognitive-complexity + -readability-function-cognitive-complexity, + -readability-use-anyofallof, + -readability-identifier-length, + -readability-suspicious-call-argument WarningsAsErrors: > -*, clang-diagnostic-*, @@ -35,6 +42,8 @@ WarningsAsErrors: > clang-analyzer-*, -clang-analyzer-cplusplus*, bugprone-*, + -bugprone-easily-swappable-parameters, + -bugprone-exception-escape, cppcoreguidelines-*, -cppcoreguidelines-macro-usage, -cppcoreguidelines-pro-type-static-cast-downcast, @@ -45,8 +54,10 @@ WarningsAsErrors: > -cppcoreguidelines-pro-bounds-constant-array-index, -cppcoreguidelines-avoid-magic-numbers, -cppcoreguidelines-non-private-member-variables-in-classes, + -cppcoreguidelines-owning-memory, misc-*, -misc-non-private-member-variables-in-classes, + -misc-no-recursion, modernize-*, -modernize-use-trailing-return-type, -modernize-use-nodiscard, @@ -57,6 +68,9 @@ WarningsAsErrors: > -readability-magic-numbers, -readability-isolate-declaration, -readability-function-cognitive-complexity + -readability-use-anyofallof, + -readability-identifier-length, + -readability-suspicious-call-argument HeaderFilterRegex: '.*' AnalyzeTemporaryDtors: false FormatStyle: none @@ -67,4 +81,6 @@ CheckOptions: value: '1' - key: cppcoreguidelines-special-member-functions.AllowSoleDefaultDtor value: '1' + - key: cppcoreguidelines-pro-type-member-init.IgnoreArrays + value: '1' ... diff --git a/trajopt_sco/CMakeLists.txt b/trajopt_sco/CMakeLists.txt index 97525fb6..bdc6416b 100644 --- a/trajopt_sco/CMakeLists.txt +++ b/trajopt_sco/CMakeLists.txt @@ -113,7 +113,7 @@ target_compile_options(${PROJECT_NAME} PRIVATE ${TRAJOPT_COMPILE_OPTIONS_PRIVATE target_compile_options(${PROJECT_NAME} PUBLIC ${TRAJOPT_COMPILE_OPTIONS_PUBLIC}) target_compile_definitions(${PROJECT_NAME} PUBLIC ${TRAJOPT_COMPILE_DEFINITIONS}) target_cxx_version(${PROJECT_NAME} PUBLIC VERSION ${TRAJOPT_CXX_VERSION}) -# target_clang_tidy(${PROJECT_NAME} ENABLE ${TRAJOPT_ENABLE_CLANG_TIDY}) +target_clang_tidy(${PROJECT_NAME} ENABLE ${TRAJOPT_ENABLE_CLANG_TIDY}) target_include_directories(${PROJECT_NAME} PUBLIC "$" "$") diff --git a/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp b/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp index 47b48eb7..8b8a03ce 100644 --- a/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp +++ b/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp @@ -67,7 +67,7 @@ void ser(int fp, std::vector& x, SerMode mode) struct bpmpd_input { - int m, n, nz, qn, qnz; + int m{ 0 }, n{ 0 }, nz{ 0 }, qn{ 0 }, qnz{ 0 }; std::vector acolcnt, acolidx; std::vector acolnzs; std::vector qcolcnt, qcolidx; @@ -114,7 +114,7 @@ const char CHECK_CHAR = 111; inline void ser(int fp, bpmpd_input& bi, SerMode mode) { - char scorrect = 'z', s = (mode == SER) ? scorrect : 0; + char scorrect = 'z', s = (mode == SER) ? scorrect : 0; // NOLINT ser(fp, s, mode); if (s == EXIT_CHAR) { @@ -142,8 +142,8 @@ struct bpmpd_output { std::vector primal, dual; std::vector status; - int code; - double opt; + int code{ 0 }; + double opt{ 0 }; bpmpd_output() = default; bpmpd_output(std::vector primal, std::vector dual, std::vector status, int code, double opt) : primal(std::move(primal)), dual(std::move(dual)), status(std::move(status)), code(code), opt(opt) @@ -153,7 +153,7 @@ struct bpmpd_output inline void ser(int fp, bpmpd_output& bo, SerMode mode) { - char scorrect = CHECK_CHAR, s = (mode == SER) ? scorrect : 0; + char scorrect = CHECK_CHAR, s = (mode == SER) ? scorrect : 0; // NOLINT ser(fp, s, mode); if (s == EXIT_CHAR) { diff --git a/trajopt_sco/include/trajopt_sco/modeling.hpp b/trajopt_sco/include/trajopt_sco/modeling.hpp index 65484ec2..e79fbab9 100644 --- a/trajopt_sco/include/trajopt_sco/modeling.hpp +++ b/trajopt_sco/include/trajopt_sco/modeling.hpp @@ -45,10 +45,10 @@ class ConvexObjective void addL2Norm(const AffExprVector&); void addMax(const AffExprVector&); - bool inModel() { return model_ != nullptr; } + bool inModel() const { return model_ != nullptr; } void addConstraintsToModel(); void removeFromModel(); - double value(const DblVec& x); + double value(const DblVec& x) const; Model* model_; /// The objective Function diff --git a/trajopt_sco/include/trajopt_sco/optimizers.hpp b/trajopt_sco/include/trajopt_sco/optimizers.hpp index cfaefc82..22f4d289 100644 --- a/trajopt_sco/include/trajopt_sco/optimizers.hpp +++ b/trajopt_sco/include/trajopt_sco/optimizers.hpp @@ -3,6 +3,7 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include +#include TRAJOPT_IGNORE_WARNINGS_POP #include @@ -22,24 +23,25 @@ enum OptStatus OPT_FAILED, INVALID }; -static const char* OptStatus_strings[] = { "CONVERGED", - "SCO_ITERATION_LIMIT", - "PENALTY_ITERATION_LIMIT", - "FAILED", - "INVALID" }; +static const std::array OptStatus_strings = { "CONVERGED", + "SCO_ITERATION_LIMIT", + "PENALTY_ITERATION_LIMIT", + "FAILED", + "INVALID" }; inline std::string statusToString(OptStatus status) { return OptStatus_strings[status]; } struct OptResults { DblVec x; // solution estimate - OptStatus status; - double total_cost; + OptStatus status{ INVALID }; + double total_cost{ 0 }; DblVec cost_vals; DblVec cnt_viols; - int n_func_evals, n_qp_solves; + int n_func_evals{ 0 }, n_qp_solves{ 0 }; void clear() { x.clear(); status = INVALID; + total_cost = 0; cost_vals.clear(); cnt_viols.clear(); n_func_evals = 0; diff --git a/trajopt_sco/include/trajopt_sco/osqp_interface.hpp b/trajopt_sco/include/trajopt_sco/osqp_interface.hpp index 1beb8625..d16057b0 100644 --- a/trajopt_sco/include/trajopt_sco/osqp_interface.hpp +++ b/trajopt_sco/include/trajopt_sco/osqp_interface.hpp @@ -17,7 +17,7 @@ struct OSQPModelConfig : public ModelConfig OSQPModelConfig(); - OSQPSettings settings; + OSQPSettings settings{}; }; /** @@ -35,7 +35,7 @@ class OSQPModel : public Model { /** OSQPData. Some fields here (`osp_data_.A` and `osp_data_.P`) are * automatically allocated by OSQP, but * deallocated by us. */ - OSQPData osqp_data_; + OSQPData osqp_data_{}; /** OSQP Workspace. Memory here is managed by OSQP */ OSQPWorkspace* osqp_workspace_{ nullptr }; diff --git a/trajopt_sco/include/trajopt_sco/qpoases_interface.hpp b/trajopt_sco/include/trajopt_sco/qpoases_interface.hpp index fe881d00..55138baa 100644 --- a/trajopt_sco/include/trajopt_sco/qpoases_interface.hpp +++ b/trajopt_sco/include/trajopt_sco/qpoases_interface.hpp @@ -73,7 +73,11 @@ class qpOASESModel : public Model public: qpOASESModel(); - virtual ~qpOASESModel(); + ~qpOASESModel() override; + qpOASESModel(const qpOASESModel&) = default; + qpOASESModel& operator=(const qpOASESModel&) = default; + qpOASESModel(qpOASESModel&&) = default; + qpOASESModel& operator=(qpOASESModel&&) = default; Var addVar(const std::string& name) override; Cnt addEqCnt(const AffExpr&, const std::string& name) override; @@ -85,10 +89,10 @@ class qpOASESModel : public Model void update() override; void setVarBounds(const VarVector& vars, const DblVec& lower, const DblVec& upper) override; DblVec getVarValues(const VarVector& vars) const override; - virtual CvxOptStatus optimize() override; - virtual void setObjective(const AffExpr&) override; - virtual void setObjective(const QuadExpr&) override; - virtual void writeToFile(const std::string& fname) const override; - virtual VarVector getVars() const override; + CvxOptStatus optimize() override; + void setObjective(const AffExpr&) override; + void setObjective(const QuadExpr&) override; + void writeToFile(const std::string& fname) const override; + VarVector getVars() const override; }; } // namespace sco diff --git a/trajopt_sco/include/trajopt_sco/solver_interface.hpp b/trajopt_sco/include/trajopt_sco/solver_interface.hpp index 40c6ba54..546b7f04 100644 --- a/trajopt_sco/include/trajopt_sco/solver_interface.hpp +++ b/trajopt_sco/include/trajopt_sco/solver_interface.hpp @@ -136,7 +136,7 @@ struct CntRep { using Ptr = std::shared_ptr; - CntRep(std::size_t _index, void* _creator) : index(_index), removed(false), creator(_creator) {} + CntRep(std::size_t _index, void* _creator) : index(_index), creator(_creator) {} CntRep(const CntRep&) = default; CntRep& operator=(const CntRep&) = default; CntRep(CntRep&&) = default; @@ -144,9 +144,9 @@ struct CntRep ~CntRep() = default; std::size_t index; - bool removed; + bool removed{ false }; void* creator; - ConstraintType type; + ConstraintType type{ ConstraintType::EQ }; std::string expr; // todo placeholder }; @@ -181,7 +181,7 @@ struct AffExpr AffExpr& operator=(AffExpr&&) = default; explicit AffExpr(double a) : constant(a) {} - explicit AffExpr(const Var& v) : constant(0), coeffs(1, 1), vars(1, v) {} + explicit AffExpr(const Var& v) : coeffs(1, 1), vars(1, v) {} size_t size() const { return coeffs.size(); } double value(const double* x) const; diff --git a/trajopt_sco/include/trajopt_sco/solver_utils.hpp b/trajopt_sco/include/trajopt_sco/solver_utils.hpp index 52d6c459..72e56d95 100644 --- a/trajopt_sco/include/trajopt_sco/solver_utils.hpp +++ b/trajopt_sco/include/trajopt_sco/solver_utils.hpp @@ -64,7 +64,7 @@ void exprToEigen(const QuadExpr& expr, void exprToEigen(const AffExprVector& expr_vec, Eigen::SparseMatrix& sparse_matrix, Eigen::VectorXd& vector, - const Eigen::Index& n_vars = -1); + Eigen::Index n_vars = -1); /** * @brief Converts triplets to an `Eigen::SparseMatrix`. * @param [in] rows_i a vector of row indices @@ -121,7 +121,7 @@ void eigenToCSC(Eigen::SparseMatrix& sparse_matrix, if (sizeof(T) != sizeof(int)) { IntVec row_indices_int, column_pointers_int; - auto si_p = sm_ref.get().innerIndexPtr(); + auto* si_p = sm_ref.get().innerIndexPtr(); row_indices_int.assign(si_p, si_p + sm_ref.get().nonZeros()); row_indices.clear(); row_indices.reserve(row_indices_int.size()); @@ -137,7 +137,7 @@ void eigenToCSC(Eigen::SparseMatrix& sparse_matrix, } else { - auto si_p = sm_ref.get().innerIndexPtr(); + auto* si_p = sm_ref.get().innerIndexPtr(); row_indices.assign(si_p, si_p + sm_ref.get().nonZeros()); si_p = sm_ref.get().outerIndexPtr(); @@ -148,7 +148,7 @@ void eigenToCSC(Eigen::SparseMatrix& sparse_matrix, // pointers ends with the number of non-zero elements column_pointers.push_back(static_cast(sm_ref.get().nonZeros())); - auto csc_v = sm_ref.get().valuePtr(); + auto* csc_v = sm_ref.get().valuePtr(); values.assign(csc_v, csc_v + sm_ref.get().nonZeros()); } } // namespace sco diff --git a/trajopt_sco/src/bpmpd_interface.cpp b/trajopt_sco/src/bpmpd_interface.cpp index d82acb0e..c5c35d00 100644 --- a/trajopt_sco/src/bpmpd_interface.cpp +++ b/trajopt_sco/src/bpmpd_interface.cpp @@ -150,7 +150,7 @@ logfile it generates. namespace sco { -static double BPMPD_BIG = 1e+30; +static const double BPMPD_BIG = 1e+30; Model::Ptr createBPMPDModel() { @@ -163,10 +163,10 @@ Model::Ptr createBPMPDModel() pid_t popen2(const char* command, int* infp, int* outfp) { - int p_stdin[2], p_stdout[2]; - pid_t pid; + std::array p_stdin{}, p_stdout{}; + pid_t pid{ 0 }; - if (pipe(p_stdin) != 0 || pipe(p_stdout) != 0) + if (pipe(p_stdin.data()) != 0 || pipe(p_stdout.data()) != 0) return -1; pid = fork(); @@ -202,14 +202,14 @@ pid_t popen2(const char* command, int* infp, int* outfp) return pid; } -static pid_t gPID = 0; -static int gPipeIn = 0; -static int gPipeOut = 0; +static pid_t gPID = 0; // NOLINT +static int gPipeIn = 0; // NOLINT +static int gPipeOut = 0; // NOLINT void fexit() { - char text[1] = { bpmpd_io::EXIT_CHAR }; - long n = write(gPipeIn, text, 1); + std::array text{ bpmpd_io::EXIT_CHAR }; + long n = write(gPipeIn, text.data(), 1); ALWAYS_ASSERT(n == 1); } @@ -271,7 +271,7 @@ void BPMPDModel::removeCnts(const CntVector& cnts) { SizeTVec inds; cnts2inds(cnts, inds); - for (auto& cnt : cnts) + for (auto& cnt : cnts) // NOLINT cnt.cnt_rep->removed = true; } @@ -438,8 +438,8 @@ CvxOptStatus BPMPDModel::optimize() } #define VECINC(vec) \ - for (unsigned i = 0; i < (vec).size(); ++i) \ - ++(vec)[i]; + for (auto& v : (vec)) \ + ++v; VECINC(acolidx); VECINC(qcolidx); #undef VECINC diff --git a/trajopt_sco/src/modeling.cpp b/trajopt_sco/src/modeling.cpp index d7cdedfc..a0b6075d 100644 --- a/trajopt_sco/src/modeling.cpp +++ b/trajopt_sco/src/modeling.cpp @@ -19,7 +19,7 @@ void ConvexObjective::addAffExpr(const AffExpr& affexpr) { exprInc(quad_, affexp void ConvexObjective::addQuadExpr(const QuadExpr& quadexpr) { exprInc(quad_, quadexpr); } void ConvexObjective::addHinge(const AffExpr& affexpr, double coeff) { - Var hinge = model_->addVar("hinge", 0, INFINITY); + Var hinge = model_->addVar("hinge", 0, static_cast(INFINITY)); vars_.push_back(hinge); ineqs_.push_back(affexpr); exprDec(ineqs_.back(), hinge); @@ -30,8 +30,8 @@ void ConvexObjective::addHinge(const AffExpr& affexpr, double coeff) void ConvexObjective::addAbs(const AffExpr& affexpr, double coeff) { // Add variables that will enforce ABS - Var neg = model_->addVar("neg", 0, INFINITY); - Var pos = model_->addVar("pos", 0, INFINITY); + Var neg = model_->addVar("neg", 0, static_cast(INFINITY)); + Var pos = model_->addVar("pos", 0, static_cast(INFINITY)); vars_.push_back(neg); vars_.push_back(pos); // Coeff will be applied whenever neg/pos are not 0 @@ -76,7 +76,7 @@ void ConvexObjective::addL2Norm(const AffExprVector& ev) void ConvexObjective::addMax(const AffExprVector& ev) { - Var m = model_->addVar("max", -INFINITY, INFINITY); + Var m = model_->addVar("max", static_cast(-INFINITY), static_cast(INFINITY)); ineqs_.reserve(ineqs_.size() + ev.size()); for (const auto& i : ev) { @@ -148,7 +148,7 @@ ConvexConstraints::~ConvexConstraints() removeFromModel(); } -double ConvexObjective::value(const DblVec& x) { return quad_.value(x); } +double ConvexObjective::value(const DblVec& x) const { return quad_.value(x); } DblVec Constraint::violations(const DblVec& x) { DblVec val = value(x); @@ -175,7 +175,8 @@ OptProb::OptProb(ModelType convex_solver, const ModelConfig::ConstPtr& convex_so } VarVector OptProb::createVariables(const std::vector& names) { - return createVariables(names, DblVec(names.size(), -INFINITY), DblVec(names.size(), INFINITY)); + return createVariables( + names, DblVec(names.size(), static_cast(-INFINITY)), DblVec(names.size(), static_cast(INFINITY))); } VarVector OptProb::createVariables(const std::vector& names, const DblVec& lb, const DblVec& ub) diff --git a/trajopt_sco/src/modeling_utils.cpp b/trajopt_sco/src/modeling_utils.cpp index ceefe4dd..7a06fc66 100644 --- a/trajopt_sco/src/modeling_utils.cpp +++ b/trajopt_sco/src/modeling_utils.cpp @@ -57,7 +57,7 @@ ConvexObjective::Ptr CostFromFunc::convex(const DblVec& x, Model* model) auto out = std::make_shared(model); if (!full_hessian_) { - double val; + double val{ NAN }; Eigen::VectorXd grad, hess; calcGradAndDiagHess(*f_, x_eigen, epsilon_, val, grad, hess); hess = hess.cwiseMax(Eigen::VectorXd::Zero(hess.size())); @@ -71,7 +71,7 @@ ConvexObjective::Ptr CostFromFunc::convex(const DblVec& x, Model* model) } else { - double val; + double val{ NAN }; Eigen::VectorXd grad; Eigen::MatrixXd hess; calcGradHess(f_, x_eigen, epsilon_, val, grad, hess); @@ -81,8 +81,8 @@ ConvexObjective::Ptr CostFromFunc::convex(const DblVec& x, Model* model) Eigen::VectorXd eigvals = es.eigenvalues(); Eigen::MatrixXd eigvecs = es.eigenvectors(); for (long int i = 0, end = x_eigen.size(); i != end; ++i) - { // tricky --- eigen size() is signed - if (eigvals(i) > 0) + { // tricky --- eigen size() is signed + if (eigvals(i) > 0) // NOLINT pos_hess += eigvals(i) * eigvecs.col(i) * eigvecs.col(i).transpose(); } diff --git a/trajopt_sco/src/optimizers.cpp b/trajopt_sco/src/optimizers.cpp index d8a2b117..f5b0d1f3 100644 --- a/trajopt_sco/src/optimizers.cpp +++ b/trajopt_sco/src/optimizers.cpp @@ -518,30 +518,30 @@ void BasicTrustRegionSQPResults::writeConstraints(std::FILE* stream, bool header void BasicTrustRegionSQPResults::printRaw() const { std::cout << "\nmodel_var_vals:"; - for (auto& i : model_var_vals) + for (const auto& i : model_var_vals) std::cout << i << ", "; std::cout << "\nmodel_cost_vals: "; - for (auto& i : model_cost_vals) + for (const auto& i : model_cost_vals) std::cout << i << ", "; std::cout << "\nmodel_cnt_viols: "; - for (auto& i : model_cnt_viols) + for (const auto& i : model_cnt_viols) std::cout << i << ", "; std::cout << "\nnew_x: "; - for (auto& i : new_x) + for (const auto& i : new_x) std::cout << i << ", "; std::cout << "\nnew_cost_vals: "; - for (auto& i : new_cost_vals) + for (const auto& i : new_cost_vals) std::cout << i << ", "; std::cout << "\nold_cost_vals: "; - for (auto& i : old_cost_vals) + for (const auto& i : old_cost_vals) std::cout << i << ", "; std::cout << "\nnew_cnt_viols: "; - for (auto& i : new_cnt_viols) + for (const auto& i : new_cnt_viols) std::cout << i << ", "; std::cout << "\nold_cnt_viols: "; - for (auto& i : old_cnt_viols) + for (const auto& i : old_cnt_viols) std::cout << i << ", "; std::cout << "\nold_merit: " << old_merit << " \n"; @@ -552,16 +552,16 @@ void BasicTrustRegionSQPResults::printRaw() const std::cout << "merit_improve_ratio: " << merit_improve_ratio << " \n"; std::cout << "merit_error_coeffs: "; - for (auto& i : merit_error_coeffs) + for (const auto& i : merit_error_coeffs) std::cout << i << ", "; std::cout << "\nvar_names: "; - for (auto& i : var_names) + for (const auto& i : var_names) std::cout << i << ", "; std::cout << "\ncost_names: "; - for (auto& i : cost_names) + for (const auto& i : cost_names) std::cout << i << ", "; std::cout << "\ncnt_names: "; - for (auto& i : cnt_names) + for (const auto& i : cnt_names) std::cout << i << ", "; } diff --git a/trajopt_sco/src/osqp_interface.cpp b/trajopt_sco/src/osqp_interface.cpp index cbecc03b..f2e161ce 100644 --- a/trajopt_sco/src/osqp_interface.cpp +++ b/trajopt_sco/src/osqp_interface.cpp @@ -27,8 +27,8 @@ OSQPModelConfig::OSQPModelConfig() settings.eps_rel = 1e-6; settings.max_iter = 8192; settings.polish = 1; - settings.adaptive_rho = true; - settings.verbose = SUPER_DEBUG_MODE; + settings.adaptive_rho = 1; + settings.verbose = static_cast(SUPER_DEBUG_MODE); } Model::Ptr createOSQPModel(const ModelConfig::ConstPtr& config = nullptr) @@ -40,7 +40,9 @@ OSQPModel::OSQPModel(const ModelConfig::ConstPtr& config) : P_(nullptr), A_(null { // tuning parameters to be less accurate, but add a polishing step if (config != nullptr) + { config_.settings = std::dynamic_pointer_cast(config)->settings; + } } OSQPModel::~OSQPModel() @@ -54,7 +56,8 @@ OSQPModel::~OSQPModel() var.var_rep->removed = true; for (Cnt& cnt : cnts_) cnt.cnt_rep->removed = true; - update(); + + OSQPModel::update(); } Var OSQPModel::addVar(const std::string& name) @@ -87,7 +90,7 @@ void OSQPModel::removeVars(const VarVector& vars) { SizeTVec inds; vars2inds(vars, inds); - for (auto& var : vars) + for (const auto& var : vars) var.var_rep->removed = true; } @@ -95,7 +98,7 @@ void OSQPModel::removeCnts(const CntVector& cnts) { SizeTVec inds; cnts2inds(cnts, inds); - for (auto& cnt : cnts) + for (const auto& cnt : cnts) cnt.cnt_rep->removed = true; } @@ -135,15 +138,16 @@ void OSQPModel::updateConstraints() { const size_t n = vars_.size(); const size_t m = cnts_.size(); - const auto n_int = static_cast(n); - const auto m_int = static_cast(m); + const auto n_int = static_cast(n); + const auto m_int = static_cast(m); osqp_data_.m = static_cast(m) + static_cast(n); Eigen::SparseMatrix sm; Eigen::VectorXd v; - exprToEigen(cnt_exprs_, sm, v, static_cast(n)); - sm.conservativeResize(m_int + n_int, Eigen::NoChange_t(n)); + exprToEigen(cnt_exprs_, sm, v, n_int); + sm.conservativeResize(m_int + n_int, + Eigen::NoChange_t(n_int)); // NOLINT(clang-analyzer-core.UndefinedBinaryOperatorResult) l_.clear(); l_.resize(m + n, -OSQP_INFINITY); @@ -202,7 +206,7 @@ void OSQPModel::updateConstraints() void OSQPModel::createOrUpdateSolver() { updateObjective(); - updateConstraints(); + updateConstraints(); // NOLINT(clang-analyzer-core.UndefinedBinaryOperatorResult) // TODO atm we are not updating the workspace, but recreating it each time. // In the future, we will checking sparsity did not change and update instead @@ -211,7 +215,7 @@ void OSQPModel::createOrUpdateSolver() // Setup workspace - this should be called only once auto ret = osqp_setup(&osqp_workspace_, &osqp_data_, &config_.settings); - if (ret) + if (ret != 0) { // In this case, no data got allocated, so don't try to free it. if (ret == OSQP_DATA_VALIDATION_ERROR || ret == OSQP_SETTINGS_VALIDATION_ERROR) @@ -293,7 +297,7 @@ CvxOptStatus OSQPModel::optimize() update(); try { - createOrUpdateSolver(); + createOrUpdateSolver(); // NOLINT(clang-analyzer-core.UndefinedBinaryOperatorResult) } catch (std::exception& e) { diff --git a/trajopt_sco/src/qpoases_interface.cpp b/trajopt_sco/src/qpoases_interface.cpp index de9a4827..c202e0bd 100644 --- a/trajopt_sco/src/qpoases_interface.cpp +++ b/trajopt_sco/src/qpoases_interface.cpp @@ -3,7 +3,7 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include #include -#include +#include TRAJOPT_IGNORE_WARNINGS_POP #include @@ -15,7 +15,7 @@ using namespace qpOASES; namespace sco { -double QPOASES_INFTY = qpOASES::INFTY; +static const double QPOASES_INFTY = qpOASES::INFTY; Model::Ptr createqpOASESModel() { @@ -35,7 +35,7 @@ qpOASESModel::qpOASESModel() qpoases_options_.ensureConsistency(); } -qpOASESModel::~qpOASESModel() {} +qpOASESModel::~qpOASESModel() = default; Var qpOASESModel::addVar(const std::string& name) { vars_.push_back(std::make_shared(vars_.size(), name, this)); @@ -70,28 +70,31 @@ void qpOASESModel::removeVars(const VarVector& vars) { IntVec inds; vars2inds(vars, inds); - for (size_t i = 0; i < vars.size(); ++i) - vars[i].var_rep->removed = true; + for (const auto& var : vars) + var.var_rep->removed = true; } void qpOASESModel::removeCnts(const CntVector& cnts) { IntVec inds; cnts2inds(cnts, inds); - for (size_t i = 0; i < cnts.size(); ++i) - cnts[i].cnt_rep->removed = true; + for (const auto& cnt : cnts) + cnt.cnt_rep->removed = true; } void qpOASESModel::updateObjective() { - const Eigen::Index n = Eigen::Index(vars_.size()); + const auto n = static_cast(vars_.size()); Eigen::SparseMatrix sm; exprToEigen(objective_, sm, g_, n, true, true); eigenToCSC(sm, H_row_indices_, H_column_pointers_, H_csc_data_); - H_ = SymSparseMat( - (int)vars_.size(), (int)vars_.size(), H_row_indices_.data(), H_column_pointers_.data(), H_csc_data_.data()); + H_ = SymSparseMat(static_cast(vars_.size()), + static_cast(vars_.size()), + H_row_indices_.data(), + H_column_pointers_.data(), + H_csc_data_.data()); H_.createDiagInfo(); } @@ -107,17 +110,20 @@ void qpOASESModel::updateConstraints() Eigen::SparseMatrix sm; Eigen::VectorXd v; - exprToEigen(cnt_exprs_, sm, v, Eigen::Index(n)); + exprToEigen(cnt_exprs_, sm, v, static_cast(n)); for (size_t i_cnt = 0; i_cnt < m; ++i_cnt) { - lbA_[i_cnt] = (cnt_types_[i_cnt] == INEQ) ? -QPOASES_INFTY : v[Eigen::Index(i_cnt)]; - ubA_[i_cnt] = v[Eigen::Index(i_cnt)]; + lbA_[i_cnt] = (cnt_types_[i_cnt] == INEQ) ? -QPOASES_INFTY : v[static_cast(i_cnt)]; + ubA_[i_cnt] = v[static_cast(i_cnt)]; } eigenToCSC(sm, A_row_indices_, A_column_pointers_, A_csc_data_); - A_ = SparseMatrix( - (int)cnts_.size(), (int)vars_.size(), A_row_indices_.data(), A_column_pointers_.data(), A_csc_data_.data()); + A_ = SparseMatrix(static_cast(cnts_.size()), + static_cast(vars_.size()), + A_row_indices_.data(), + A_column_pointers_.data(), + A_csc_data_.data()); } bool qpOASESModel::updateSolver() @@ -217,7 +223,7 @@ CvxOptStatus qpOASESModel::optimize() // Solve Problem int nWSR = 255; - if (qpoases_problem_->isInitialised()) + if (qpoases_problem_->isInitialised() == qpOASES::BT_TRUE) { val = qpoases_problem_->hotstart( &H_, g_.data(), &A_, lb_.data(), ub_.data(), lbA_.data(), ubA_.data(), nWSR, nullptr); @@ -237,23 +243,20 @@ CvxOptStatus qpOASESModel::optimize() { // opt += m_objective.affexpr.constant; solution_.resize(vars_.size(), 0.); - val = qpoases_problem_->getPrimalSolution(solution_.data()); + // val = qpoases_problem_->getPrimalSolution(solution_.data()); + qpoases_problem_->getPrimalSolution(solution_.data()); return CVX_SOLVED; } - else if (val == qpOASES::RET_INIT_FAILED_INFEASIBILITY) + + if (val == qpOASES::RET_INIT_FAILED_INFEASIBILITY) { return CVX_INFEASIBLE; } - else - { - return CVX_FAILED; - } + + return CVX_FAILED; } void qpOASESModel::setObjective(const AffExpr& expr) { objective_.affexpr = expr; } void qpOASESModel::setObjective(const QuadExpr& expr) { objective_ = expr; } -void qpOASESModel::writeToFile(const std::string& /*fname*/) const -{ - return; // NOT IMPLEMENTED -} +void qpOASESModel::writeToFile(const std::string& /*fname*/) const {} VarVector qpOASESModel::getVars() const { return vars_; } } // namespace sco diff --git a/trajopt_sco/src/solver_interface.cpp b/trajopt_sco/src/solver_interface.cpp index f6b534ce..aae55ca8 100644 --- a/trajopt_sco/src/solver_interface.cpp +++ b/trajopt_sco/src/solver_interface.cpp @@ -298,7 +298,7 @@ Model::Ptr createModel(ModelType model_type, const ModelConfig::ConstPtr& model_ if (solver == ModelType::AUTO_SOLVER) { - if (solver_env && std::string(solver_env) != "AUTO_SOLVER") + if (solver_env != nullptr && std::string(solver_env) != "AUTO_SOLVER") { try { diff --git a/trajopt_sco/src/solver_utils.cpp b/trajopt_sco/src/solver_utils.cpp index 55bfc818..4abd1fbf 100644 --- a/trajopt_sco/src/solver_utils.cpp +++ b/trajopt_sco/src/solver_utils.cpp @@ -39,6 +39,7 @@ void exprToEigen(const QuadExpr& expr, vars2inds(expr.vars1, ind1); SizeTVec ind2; vars2inds(expr.vars2, ind2); + assert((ind2.size() == ind1.size()) && (ind1.size() == expr.coeffs.size())); // NOLINT sparse_matrix.resize(n_vars, n_vars); sparse_matrix.reserve(static_cast(2 * expr.size())); @@ -50,12 +51,15 @@ void exprToEigen(const QuadExpr& expr, { if (expr.coeffs[i] != 0.0) { + // NOLINTNEXTLINE if (ind1[i] == ind2[i]) + { sparse_matrix.coeffRef(static_cast(ind1[i]), static_cast(ind2[i])) += expr.coeffs[i]; + } else { - Eigen::Index c, r; + Eigen::Index c{ 0 }, r{ 0 }; if (ind1[i] < ind2[i]) { r = static_cast(ind1[i]); @@ -85,7 +89,7 @@ void exprToEigen(const QuadExpr& expr, void exprToEigen(const AffExprVector& expr_vec, Eigen::SparseMatrix& sparse_matrix, Eigen::VectorXd& vector, - const Eigen::Index& n_vars) + Eigen::Index n_vars) { vector.resize(static_cast(expr_vec.size())); vector.setZero(); @@ -123,13 +127,19 @@ void tripletsToEigen(const IntVec& rows_i, const DblVec& values_ij, Eigen::SparseMatrix& sparse_matrix) { + assert((rows_i.size() == cols_j.size()) && (rows_i.size() == values_ij.size())); // NOLINT + sparse_matrix.reserve(static_cast(values_ij.size())); + using T = Eigen::Triplet; - std::vector> triplets; + thread_local std::vector> triplets; + triplets.clear(); + triplets.reserve(values_ij.size()); + + // NOLINTNEXTLINE for (unsigned int i = 0; i < values_ij.size(); ++i) - { triplets.emplace_back(rows_i[i], cols_j[i], values_ij[i]); - } - sparse_matrix.setFromTriplets(triplets.begin(), triplets.end()); // NOLINT + + sparse_matrix.setFromTriplets(triplets.begin(), triplets.end()); } void eigenToTriplets(const Eigen::SparseMatrix& sparse_matrix, @@ -137,7 +147,7 @@ void eigenToTriplets(const Eigen::SparseMatrix& sparse_matrix, IntVec& cols_j, DblVec& values_ij) { - auto& sm = sparse_matrix; + const auto& sm = sparse_matrix; rows_i.reserve(rows_i.size() + static_cast(sm.nonZeros())); cols_j.reserve(cols_j.size() + static_cast(sm.nonZeros())); values_ij.reserve(values_ij.size() + static_cast(sm.nonZeros()));