Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Various include cleanups and clang-tidy fixes, and an error message fix. #408

Merged
merged 1 commit into from
Jun 10, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 5 additions & 5 deletions trajopt/src/collision_terms.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down Expand Up @@ -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
{
Expand Down Expand Up @@ -949,7 +949,7 @@ void SingleTimestepCollisionEvaluator::CalcCollisions(const Eigen::Ref<const Eig
const auto& zero_coeff_pairs = getSafetyMarginData()->getPairsWithZeroCoeff();
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;
Expand Down Expand Up @@ -1150,7 +1150,7 @@ void DiscreteCollisionEvaluator::CalcCollisions(const Eigen::Ref<const Eigen::Ve
const auto& zero_coeff_pairs = getSafetyMarginData()->getPairsWithZeroCoeff();
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;
Expand Down Expand Up @@ -1386,7 +1386,7 @@ void CastCollisionEvaluator::CalcCollisions(const Eigen::Ref<const Eigen::Vector
const auto& zero_coeff_pairs = getSafetyMarginData()->getPairsWithZeroCoeff();
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;
Expand Down
2 changes: 1 addition & 1 deletion trajopt/src/problem_description.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down
1 change: 0 additions & 1 deletion trajopt_common/include/trajopt_common/cache.h
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@
#include <trajopt_common/macros.h>
TRAJOPT_IGNORE_WARNINGS_PUSH
#include <Eigen/Eigen>
#include <memory>
#include <algorithm>
TRAJOPT_IGNORE_WARNINGS_POP

Expand Down
1 change: 0 additions & 1 deletion trajopt_common/include/trajopt_common/config.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,6 @@
#include <trajopt_common/macros.h>
TRAJOPT_IGNORE_WARNINGS_PUSH
#include <boost/program_options.hpp>
#include <iostream>
#include <memory>
#include <string>
#include <vector>
Expand Down
1 change: 0 additions & 1 deletion trajopt_common/include/trajopt_common/stl_to_string.hpp
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
#pragma once
#include <trajopt_common/macros.h>
TRAJOPT_IGNORE_WARNINGS_PUSH
#include <iostream>
#include <map>
#include <set>
#include <sstream>
Expand Down
1 change: 0 additions & 1 deletion trajopt_common/src/logging.cpp
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
#include <trajopt_common/macros.h>
TRAJOPT_IGNORE_WARNINGS_PUSH
#include <cstdlib>
#include <iostream>
#include <string>
TRAJOPT_IGNORE_WARNINGS_POP

Expand Down
7 changes: 3 additions & 4 deletions trajopt_sco/src/bpmpd_interface.cpp
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
#include <trajopt_common/macros.h>
TRAJOPT_IGNORE_WARNINGS_PUSH
#include <cmath>
#include <fstream>
#include <csignal>
#include <array>
#include <mutex>
Expand Down Expand Up @@ -238,23 +237,23 @@ BPMPDModel::BPMPDModel()
Var BPMPDModel::addVar(const std::string& name)
{
std::scoped_lock lock(m_mutex);
m_vars.push_back(std::make_shared<VarRep>(m_vars.size(), name, this));
m_vars.emplace_back(std::make_shared<VarRep>(m_vars.size(), name, this));
m_lbs.push_back(-BPMPD_BIG);
m_ubs.push_back(BPMPD_BIG);
return m_vars.back();
}
Cnt BPMPDModel::addEqCnt(const AffExpr& expr, const std::string& /*name*/)
{
std::scoped_lock lock(m_mutex);
m_cnts.push_back(std::make_shared<CntRep>(m_cnts.size(), this));
m_cnts.emplace_back(std::make_shared<CntRep>(m_cnts.size(), this));
m_cntExprs.push_back(expr);
m_cntTypes.push_back(EQ);
return m_cnts.back();
}
Cnt BPMPDModel::addIneqCnt(const AffExpr& expr, const std::string& /*name*/)
{
std::scoped_lock lock(m_mutex);
m_cnts.push_back(std::make_shared<CntRep>(m_cnts.size(), this));
m_cnts.emplace_back(std::make_shared<CntRep>(m_cnts.size(), this));
m_cntExprs.push_back(expr);
m_cntTypes.push_back(INEQ);
return m_cnts.back();
Expand Down
1 change: 0 additions & 1 deletion trajopt_sco/src/modeling.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,6 @@
TRAJOPT_IGNORE_WARNINGS_PUSH
#include <boost/format.hpp>
#include <cstdio>
#include <iostream>
#include <sstream>
TRAJOPT_IGNORE_WARNINGS_POP

Expand Down
1 change: 0 additions & 1 deletion trajopt_sco/src/modeling_utils.cpp
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
#include <trajopt_common/macros.h>
TRAJOPT_IGNORE_WARNINGS_PUSH
#include <Eigen/Eigenvalues>
#include <iostream>
TRAJOPT_IGNORE_WARNINGS_POP

#include <trajopt_sco/expr_ops.hpp>
Expand Down
7 changes: 3 additions & 4 deletions trajopt_sco/src/osqp_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,6 @@ TRAJOPT_IGNORE_WARNINGS_PUSH
#include <Eigen/SparseCore>
#include <fstream>
#include <csignal>
#include <iomanip>
TRAJOPT_IGNORE_WARNINGS_POP

#include <trajopt_sco/osqp_interface.hpp>
Expand Down Expand Up @@ -63,7 +62,7 @@ OSQPModel::~OSQPModel()
Var OSQPModel::addVar(const std::string& name)
{
std::scoped_lock lock(mutex_);
vars_.push_back(std::make_shared<VarRep>(vars_.size(), name, this));
vars_.emplace_back(std::make_shared<VarRep>(vars_.size(), name, this));
lbs_.push_back(-OSQP_INFINITY);
ubs_.push_back(OSQP_INFINITY);
return vars_.back();
Expand All @@ -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<CntRep>(cnts_.size(), this));
cnts_.emplace_back(std::make_shared<CntRep>(cnts_.size(), this));
cnt_exprs_.push_back(expr);
cnt_types_.push_back(EQ);
return cnts_.back();
Expand All @@ -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<CntRep>(cnts_.size(), this));
cnts_.emplace_back(std::make_shared<CntRep>(cnts_.size(), this));
cnt_exprs_.push_back(expr);
cnt_types_.push_back(INEQ);
return cnts_.back();
Expand Down
7 changes: 3 additions & 4 deletions trajopt_sco/src/qpoases_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,6 @@
TRAJOPT_IGNORE_WARNINGS_PUSH
#include <cmath>
#include <Eigen/Eigen>
#include <fstream>
#include <csignal>
TRAJOPT_IGNORE_WARNINGS_POP

Expand Down Expand Up @@ -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<VarRep>(vars_.size(), name, this));
vars_.emplace_back(std::make_shared<VarRep>(vars_.size(), name, this));
lb_.push_back(-QPOASES_INFTY);
ub_.push_back(QPOASES_INFTY);
return vars_.back();
Expand All @@ -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<CntRep>(cnts_.size(), this));
cnts_.emplace_back(std::make_shared<CntRep>(cnts_.size(), this));
cnt_exprs_.push_back(expr);
cnt_types_.push_back(EQ);
return cnts_.back();
Expand All @@ -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<CntRep>(cnts_.size(), this));
cnts_.emplace_back(std::make_shared<CntRep>(cnts_.size(), this));
cnt_exprs_.push_back(expr);
cnt_types_.push_back(INEQ);
return cnts_.back();
Expand Down
Loading