From 91c720834ca9d888dcc09ee05c26f5ffd8c37950 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Mon, 10 Jun 2024 10:24:52 +0200 Subject: [PATCH] Various include cleanups and clang-tidy fixes, and an error message fix. --- trajopt/src/collision_terms.cpp | 10 +++++----- trajopt/src/problem_description.cpp | 2 +- trajopt_common/include/trajopt_common/cache.h | 1 - trajopt_common/include/trajopt_common/config.hpp | 1 - .../include/trajopt_common/stl_to_string.hpp | 1 - trajopt_common/src/logging.cpp | 1 - trajopt_sco/src/bpmpd_interface.cpp | 7 +++---- trajopt_sco/src/modeling.cpp | 1 - trajopt_sco/src/modeling_utils.cpp | 1 - trajopt_sco/src/osqp_interface.cpp | 7 +++---- trajopt_sco/src/qpoases_interface.cpp | 7 +++---- 11 files changed, 15 insertions(+), 24 deletions(-) diff --git a/trajopt/src/collision_terms.cpp b/trajopt/src/collision_terms.cpp index 8f372cfb..28853f00 100644 --- a/trajopt/src/collision_terms.cpp +++ b/trajopt/src/collision_terms.cpp @@ -435,7 +435,7 @@ void CollisionEvaluator::CollisionsToDistanceExpressionsW(sco::AffExprVector& ex exprs_data.push_back(data); if (!found[0] && !found[1]) { - exprs.push_back(sco::AffExpr(0)); + exprs.emplace_back(0); } else { @@ -529,7 +529,7 @@ void CollisionEvaluator::CollisionsToDistanceExpressionsContinuousW( exprs_data.push_back(data); if (!found[0] && !found[1]) { - exprs.push_back(sco::AffExpr(0)); + exprs.emplace_back(0); } else { @@ -949,7 +949,7 @@ void SingleTimestepCollisionEvaluator::CalcCollisions(const Eigen::RefgetPairsWithZeroCoeff(); auto filter = [this, &zero_coeff_pairs](tesseract_collision::ContactResultMap::PairType& pair) { // Remove pairs with zero coeffs - if (std::find(zero_coeff_pairs.begin(), zero_coeff_pairs.end(), pair.first) != zero_coeff_pairs.end()) + if (zero_coeff_pairs.find(pair.first) != zero_coeff_pairs.end()) { pair.second.clear(); return; @@ -1150,7 +1150,7 @@ void DiscreteCollisionEvaluator::CalcCollisions(const Eigen::RefgetPairsWithZeroCoeff(); auto filter = [this, &zero_coeff_pairs](tesseract_collision::ContactResultMap::PairType& pair) { // Remove pairs with zero coeffs - if (std::find(zero_coeff_pairs.begin(), zero_coeff_pairs.end(), pair.first) != zero_coeff_pairs.end()) + if (zero_coeff_pairs.find(pair.first) != zero_coeff_pairs.end()) { pair.second.clear(); return; @@ -1386,7 +1386,7 @@ void CastCollisionEvaluator::CalcCollisions(const Eigen::RefgetPairsWithZeroCoeff(); auto filter = [this, &zero_coeff_pairs](tesseract_collision::ContactResultMap::PairType& pair) { // Remove pairs with zero coeffs - if (std::find(zero_coeff_pairs.begin(), zero_coeff_pairs.end(), pair.first) != zero_coeff_pairs.end()) + if (zero_coeff_pairs.find(pair.first) != zero_coeff_pairs.end()) { pair.second.clear(); return; diff --git a/trajopt/src/problem_description.cpp b/trajopt/src/problem_description.cpp index c1f7ae19..eb6aa47b 100644 --- a/trajopt/src/problem_description.cpp +++ b/trajopt/src/problem_description.cpp @@ -1089,7 +1089,7 @@ void JointPosTermInfo::hatch(TrajOptProb& prob) // Check if parameters are the correct size. checkParameterSize(coeffs, n_dof, "JointPosTermInfo coeffs", true); - checkParameterSize(targets, n_dof, "JointPosTermInfo upper_tols", true); + checkParameterSize(targets, n_dof, "JointPosTermInfo targets", true); checkParameterSize(upper_tols, n_dof, "JointPosTermInfo upper_tols", true); checkParameterSize(lower_tols, n_dof, "JointPosTermInfo lower_tols", true); diff --git a/trajopt_common/include/trajopt_common/cache.h b/trajopt_common/include/trajopt_common/cache.h index 2d0954b4..bbf4bed4 100644 --- a/trajopt_common/include/trajopt_common/cache.h +++ b/trajopt_common/include/trajopt_common/cache.h @@ -4,7 +4,6 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH #include -#include #include TRAJOPT_IGNORE_WARNINGS_POP diff --git a/trajopt_common/include/trajopt_common/config.hpp b/trajopt_common/include/trajopt_common/config.hpp index 2c67c1b1..bb9089fb 100644 --- a/trajopt_common/include/trajopt_common/config.hpp +++ b/trajopt_common/include/trajopt_common/config.hpp @@ -2,7 +2,6 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH #include -#include #include #include #include diff --git a/trajopt_common/include/trajopt_common/stl_to_string.hpp b/trajopt_common/include/trajopt_common/stl_to_string.hpp index 6297ccea..4c1daa70 100644 --- a/trajopt_common/include/trajopt_common/stl_to_string.hpp +++ b/trajopt_common/include/trajopt_common/stl_to_string.hpp @@ -1,7 +1,6 @@ #pragma once #include TRAJOPT_IGNORE_WARNINGS_PUSH -#include #include #include #include diff --git a/trajopt_common/src/logging.cpp b/trajopt_common/src/logging.cpp index 53cc7fd1..b027dd13 100644 --- a/trajopt_common/src/logging.cpp +++ b/trajopt_common/src/logging.cpp @@ -1,7 +1,6 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH #include -#include #include TRAJOPT_IGNORE_WARNINGS_POP diff --git a/trajopt_sco/src/bpmpd_interface.cpp b/trajopt_sco/src/bpmpd_interface.cpp index c641307c..e37ce4ff 100644 --- a/trajopt_sco/src/bpmpd_interface.cpp +++ b/trajopt_sco/src/bpmpd_interface.cpp @@ -1,7 +1,6 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH #include -#include #include #include #include @@ -238,7 +237,7 @@ BPMPDModel::BPMPDModel() Var BPMPDModel::addVar(const std::string& name) { std::scoped_lock lock(m_mutex); - m_vars.push_back(std::make_shared(m_vars.size(), name, this)); + m_vars.emplace_back(std::make_shared(m_vars.size(), name, this)); m_lbs.push_back(-BPMPD_BIG); m_ubs.push_back(BPMPD_BIG); return m_vars.back(); @@ -246,7 +245,7 @@ Var BPMPDModel::addVar(const std::string& name) Cnt BPMPDModel::addEqCnt(const AffExpr& expr, const std::string& /*name*/) { std::scoped_lock lock(m_mutex); - m_cnts.push_back(std::make_shared(m_cnts.size(), this)); + m_cnts.emplace_back(std::make_shared(m_cnts.size(), this)); m_cntExprs.push_back(expr); m_cntTypes.push_back(EQ); return m_cnts.back(); @@ -254,7 +253,7 @@ Cnt BPMPDModel::addEqCnt(const AffExpr& expr, const std::string& /*name*/) Cnt BPMPDModel::addIneqCnt(const AffExpr& expr, const std::string& /*name*/) { std::scoped_lock lock(m_mutex); - m_cnts.push_back(std::make_shared(m_cnts.size(), this)); + m_cnts.emplace_back(std::make_shared(m_cnts.size(), this)); m_cntExprs.push_back(expr); m_cntTypes.push_back(INEQ); return m_cnts.back(); diff --git a/trajopt_sco/src/modeling.cpp b/trajopt_sco/src/modeling.cpp index 402d3f1b..9282209b 100644 --- a/trajopt_sco/src/modeling.cpp +++ b/trajopt_sco/src/modeling.cpp @@ -2,7 +2,6 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include -#include #include TRAJOPT_IGNORE_WARNINGS_POP diff --git a/trajopt_sco/src/modeling_utils.cpp b/trajopt_sco/src/modeling_utils.cpp index a39e1e31..a0ad105b 100644 --- a/trajopt_sco/src/modeling_utils.cpp +++ b/trajopt_sco/src/modeling_utils.cpp @@ -1,7 +1,6 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH #include -#include TRAJOPT_IGNORE_WARNINGS_POP #include diff --git a/trajopt_sco/src/osqp_interface.cpp b/trajopt_sco/src/osqp_interface.cpp index 24b6cd62..3057a4c5 100644 --- a/trajopt_sco/src/osqp_interface.cpp +++ b/trajopt_sco/src/osqp_interface.cpp @@ -5,7 +5,6 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include #include -#include TRAJOPT_IGNORE_WARNINGS_POP #include @@ -63,7 +62,7 @@ OSQPModel::~OSQPModel() Var OSQPModel::addVar(const std::string& name) { std::scoped_lock lock(mutex_); - vars_.push_back(std::make_shared(vars_.size(), name, this)); + vars_.emplace_back(std::make_shared(vars_.size(), name, this)); lbs_.push_back(-OSQP_INFINITY); ubs_.push_back(OSQP_INFINITY); return vars_.back(); @@ -72,7 +71,7 @@ Var OSQPModel::addVar(const std::string& name) Cnt OSQPModel::addEqCnt(const AffExpr& expr, const std::string& /*name*/) { std::scoped_lock lock(mutex_); - cnts_.push_back(std::make_shared(cnts_.size(), this)); + cnts_.emplace_back(std::make_shared(cnts_.size(), this)); cnt_exprs_.push_back(expr); cnt_types_.push_back(EQ); return cnts_.back(); @@ -81,7 +80,7 @@ Cnt OSQPModel::addEqCnt(const AffExpr& expr, const std::string& /*name*/) Cnt OSQPModel::addIneqCnt(const AffExpr& expr, const std::string& /*name*/) { std::scoped_lock lock(mutex_); - cnts_.push_back(std::make_shared(cnts_.size(), this)); + cnts_.emplace_back(std::make_shared(cnts_.size(), this)); cnt_exprs_.push_back(expr); cnt_types_.push_back(INEQ); return cnts_.back(); diff --git a/trajopt_sco/src/qpoases_interface.cpp b/trajopt_sco/src/qpoases_interface.cpp index b65fcbc4..ca84a2ad 100644 --- a/trajopt_sco/src/qpoases_interface.cpp +++ b/trajopt_sco/src/qpoases_interface.cpp @@ -2,7 +2,6 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include -#include #include TRAJOPT_IGNORE_WARNINGS_POP @@ -39,7 +38,7 @@ qpOASESModel::~qpOASESModel() = default; Var qpOASESModel::addVar(const std::string& name) { std::scoped_lock lock(mutex_); - vars_.push_back(std::make_shared(vars_.size(), name, this)); + vars_.emplace_back(std::make_shared(vars_.size(), name, this)); lb_.push_back(-QPOASES_INFTY); ub_.push_back(QPOASES_INFTY); return vars_.back(); @@ -48,7 +47,7 @@ Var qpOASESModel::addVar(const std::string& name) Cnt qpOASESModel::addEqCnt(const AffExpr& expr, const std::string& /*name*/) { std::scoped_lock lock(mutex_); - cnts_.push_back(std::make_shared(cnts_.size(), this)); + cnts_.emplace_back(std::make_shared(cnts_.size(), this)); cnt_exprs_.push_back(expr); cnt_types_.push_back(EQ); return cnts_.back(); @@ -57,7 +56,7 @@ Cnt qpOASESModel::addEqCnt(const AffExpr& expr, const std::string& /*name*/) Cnt qpOASESModel::addIneqCnt(const AffExpr& expr, const std::string& /*name*/) { std::scoped_lock lock(mutex_); - cnts_.push_back(std::make_shared(cnts_.size(), this)); + cnts_.emplace_back(std::make_shared(cnts_.size(), this)); cnt_exprs_.push_back(expr); cnt_types_.push_back(INEQ); return cnts_.back();