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::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;
@@ -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;
@@ -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;
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_common/macros.h>
 TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <Eigen/Eigen>
-#include <memory>
 #include <algorithm>
 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_common/macros.h>
 TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <boost/program_options.hpp>
-#include <iostream>
 #include <memory>
 #include <string>
 #include <vector>
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_common/macros.h>
 TRAJOPT_IGNORE_WARNINGS_PUSH
-#include <iostream>
 #include <map>
 #include <set>
 #include <sstream>
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_common/macros.h>
 TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <cstdlib>
-#include <iostream>
 #include <string>
 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_common/macros.h>
 TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <cmath>
-#include <fstream>
 #include <csignal>
 #include <array>
 #include <mutex>
@@ -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<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();
@@ -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<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();
@@ -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<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();
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 <boost/format.hpp>
 #include <cstdio>
-#include <iostream>
 #include <sstream>
 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_common/macros.h>
 TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <Eigen/Eigenvalues>
-#include <iostream>
 TRAJOPT_IGNORE_WARNINGS_POP
 
 #include <trajopt_sco/expr_ops.hpp>
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 <Eigen/SparseCore>
 #include <fstream>
 #include <csignal>
-#include <iomanip>
 TRAJOPT_IGNORE_WARNINGS_POP
 
 #include <trajopt_sco/osqp_interface.hpp>
@@ -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();
@@ -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();
@@ -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();
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 <cmath>
 #include <Eigen/Eigen>
-#include <fstream>
 #include <csignal>
 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<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();
@@ -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();
@@ -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();