diff --git a/.clang-format b/.clang-format
index 792fd96a..3c94dd20 100644
--- a/.clang-format
+++ b/.clang-format
@@ -53,15 +53,16 @@ BreakBeforeBraces: Custom
 
 # Control of individual brace wrapping cases
 BraceWrapping: {
-    AfterClass: 'true'
-    AfterControlStatement: 'true'
-    AfterEnum : 'true'
-    AfterFunction : 'true'
-    AfterNamespace : 'true'
-    AfterStruct : 'true'
-    AfterUnion : 'true'
-    BeforeCatch : 'true'
-    BeforeElse : 'true'
-    IndentBraces : 'false'
+    AfterCaseLabel: 'true',
+    AfterClass: 'true',
+    AfterControlStatement: 'true',
+    AfterEnum : 'true',
+    AfterFunction : 'true',
+    AfterNamespace : 'true',
+    AfterStruct : 'true',
+    AfterUnion : 'true',
+    BeforeCatch : 'true',
+    BeforeElse : 'true',
+    IndentBraces : 'false',
 }
 ...
diff --git a/.clang-tidy b/.clang-tidy
index 03792ea2..94194d7c 100644
--- a/.clang-tidy
+++ b/.clang-tidy
@@ -20,6 +20,7 @@ Checks: >
     -cppcoreguidelines-non-private-member-variables-in-classes,
     -cppcoreguidelines-owning-memory,
     misc-*,
+    -misc-include-cleaner,
     -misc-non-private-member-variables-in-classes,
     -misc-no-recursion,
     modernize-*,
@@ -56,6 +57,7 @@ WarningsAsErrors: >
     -cppcoreguidelines-non-private-member-variables-in-classes,
     -cppcoreguidelines-owning-memory,
     misc-*,
+    -misc-include-cleaner,
     -misc-non-private-member-variables-in-classes,
     -misc-no-recursion,
     modernize-*,
diff --git a/trajopt/include/trajopt/json_marshal.hpp b/trajopt/include/trajopt/json_marshal.hpp
index ad678c18..a5676e8c 100644
--- a/trajopt/include/trajopt/json_marshal.hpp
+++ b/trajopt/include/trajopt/json_marshal.hpp
@@ -3,7 +3,6 @@
 TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <Eigen/Eigen>
 #include <boost/format.hpp>
-#include <sstream>
 #include <string>
 #include <vector>
 TRAJOPT_IGNORE_WARNINGS_POP
diff --git a/trajopt/include/trajopt/problem_description.hpp b/trajopt/include/trajopt/problem_description.hpp
index 38cd481a..5e6563a8 100644
--- a/trajopt/include/trajopt/problem_description.hpp
+++ b/trajopt/include/trajopt/problem_description.hpp
@@ -1,7 +1,6 @@
 #pragma once
 #include <trajopt_common/macros.h>
 TRAJOPT_IGNORE_WARNINGS_PUSH
-#include <unordered_map>
 #include <type_traits>
 TRAJOPT_IGNORE_WARNINGS_POP
 
@@ -99,7 +98,7 @@ class TrajOptProb : public sco::OptProb
 
 private:
   /** @brief If true, the last column in the optimization matrix will be 1/dt */
-  bool has_time;
+  bool has_time{ false };
   VarArray m_traj_vars;
   std::shared_ptr<const tesseract_kinematics::JointGroup> m_kin;
   std::shared_ptr<const tesseract_environment::Environment> m_env;
@@ -314,7 +313,7 @@ struct DynamicCartPoseTermInfo : public TermInfo
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
   /** @brief Timestep at which to apply term */
-  int timestep;
+  int timestep{ 0 };
   /** @brief Coefficients for position and rotation */
   Eigen::Vector3d pos_coeffs, rot_coeffs;
   /** @brief Link which should reach desired pos */
@@ -354,7 +353,7 @@ struct CartPoseTermInfo : public TermInfo
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
   /** @brief Timestep at which to apply term */
-  int timestep;
+  int timestep{ 0 };
   Eigen::Vector3d pos_coeffs, rot_coeffs;
 
   /** @brief Link which should reach desired pos */
diff --git a/trajopt/include/trajopt/typedefs.hpp b/trajopt/include/trajopt/typedefs.hpp
index 4091a0be..f0ab30d8 100644
--- a/trajopt/include/trajopt/typedefs.hpp
+++ b/trajopt/include/trajopt/typedefs.hpp
@@ -2,7 +2,6 @@
 #include <trajopt_common/macros.h>
 TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <Eigen/Core>
-#include <map>
 #include <vector>
 #include <tesseract_visualization/fwd.h>
 TRAJOPT_IGNORE_WARNINGS_POP
diff --git a/trajopt/include/trajopt/utils.hpp b/trajopt/include/trajopt/utils.hpp
index 9b55e7e7..1322268c 100644
--- a/trajopt/include/trajopt/utils.hpp
+++ b/trajopt/include/trajopt/utils.hpp
@@ -1,7 +1,6 @@
 #pragma once
 #include <trajopt_common/macros.h>
 TRAJOPT_IGNORE_WARNINGS_PUSH
-#include <unordered_map>
 #include <Eigen/Geometry>
 TRAJOPT_IGNORE_WARNINGS_POP
 
diff --git a/trajopt/src/collision_terms.cpp b/trajopt/src/collision_terms.cpp
index 28853f00..bef3f631 100644
--- a/trajopt/src/collision_terms.cpp
+++ b/trajopt/src/collision_terms.cpp
@@ -606,7 +606,7 @@ void CollisionEvaluator::CalcDists(const DblVec& x, DblVec& dists)
   CollisionsToDistances(*dist_results, dists);
 }
 
-inline size_t hash(const DblVec& x) { return boost::hash_range(x.begin(), x.end()); }
+inline std::size_t hash(const DblVec& x) { return boost::hash_range(x.begin(), x.end()); }
 ContactResultVectorConstPtr CollisionEvaluator::GetContactResultVectorCached(const DblVec& x)
 {
   std::pair<ContactResultMapConstPtr, ContactResultVectorConstPtr> pair = GetContactResultCached(x);
@@ -622,7 +622,7 @@ ContactResultMapConstPtr CollisionEvaluator::GetContactResultMapCached(const Dbl
 std::pair<ContactResultMapConstPtr, ContactResultVectorConstPtr>
 CollisionEvaluator::GetContactResultCached(const DblVec& x)
 {
-  size_t key = hash(sco::getDblVec(x, GetVars()));
+  std::size_t key = hash(sco::getDblVec(x, GetVars()));
   auto* it = m_cache.get(key);
   if (it != nullptr)
   {
@@ -985,7 +985,7 @@ void SingleTimestepCollisionEvaluator::Plot(const std::shared_ptr<tesseract_visu
 
   tesseract_collision::ContactResultVector dist_results_copy(dist_results.size());
   Eigen::VectorXd safety_distance(dist_results.size());
-  for (auto i = 0u; i < dist_results.size(); ++i)
+  for (auto i = 0U; i < dist_results.size(); ++i)
   {
     const tesseract_collision::ContactResult& res = dist_results[i].get();
     dist_results_copy[i] = res;
@@ -1206,7 +1206,7 @@ void DiscreteCollisionEvaluator::Plot(const std::shared_ptr<tesseract_visualizat
 
   tesseract_collision::ContactResultVector dist_results_copy(dist_results.size());
   Eigen::VectorXd safety_distance(dist_results.size());
-  for (auto i = 0u; i < dist_results.size(); ++i)
+  for (auto i = 0U; i < dist_results.size(); ++i)
   {
     const tesseract_collision::ContactResult& res = dist_results[i].get();
     dist_results_copy[i] = res;
@@ -1401,11 +1401,11 @@ void CastCollisionEvaluator::CalcCollisions(const Eigen::Ref<const Eigen::Vector
   if (dist > longest_valid_segment_length_)
   {
     // Calculate the number state to interpolate
-    size_t cnt = static_cast<size_t>(std::ceil(dist / longest_valid_segment_length_)) + 1;
+    auto cnt = static_cast<long>(std::ceil(dist / longest_valid_segment_length_)) + 1;
 
     // Create interpolated trajectory between two states that satisfies the longest valid segment length.
     tesseract_common::TrajArray subtraj(cnt, dof_vals0.size());
-    for (size_t i = 0; i < dof_vals0.size(); ++i)
+    for (long i = 0; i < dof_vals0.size(); ++i)
       subtraj.col(i) = Eigen::VectorXd::LinSpaced(cnt, dof_vals0(i), dof_vals1(i));
 
     // Perform casted collision checking for sub trajectory and store results in contacts_vector
@@ -1461,7 +1461,7 @@ void CastCollisionEvaluator::Plot(const std::shared_ptr<tesseract_visualization:
 
   tesseract_collision::ContactResultVector dist_results_copy(dist_results.size());
   Eigen::VectorXd safety_distance(dist_results.size());
-  for (auto i = 0u; i < dist_results.size(); ++i)
+  for (auto i = 0U; i < dist_results.size(); ++i)
   {
     const tesseract_collision::ContactResult& res = dist_results[i].get();
     dist_results_copy[i] = res;
diff --git a/trajopt/src/file_write_callback.cpp b/trajopt/src/file_write_callback.cpp
index 06d1098a..3011614e 100644
--- a/trajopt/src/file_write_callback.cpp
+++ b/trajopt/src/file_write_callback.cpp
@@ -103,7 +103,7 @@ std::function<void(sco::OptProb*, sco::OptResults&)> WriteCallback(std::shared_p
 
   // Write joint names
   std::vector<std::string> joint_names = prob->GetEnv()->getActiveJointNames();
-  for (size_t i = 0; i < joint_names.size(); i++)
+  for (std::size_t i = 0; i < joint_names.size(); i++)
   {
     if (i != 0)
     {
@@ -136,6 +136,8 @@ std::function<void(sco::OptProb*, sco::OptResults&)> WriteCallback(std::shared_p
   *file << std::endl;
 
   // return callback function
-  return bind(&WriteFile, file, prob->GetKin(), std::ref(prob->GetVars()), std::placeholders::_2);
+  return [file, capture0 = prob->GetKin(), &capture1 = prob->GetVars()](auto&&, auto&& PH2) {
+    WriteFile(file, capture0, capture1, std::forward<decltype(PH2)>(PH2));
+  };
 }
 }  // namespace trajopt
diff --git a/trajopt/src/kinematic_terms.cpp b/trajopt/src/kinematic_terms.cpp
index 9b1fe7fb..8bc47fbb 100644
--- a/trajopt/src/kinematic_terms.cpp
+++ b/trajopt/src/kinematic_terms.cpp
@@ -322,10 +322,10 @@ CartPoseJacCalculator::CartPoseJacCalculator(
   , source_frame_offset_(source_frame_offset)
   , target_frame_(std::move(target_frame))
   , target_frame_offset_(target_frame_offset)
+  , is_target_active_(manip_->isActiveLinkName(target_frame_))
   , indices_(indices)
   , epsilon_(DEFAULT_EPSILON)
 {
-  is_target_active_ = manip_->isActiveLinkName(target_frame_);
   assert(indices_.size() <= 6);
 
   if (is_target_active_)
@@ -459,16 +459,16 @@ Eigen::VectorXd JointVelErrCalculator::operator()(const VectorXd& var_vals) cons
 MatrixXd JointVelJacCalculator::operator()(const VectorXd& var_vals) const
 {
   // var_vals = (theta_t1, theta_t2, theta_t3 ... 1/dt_1, 1/dt_2, 1/dt_3 ...)
-  auto num_vals = static_cast<int>(var_vals.rows());
-  int half = num_vals / 2;
-  int num_vels = half - 1;
+  auto num_vals = var_vals.rows();
+  auto half = num_vals / 2;
+  auto num_vels = half - 1;
   MatrixXd jac = MatrixXd::Zero(num_vels * 2, num_vals);
 
-  for (int i = 0; i < num_vels; i++)
+  for (auto i = 0; i < num_vels; i++)
   {
     // v = (j_i+1 - j_i)*(1/dt)
     // We calculate v with the dt from the second pt
-    int time_index = i + half + 1;
+    auto time_index = i + half + 1;
     // dv_i/dj_i = -(1/dt)
     jac(i, i) = -1.0 * var_vals(time_index);
     // dv_i/dj_i+1 = (1/dt)
diff --git a/trajopt/src/problem_description.cpp b/trajopt/src/problem_description.cpp
index eb6aa47b..c9c5d6e1 100644
--- a/trajopt/src/problem_description.cpp
+++ b/trajopt/src/problem_description.cpp
@@ -111,7 +111,7 @@ TermInfo::Ptr TermInfo::fromName(const std::string& type)
     return (*name2maker[type])();
 
   // RAVELOG_ERROR("There is no cost of type %s\n", type.c_str());
-  return TermInfo::Ptr();
+  return {};
 }
 
 void ProblemConstructionInfo::readBasicInfo(const Json::Value& v)
@@ -165,7 +165,7 @@ void ProblemConstructionInfo::readCosts(const Json::Value& v)
   for (const auto& it : v)
   {
     std::string type;
-    bool use_time;
+    bool use_time{ false };
     json_marshal::childFromJson(it, type, "type");
     json_marshal::childFromJson(it, use_time, "use_time", false);
     LOG_DEBUG("reading term: %s", type.c_str());
@@ -195,7 +195,7 @@ void ProblemConstructionInfo::readConstraints(const Json::Value& v)
   for (const auto& it : v)
   {
     std::string type;
-    bool use_time;
+    bool use_time{ false };
     json_marshal::childFromJson(it, type, "type");
     json_marshal::childFromJson(it, use_time, "use_time", false);
     LOG_DEBUG("reading term: %s", type.c_str());
@@ -553,7 +553,7 @@ TrajOptProb::TrajOptProb(int n_steps, const ProblemConstructionInfo& pci)
   : OptProb(pci.basic_info.convex_solver, pci.basic_info.convex_solver_config), m_kin(pci.kin), m_env(pci.env)
 {
   const Eigen::MatrixX2d& limits = m_kin->getLimits().joint_limits;
-  auto n_dof = static_cast<int>(m_kin->numJoints());
+  auto n_dof = m_kin->numJoints();
   Eigen::VectorXd lower, upper;
   lower = limits.col(0);
   upper = limits.col(1);
@@ -587,7 +587,7 @@ TrajOptProb::TrajOptProb(int n_steps, const ProblemConstructionInfo& pci)
     }
   }
   sco::VarVector trajvarvec = createVariables(names, vlower, vupper);
-  m_traj_vars = VarArray(n_steps, n_dof + (pci.basic_info.use_time ? 1 : 0), trajvarvec.data());
+  m_traj_vars = VarArray(n_steps, static_cast<int>(n_dof) + (pci.basic_info.use_time ? 1 : 0), trajvarvec.data());
 }
 
 void UserDefinedTermInfo::fromJson(ProblemConstructionInfo& /*pci*/, const Json::Value& /*v*/)
@@ -1224,10 +1224,10 @@ void JointVelTermInfo::hatch(TrajOptProb& prob)
 
   if (term_type == (TermType::TT_COST | TermType::TT_USE_TIME))
   {
-    auto num_vels = static_cast<unsigned>(last_step - first_step);
+    auto num_vels = static_cast<std::size_t>(last_step - first_step);
 
     // Apply seperate cost to each joint b/c that is how the error function is currently written
-    for (size_t j = 0; j < n_dof; j++)
+    for (std::size_t j = 0; j < n_dof; j++)
     {
       // Get a vector of a single column of vars
       sco::VarVector joint_vars_vec = joint_vars.cblock(first_step, static_cast<int>(j), last_step - first_step + 1);
@@ -1263,10 +1263,10 @@ void JointVelTermInfo::hatch(TrajOptProb& prob)
   }
   else if (term_type == (TermType::TT_CNT | TermType::TT_USE_TIME))
   {
-    auto num_vels = static_cast<unsigned>(last_step - first_step);
+    auto num_vels = static_cast<std::size_t>(last_step - first_step);
 
     // Apply seperate cnt to each joint b/c that is how the error function is currently written
-    for (size_t j = 0; j < n_dof; j++)
+    for (std::size_t j = 0; j < n_dof; j++)
     {
       // Get a vector of a single column of vars
       sco::VarVector joint_vars_vec = joint_vars.cblock(first_step, static_cast<int>(j), last_step - first_step + 1);
@@ -1599,7 +1599,7 @@ void CollisionTermInfo::fromJson(ProblemConstructionInfo& pci, const Json::Value
   const Json::Value& params = v["params"];
 
   int n_steps = pci.basic_info.n_steps;
-  int collision_evaluator_type;
+  int collision_evaluator_type{ 0 };
   json_marshal::childFromJson(params, collision_evaluator_type, "evaluator_type", 0);
   json_marshal::childFromJson(params, use_weighted_sum, "use_weighted_sum", false);
   json_marshal::childFromJson(params, first_step, "first_step", 0);
@@ -1635,24 +1635,24 @@ void CollisionTermInfo::fromJson(ProblemConstructionInfo& pci, const Json::Value
   json_marshal::childFromJson(params, coeffs, "coeffs");
   int n_terms = last_step - first_step + 1;
   if (coeffs.size() == 1)
-    coeffs = DblVec(static_cast<size_t>(n_terms), coeffs[0]);
+    coeffs = DblVec(static_cast<std::size_t>(n_terms), coeffs[0]);
   else if (static_cast<int>(coeffs.size()) != n_terms)
   {
     PRINT_AND_THROW(boost::format("wrong size: coeffs. expected %i got %i") % n_terms % coeffs.size());
   }
   json_marshal::childFromJson(params, dist_pen, "dist_pen");
   if (dist_pen.size() == 1)
-    dist_pen = DblVec(static_cast<size_t>(n_terms), dist_pen[0]);
+    dist_pen = DblVec(static_cast<std::size_t>(n_terms), dist_pen[0]);
   else if (static_cast<int>(dist_pen.size()) != n_terms)
   {
     PRINT_AND_THROW(boost::format("wrong size: dist_pen. expected %i got %i") % n_terms % dist_pen.size());
   }
 
   // Create Contact Distance Data for each timestep
-  info.reserve(static_cast<size_t>(n_terms));
+  info.reserve(static_cast<std::size_t>(n_terms));
   for (int i = first_step; i <= last_step; ++i)
   {
-    auto index = static_cast<size_t>(i - first_step);
+    auto index = static_cast<std::size_t>(i - first_step);
     auto data = std::make_shared<trajopt_common::SafetyMarginData>(dist_pen[index], coeffs[index]);
     info.push_back(data);
   }
@@ -1680,7 +1680,7 @@ void CollisionTermInfo::fromJson(ProblemConstructionInfo& pci, const Json::Value
       json_marshal::childFromJson(*it, pair_coeffs, "coeffs");
       if (pair_coeffs.size() == 1)
       {
-        pair_coeffs = DblVec(static_cast<size_t>(n_terms), pair_coeffs[0]);
+        pair_coeffs = DblVec(static_cast<std::size_t>(n_terms), pair_coeffs[0]);
       }
       else if (static_cast<int>(pair_coeffs.size()) != n_terms)
       {
@@ -1691,7 +1691,7 @@ void CollisionTermInfo::fromJson(ProblemConstructionInfo& pci, const Json::Value
       json_marshal::childFromJson(*it, pair_dist_pen, "dist_pen");
       if (pair_dist_pen.size() == 1)
       {
-        pair_dist_pen = DblVec(static_cast<size_t>(n_terms), pair_dist_pen[0]);
+        pair_dist_pen = DblVec(static_cast<std::size_t>(n_terms), pair_dist_pen[0]);
       }
       else if (static_cast<int>(pair_dist_pen.size()) != n_terms)
       {
@@ -1700,7 +1700,7 @@ void CollisionTermInfo::fromJson(ProblemConstructionInfo& pci, const Json::Value
 
       for (auto i = first_step; i <= last_step; ++i)
       {
-        auto index = static_cast<size_t>(i - first_step);
+        auto index = static_cast<std::size_t>(i - first_step);
         trajopt_common::SafetyMarginData::Ptr& data = info[index];
         for (const auto& p : pair)
         {
@@ -1738,7 +1738,7 @@ void CollisionTermInfo::hatch(TrajOptProb& prob)
         bool current_fixed = std::find(fixed_steps.begin(), fixed_steps.end(), i) != fixed_steps.end();
         bool next_fixed = std::find(fixed_steps.begin(), fixed_steps.end(), i + 1) != fixed_steps.end();
 
-        CollisionExpressionEvaluatorType expression_evaluator_type;
+        CollisionExpressionEvaluatorType expression_evaluator_type{};
         if (!current_fixed && !next_fixed)
         {
           expression_evaluator_type = (use_weighted_sum) ?
@@ -1764,7 +1764,7 @@ void CollisionTermInfo::hatch(TrajOptProb& prob)
 
         auto c = std::make_shared<CollisionCost>(prob.GetKin(),
                                                  prob.GetEnv(),
-                                                 info[static_cast<size_t>(i - first_step)],
+                                                 info[static_cast<std::size_t>(i - first_step)],
                                                  contact_test_type,
                                                  longest_valid_segment_length,
                                                  prob.GetVarRow(i, 0, n_dof),
@@ -1788,7 +1788,7 @@ void CollisionTermInfo::hatch(TrajOptProb& prob)
         {
           auto c = std::make_shared<CollisionCost>(prob.GetKin(),
                                                    prob.GetEnv(),
-                                                   info[static_cast<size_t>(i - first_step)],
+                                                   info[static_cast<std::size_t>(i - first_step)],
                                                    contact_test_type,
                                                    prob.GetVarRow(i, 0, n_dof),
                                                    expression_evaluator_type,
@@ -1810,7 +1810,7 @@ void CollisionTermInfo::hatch(TrajOptProb& prob)
         bool current_fixed = std::find(fixed_steps.begin(), fixed_steps.end(), i) != fixed_steps.end();
         bool next_fixed = std::find(fixed_steps.begin(), fixed_steps.end(), i + 1) != fixed_steps.end();
 
-        CollisionExpressionEvaluatorType expression_evaluator_type;
+        CollisionExpressionEvaluatorType expression_evaluator_type{};
         if (!current_fixed && !next_fixed)
         {
           expression_evaluator_type = (use_weighted_sum) ?
@@ -1836,7 +1836,7 @@ void CollisionTermInfo::hatch(TrajOptProb& prob)
 
         auto c = std::make_shared<CollisionConstraint>(prob.GetKin(),
                                                        prob.GetEnv(),
-                                                       info[static_cast<size_t>(i - first_step)],
+                                                       info[static_cast<std::size_t>(i - first_step)],
                                                        contact_test_type,
                                                        longest_valid_segment_length,
                                                        prob.GetVarRow(i, 0, n_dof),
@@ -1860,7 +1860,7 @@ void CollisionTermInfo::hatch(TrajOptProb& prob)
         {
           auto c = std::make_shared<CollisionConstraint>(prob.GetKin(),
                                                          prob.GetEnv(),
-                                                         info[static_cast<size_t>(i - first_step)],
+                                                         info[static_cast<std::size_t>(i - first_step)],
                                                          contact_test_type,
                                                          prob.GetVarRow(i, 0, n_dof),
                                                          expression_evaluator_type,
@@ -1899,8 +1899,8 @@ void TotalTimeTermInfo::hatch(TrajOptProb& prob)
   }
 
   // Get correct penalty type
-  sco::PenaltyType penalty_type;
-  sco::ConstraintType constraint_type;
+  sco::PenaltyType penalty_type{};
+  sco::ConstraintType constraint_type{};
   if (trajopt_common::doubleEquals(limit, 0.0))
   {
     penalty_type = sco::SQUARED;
diff --git a/trajopt/src/utils.cpp b/trajopt/src/utils.cpp
index 45fb93d6..c8752d8e 100644
--- a/trajopt/src/utils.cpp
+++ b/trajopt/src/utils.cpp
@@ -41,11 +41,11 @@ void AddVarArrays(sco::OptProb& prob,
                   const std::vector<std::string>& name_prefix,
                   const std::vector<VarArray*>& newvars)
 {
-  size_t n_arr = name_prefix.size();
+  std::size_t n_arr = name_prefix.size();
   assert(static_cast<unsigned>(n_arr) == newvars.size());
 
   std::vector<Eigen::MatrixXi> index(n_arr);
-  for (size_t i = 0; i < n_arr; ++i)
+  for (std::size_t i = 0; i < n_arr; ++i)
   {
     newvars[i]->resize(rows, cols[i]);
     index[i].resize(rows, cols[i]);
@@ -55,7 +55,7 @@ void AddVarArrays(sco::OptProb& prob,
   int var_idx = prob.getNumVars();
   for (int i = 0; i < rows; ++i)
   {
-    for (size_t k = 0; k < n_arr; ++k)
+    for (std::size_t k = 0; k < n_arr; ++k)
     {
       for (int j = 0; j < cols[k]; ++j)
       {
@@ -68,13 +68,13 @@ void AddVarArrays(sco::OptProb& prob,
   prob.createVariables(names);  // note that w,r, are both unbounded
 
   const std::vector<sco::Var>& vars = prob.getVars();
-  for (size_t k = 0; k < n_arr; ++k)
+  for (std::size_t k = 0; k < n_arr; ++k)
   {
     for (int i = 0; i < rows; ++i)
     {
       for (int j = 0; j < cols[k]; ++j)
       {
-        (*newvars[k])(i, j) = vars[static_cast<size_t>(index[k](i, j))];
+        (*newvars[k])(i, j) = vars[static_cast<std::size_t>(index[k](i, j))];
       }
     }
   }
diff --git a/trajopt/test/interface_unit.cpp b/trajopt/test/interface_unit.cpp
index 5c8909f0..13147ffc 100644
--- a/trajopt/test/interface_unit.cpp
+++ b/trajopt/test/interface_unit.cpp
@@ -243,7 +243,7 @@ TEST_F(InterfaceTest, bitmask_test)
   std::vector<bool> cnt{ true, false, true, false };
   std::vector<bool> time{ false, false, true, true };
 
-  for (size_t i = 0; i < types.size(); i++)
+  for (std::size_t i = 0; i < types.size(); i++)
   {
     std::bitset<8> x(static_cast<long long unsigned>(types[i]));
 
diff --git a/trajopt/test/kinematic_costs_unit.cpp b/trajopt/test/kinematic_costs_unit.cpp
index 27cdece5..9cbaebdb 100644
--- a/trajopt/test/kinematic_costs_unit.cpp
+++ b/trajopt/test/kinematic_costs_unit.cpp
@@ -114,7 +114,7 @@ TEST_F(KinematicCostsTest, CartPoseJacCalculator)  // NOLINT
 //  Eigen::VectorXd values(15);
 //  values.setZero();
 //  std::vector<std::string> joint_names = kin->getJointNames();
-//  for (size_t i = 0; i < 15; ++i)
+//  for (std::size_t i = 0; i < 15; ++i)
 //  {
 //    if (joint_names[i] == "r_elbow_flex_joint" || joint_names[i] == "l_elbow_flex_joint")
 //      values(static_cast<long>(i)) = -0.15;
diff --git a/trajopt/test/numerical_ik_unit.cpp b/trajopt/test/numerical_ik_unit.cpp
index 03537953..e2ba8bbd 100644
--- a/trajopt/test/numerical_ik_unit.cpp
+++ b/trajopt/test/numerical_ik_unit.cpp
@@ -88,7 +88,7 @@ void runTest(const Environment::Ptr& env, const Visualization::Ptr& /*plotter*/,
   //  }
 
   CONSOLE_BRIDGE_logDebug("DOF: %d", prob->GetNumDOF());
-  opt->initialize(DblVec(static_cast<size_t>(prob->GetNumDOF()), 0));
+  opt->initialize(DblVec(static_cast<std::size_t>(prob->GetNumDOF()), 0));
   double tStart = GetClock();
   CONSOLE_BRIDGE_logDebug("Size: %d", opt->x().size());
   std::stringstream ss;
diff --git a/trajopt_common/include/trajopt_common/basic_array.hpp b/trajopt_common/include/trajopt_common/basic_array.hpp
index 1f971e24..d2995bd0 100644
--- a/trajopt_common/include/trajopt_common/basic_array.hpp
+++ b/trajopt_common/include/trajopt_common/basic_array.hpp
@@ -26,7 +26,7 @@ struct BasicArray
   {
     m_nRow = nRow;
     m_nCol = nCol;
-    m_data.resize(static_cast<size_t>(m_nRow) * static_cast<size_t>(m_nCol));
+    m_data.resize(static_cast<std::size_t>(m_nRow) * static_cast<std::size_t>(m_nCol));
   }
 
   int rows() const { return m_nRow; }
@@ -47,19 +47,19 @@ struct BasicArray
   }
   std::vector<T> rblock(int startRow, int startCol, int nCol) const
   {
-    std::vector<T> out(static_cast<size_t>(nCol));
+    std::vector<T> out(static_cast<std::size_t>(nCol));
     for (int iCol = 0; iCol < nCol; ++iCol)
     {
-      out[static_cast<size_t>(iCol)] = at(startRow, iCol + startCol);
+      out[static_cast<std::size_t>(iCol)] = at(startRow, iCol + startCol);
     }
     return out;
   }
   std::vector<T> cblock(int startRow, int startCol, int nRow) const
   {
-    std::vector<T> out(static_cast<size_t>(nRow));
+    std::vector<T> out(static_cast<std::size_t>(nRow));
     for (int iRow = 0; iRow < nRow; ++iRow)
     {
-      out[static_cast<size_t>(iRow)] = at(iRow + startRow, startCol);
+      out[static_cast<std::size_t>(iRow)] = at(iRow + startRow, startCol);
     }
     return out;
   }
@@ -80,19 +80,19 @@ struct BasicArray
   BasicArray bottomRows(int n) { return middleRows(m_nRow - n, n); }
   const T& at(int row, int col) const
   {
-    return m_data.at(static_cast<size_t>(row) * static_cast<size_t>(m_nCol) + static_cast<size_t>(col));
+    return m_data.at(static_cast<std::size_t>(row) * static_cast<std::size_t>(m_nCol) + static_cast<std::size_t>(col));
   }
   T& at(int row, int col)
   {
-    return m_data.at(static_cast<size_t>(row) * static_cast<size_t>(m_nCol) + static_cast<size_t>(col));
+    return m_data.at(static_cast<std::size_t>(row) * static_cast<std::size_t>(m_nCol) + static_cast<std::size_t>(col));
   }
   const T& operator()(int row, int col) const
   {
-    return m_data.at(static_cast<size_t>(row) * static_cast<size_t>(m_nCol) + static_cast<size_t>(col));
+    return m_data.at(static_cast<std::size_t>(row) * static_cast<std::size_t>(m_nCol) + static_cast<std::size_t>(col));
   }
   T& operator()(int row, int col)
   {
-    return m_data.at(static_cast<size_t>(row) * static_cast<size_t>(m_nCol) + static_cast<size_t>(col));
+    return m_data.at(static_cast<std::size_t>(row) * static_cast<std::size_t>(m_nCol) + static_cast<std::size_t>(col));
   }
   std::vector<T> col(int col)
   {
@@ -106,7 +106,7 @@ struct BasicArray
   std::vector<T> row(int row)
   {
     std::vector<T> out;
-    out.reserve(static_cast<size_t>(m_nCol));
+    out.reserve(static_cast<std::size_t>(m_nCol));
     for (int col = 0; col < m_nCol; col++)
       out.push_back(at(row, col));
     return out;
diff --git a/trajopt_common/include/trajopt_common/eigen_conversions.hpp b/trajopt_common/include/trajopt_common/eigen_conversions.hpp
index d44cdbde..5811180d 100644
--- a/trajopt_common/include/trajopt_common/eigen_conversions.hpp
+++ b/trajopt_common/include/trajopt_common/eigen_conversions.hpp
@@ -9,7 +9,7 @@ namespace trajopt_common
 {
 inline std::vector<double> toDblVec(const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>& x)
 {
-  return std::vector<double>(x.data(), x.data() + x.size());
+  return { x.data(), x.data() + x.size() };
 }
 inline Eigen::VectorXd toVectorXd(const std::vector<double>& x)
 {
diff --git a/trajopt_common/include/trajopt_common/eigen_slicing.hpp b/trajopt_common/include/trajopt_common/eigen_slicing.hpp
index 8797cfa7..1da31c5e 100644
--- a/trajopt_common/include/trajopt_common/eigen_slicing.hpp
+++ b/trajopt_common/include/trajopt_common/eigen_slicing.hpp
@@ -11,7 +11,7 @@ template <typename VectorT>
 VectorT fancySlice(const VectorT& x, const std::vector<int>& inds)
 {
   VectorT out(inds.size());
-  for (size_t i = 0; i < inds.size(); ++i)
+  for (std::size_t i = 0; i < inds.size(); ++i)
     out[i] = x[inds[i]];
   return out;
 }
diff --git a/trajopt_common/include/trajopt_common/stl_to_string.hpp b/trajopt_common/include/trajopt_common/stl_to_string.hpp
index 4c1daa70..108690a2 100644
--- a/trajopt_common/include/trajopt_common/stl_to_string.hpp
+++ b/trajopt_common/include/trajopt_common/stl_to_string.hpp
@@ -21,7 +21,7 @@ std::string Str(const std::vector<T>& x)
   ss << "(";
   if (!x.empty())
     ss << x[0];
-  for (size_t i = 1; i < x.size(); ++i)
+  for (std::size_t i = 1; i < x.size(); ++i)
     ss << ", " << x[i];
   ss << ")";
   return ss.str();
diff --git a/trajopt_common/include/trajopt_common/vector_ops.hpp b/trajopt_common/include/trajopt_common/vector_ops.hpp
index a379282c..425c2c2a 100644
--- a/trajopt_common/include/trajopt_common/vector_ops.hpp
+++ b/trajopt_common/include/trajopt_common/vector_ops.hpp
@@ -8,9 +8,9 @@ namespace trajopt_common
 {
 inline std::vector<int> arange(int n)
 {
-  std::vector<int> out(static_cast<size_t>(n));
+  std::vector<int> out(static_cast<std::size_t>(n));
   for (int i = 0; i < n; ++i)
-    out[static_cast<size_t>(i)] = i;
+    out[static_cast<std::size_t>(i)] = i;
   return out;
 }
 
diff --git a/trajopt_common/src/stl_to_string.cpp b/trajopt_common/src/stl_to_string.cpp
index 02af7b92..4868fe2b 100644
--- a/trajopt_common/src/stl_to_string.cpp
+++ b/trajopt_common/src/stl_to_string.cpp
@@ -9,7 +9,7 @@ std::string Str_impl(const std::vector<T>& x)
   ss << "(";
   if (!x.empty())
     ss << x[0];
-  for (size_t i = 1; i < x.size(); ++i)
+  for (std::size_t i = 1; i < x.size(); ++i)
     ss << ", " << x[i];
   ss << ")";
   return ss.str();
diff --git a/trajopt_common/src/test_logging.cpp b/trajopt_common/src/test_logging.cpp
deleted file mode 100644
index a991c81b..00000000
--- a/trajopt_common/src/test_logging.cpp
+++ /dev/null
@@ -1,12 +0,0 @@
-#include <trajopt_ros/utils/logging.hpp>
-
-int main()
-{
-  LOG_FATAL("fatal");
-  LOG_ERROR("error");
-  LOG_WARN("warn");
-  LOG_INFO("info");
-  LOG_DEBUG("debug");
-  LOG_TRACE("trace");
-  printf("hi\n");
-}
diff --git a/trajopt_common/src/utils.cpp b/trajopt_common/src/utils.cpp
index b709154b..3e3b8235 100644
--- a/trajopt_common/src/utils.cpp
+++ b/trajopt_common/src/utils.cpp
@@ -53,7 +53,7 @@ std::vector<SafetyMarginData::Ptr> createSafetyMarginDataVector(int num_elements
                                                                 double default_safety_margin_coeff)
 {
   std::vector<SafetyMarginData::Ptr> info;
-  info.reserve(static_cast<size_t>(num_elements));
+  info.reserve(static_cast<std::size_t>(num_elements));
   for (auto i = 0; i < num_elements; ++i)
     info.push_back(std::make_shared<SafetyMarginData>(default_safety_margin, default_safety_margin_coeff));
 
diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_evaluators.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_evaluators.h
index 3908a6f7..bd3011ce 100644
--- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_evaluators.h
+++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_evaluators.h
@@ -43,7 +43,7 @@ TRAJOPT_IGNORE_WARNINGS_POP
 namespace trajopt_ifopt
 {
 using GetStateFn = std::function<tesseract_common::TransformMap(const Eigen::Ref<const Eigen::VectorXd>& joint_values)>;
-using CollisionCache = trajopt_common::Cache<size_t, std::shared_ptr<const trajopt_common::CollisionCacheData>>;
+using CollisionCache = trajopt_common::Cache<std::size_t, std::shared_ptr<const trajopt_common::CollisionCacheData>>;
 
 /**
  * @brief This collision evaluator operates on two states and checks for collision between the two states using a
diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_evaluators.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_evaluators.h
index a8f3fddf..dc459304 100644
--- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_evaluators.h
+++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_evaluators.h
@@ -42,7 +42,7 @@ TRAJOPT_IGNORE_WARNINGS_POP
 namespace trajopt_ifopt
 {
 using GetStateFn = std::function<tesseract_common::TransformMap(const Eigen::Ref<const Eigen::VectorXd>& joint_values)>;
-using CollisionCache = trajopt_common::Cache<size_t, std::shared_ptr<const trajopt_common::CollisionCacheData>>;
+using CollisionCache = trajopt_common::Cache<std::size_t, std::shared_ptr<const trajopt_common::CollisionCacheData>>;
 
 /**
  * @brief This collision evaluator only operates on a single state in the trajectory and does not check for collisions
diff --git a/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp b/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp
index 219facf8..411185f9 100644
--- a/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp
+++ b/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp
@@ -92,7 +92,7 @@ LVSContinuousCollisionEvaluator::CalcCollisionData(const Eigen::Ref<const Eigen:
                                                    const std::array<bool, 2>& position_vars_fixed,
                                                    std::size_t bounds_size)
 {
-  size_t key = trajopt_common::getHash(*collision_config_, dof_vals0, dof_vals1);
+  std::size_t key = trajopt_common::getHash(*collision_config_, dof_vals0, dof_vals1);
   auto* it = collision_cache_->get(key);
   if (it != nullptr)
   {
@@ -330,7 +330,7 @@ LVSDiscreteCollisionEvaluator::CalcCollisionData(const Eigen::Ref<const Eigen::V
                                                  const std::array<bool, 2>& position_vars_fixed,
                                                  std::size_t bounds_size)
 {
-  size_t key = getHash(*collision_config_, dof_vals0, dof_vals1);
+  std::size_t key = getHash(*collision_config_, dof_vals0, dof_vals1);
   auto* it = collision_cache_->get(key);
   if (it != nullptr)
   {
diff --git a/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp b/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp
index 297fc43a..af9da85b 100644
--- a/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp
+++ b/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp
@@ -87,7 +87,7 @@ std::shared_ptr<const trajopt_common::CollisionCacheData>
 SingleTimestepCollisionEvaluator::CalcCollisions(const Eigen::Ref<const Eigen::VectorXd>& dof_vals,
                                                  std::size_t bounds_size)
 {
-  size_t key = getHash(*collision_config_, dof_vals);
+  std::size_t key = getHash(*collision_config_, dof_vals);
   auto* it = collision_cache_->get(key);
   if (it != nullptr)
   {
diff --git a/trajopt_ifopt/src/constraints/joint_acceleration_constraint.cpp b/trajopt_ifopt/src/constraints/joint_acceleration_constraint.cpp
index 3c64b350..7bd58d7b 100644
--- a/trajopt_ifopt/src/constraints/joint_acceleration_constraint.cpp
+++ b/trajopt_ifopt/src/constraints/joint_acceleration_constraint.cpp
@@ -67,14 +67,14 @@ JointAccelConstraint::JointAccelConstraint(const Eigen::VectorXd& targets,
     throw std::runtime_error("JointAccelConstraint, coeff must be the same size of the joint postion.");
 
   // Set the bounds to the input targets
-  std::vector<ifopt::Bounds> bounds(static_cast<size_t>(GetRows()));
+  std::vector<ifopt::Bounds> bounds(static_cast<std::size_t>(GetRows()));
   // All of the positions should be exactly at their targets
   for (long j = 0; j < n_vars_; j++)
   {
     index_map_[position_vars_[static_cast<std::size_t>(j)]->GetName()] = j;
     for (long i = 0; i < n_dof_; i++)
     {
-      bounds[static_cast<size_t>(i + j * n_dof_)] = ifopt::Bounds(targets[i], targets[i]);
+      bounds[static_cast<std::size_t>(i + j * n_dof_)] = ifopt::Bounds(targets[i], targets[i]);
     }
   }
   bounds_ = bounds;
@@ -82,7 +82,7 @@ JointAccelConstraint::JointAccelConstraint(const Eigen::VectorXd& targets,
 
 Eigen::VectorXd JointAccelConstraint::GetValues() const
 {
-  Eigen::VectorXd acceleration(static_cast<size_t>(n_dof_) * position_vars_.size());
+  Eigen::VectorXd acceleration(static_cast<std::size_t>(n_dof_) * position_vars_.size());
   // Forward Diff
   for (std::size_t ind = 0; ind < position_vars_.size() - 2; ind++)
   {
diff --git a/trajopt_ifopt/src/constraints/joint_jerk_constraint.cpp b/trajopt_ifopt/src/constraints/joint_jerk_constraint.cpp
index 317a3086..98dfcea2 100644
--- a/trajopt_ifopt/src/constraints/joint_jerk_constraint.cpp
+++ b/trajopt_ifopt/src/constraints/joint_jerk_constraint.cpp
@@ -67,14 +67,14 @@ JointJerkConstraint::JointJerkConstraint(const Eigen::VectorXd& targets,
     throw std::runtime_error("JointJerkConstraint, coeff must be the same size of the joint postion.");
 
   // Set the bounds to the input targets
-  std::vector<ifopt::Bounds> bounds(static_cast<size_t>(GetRows()));
+  std::vector<ifopt::Bounds> bounds(static_cast<std::size_t>(GetRows()));
   // All of the positions should be exactly at their targets
   for (long j = 0; j < n_vars_; j++)
   {
     index_map_[position_vars_[static_cast<std::size_t>(j)]->GetName()] = j;
     for (long i = 0; i < n_dof_; i++)
     {
-      bounds[static_cast<size_t>(i + j * n_dof_)] = ifopt::Bounds(targets[i], targets[i]);
+      bounds[static_cast<std::size_t>(i + j * n_dof_)] = ifopt::Bounds(targets[i], targets[i]);
     }
   }
   bounds_ = bounds;
@@ -82,7 +82,7 @@ JointJerkConstraint::JointJerkConstraint(const Eigen::VectorXd& targets,
 
 Eigen::VectorXd JointJerkConstraint::GetValues() const
 {
-  Eigen::VectorXd acceleration(static_cast<size_t>(n_dof_) * position_vars_.size());
+  Eigen::VectorXd acceleration(static_cast<std::size_t>(n_dof_) * position_vars_.size());
   // Forward Diff
   for (std::size_t ind = 0; ind < position_vars_.size() - 3; ind++)
   {
diff --git a/trajopt_ifopt/src/constraints/joint_position_constraint.cpp b/trajopt_ifopt/src/constraints/joint_position_constraint.cpp
index 81b6a3db..05d568e4 100644
--- a/trajopt_ifopt/src/constraints/joint_position_constraint.cpp
+++ b/trajopt_ifopt/src/constraints/joint_position_constraint.cpp
@@ -63,14 +63,14 @@ JointPosConstraint::JointPosConstraint(const Eigen::VectorXd& targets,
   }
 
   // Set the bounds to the input targets
-  std::vector<ifopt::Bounds> bounds(static_cast<size_t>(GetRows()));
+  std::vector<ifopt::Bounds> bounds(static_cast<std::size_t>(GetRows()));
   // All of the positions should be exactly at their targets
   for (long j = 0; j < n_vars_; j++)
   {
     for (long i = 0; i < n_dof_; i++)
     {
       double w_target = coeffs_[i] * targets[i];
-      bounds[static_cast<size_t>(i + j * n_dof_)] = ifopt::Bounds(w_target, w_target);
+      bounds[static_cast<std::size_t>(i + j * n_dof_)] = ifopt::Bounds(w_target, w_target);
     }
   }
   bounds_ = bounds;
@@ -111,7 +111,7 @@ JointPosConstraint::JointPosConstraint(const std::vector<ifopt::Bounds>& bounds,
 Eigen::VectorXd JointPosConstraint::GetValues() const
 {
   // Get the correct variables
-  Eigen::VectorXd values(static_cast<size_t>(n_dof_ * n_vars_));
+  Eigen::VectorXd values(static_cast<std::size_t>(n_dof_ * n_vars_));
   for (const auto& position_var : position_vars_)
     values << coeffs_.cwiseProduct(this->GetVariables()->GetComponent(position_var->GetName())->GetValues());
 
diff --git a/trajopt_ifopt/src/constraints/joint_velocity_constraint.cpp b/trajopt_ifopt/src/constraints/joint_velocity_constraint.cpp
index 5001dd80..77aee299 100644
--- a/trajopt_ifopt/src/constraints/joint_velocity_constraint.cpp
+++ b/trajopt_ifopt/src/constraints/joint_velocity_constraint.cpp
@@ -67,14 +67,14 @@ JointVelConstraint::JointVelConstraint(const Eigen::VectorXd& targets,
     throw std::runtime_error("JointVelConstraint, coeff must be the same size of the joint position.");
 
   // Set the bounds to the input targets
-  std::vector<ifopt::Bounds> bounds(static_cast<size_t>(GetRows()));
+  std::vector<ifopt::Bounds> bounds(static_cast<std::size_t>(GetRows()));
   // All of the positions should be exactly at their targets
   for (long j = 0; j < n_vars_ - 1; j++)
   {
     index_map_[position_vars_[static_cast<std::size_t>(j)]->GetName()] = j;
     for (long i = 0; i < n_dof_; i++)
     {
-      bounds[static_cast<size_t>(i + j * n_dof_)] = ifopt::Bounds(targets[i], targets[i]);
+      bounds[static_cast<std::size_t>(i + j * n_dof_)] = ifopt::Bounds(targets[i], targets[i]);
     }
   }
   index_map_[position_vars_.back()->GetName()] = (n_vars_ - 1);
@@ -90,7 +90,7 @@ Eigen::VectorXd JointVelConstraint::GetValues() const
   // vel(var[1, 1]) - represents the joint velocity of DOF index 1 at timestep 1
   //
   // Velocity V = vel(var[0, 0]), vel(var[0, 1]), vel(var[0, 2]), vel(var[1, 0]), vel(var[1, 1]), vel(var[1, 2]), etc
-  Eigen::VectorXd velocity(static_cast<size_t>(n_dof_) * (static_cast<size_t>(n_vars_) - 1));
+  Eigen::VectorXd velocity(static_cast<std::size_t>(n_dof_) * (static_cast<std::size_t>(n_vars_) - 1));
 
   // Forward differentiation for the first point
   for (std::size_t ind = 0; ind < position_vars_.size() - 1; ind++)
diff --git a/trajopt_ifopt/src/variable_sets/joint_position_variable.cpp b/trajopt_ifopt/src/variable_sets/joint_position_variable.cpp
index ec58bb79..c9300b22 100644
--- a/trajopt_ifopt/src/variable_sets/joint_position_variable.cpp
+++ b/trajopt_ifopt/src/variable_sets/joint_position_variable.cpp
@@ -38,7 +38,7 @@ JointPosition::JointPosition(const Eigen::Ref<const Eigen::VectorXd>& init_value
                              std::vector<std::string> joint_names,
                              const std::string& name)
   : ifopt::VariableSet(static_cast<int>(init_value.size()), name)
-  , bounds_(std::vector<ifopt::Bounds>(static_cast<size_t>(init_value.size()), ifopt::NoBound))
+  , bounds_(std::vector<ifopt::Bounds>(static_cast<std::size_t>(init_value.size()), ifopt::NoBound))
   , values_(init_value)
   , joint_names_(std::move(joint_names))
 {
@@ -49,7 +49,7 @@ JointPosition::JointPosition(const Eigen::Ref<const Eigen::VectorXd>& init_value
                              const ifopt::Bounds& bounds,
                              const std::string& name)
   : ifopt::VariableSet(static_cast<int>(init_value.size()), name)
-  , bounds_(std::vector<ifopt::Bounds>(static_cast<size_t>(init_value.size()), bounds))
+  , bounds_(std::vector<ifopt::Bounds>(static_cast<std::size_t>(init_value.size()), bounds))
   , joint_names_(std::move(joint_names))
 {
   /** @todo Print warning if init value is not within bounds */
@@ -67,7 +67,7 @@ JointPosition::JointPosition(const Eigen::Ref<const Eigen::VectorXd>& init_value
                              const tesseract_common::KinematicLimits& bounds,
                              const std::string& name)
   : ifopt::VariableSet(static_cast<int>(init_value.size()), name)
-  , bounds_(std::vector<ifopt::Bounds>(static_cast<size_t>(init_value.size()), ifopt::NoBound))
+  , bounds_(std::vector<ifopt::Bounds>(static_cast<std::size_t>(init_value.size()), ifopt::NoBound))
   , joint_names_(std::move(joint_names))
 {
   /** @todo Print warning if init value is not within bounds */
diff --git a/trajopt_ifopt/test/cartesian_line_constraint_unit.cpp b/trajopt_ifopt/test/cartesian_line_constraint_unit.cpp
index e14a0e84..6802a642 100644
--- a/trajopt_ifopt/test/cartesian_line_constraint_unit.cpp
+++ b/trajopt_ifopt/test/cartesian_line_constraint_unit.cpp
@@ -183,7 +183,7 @@ TEST_F(CartesianLineConstraintUnit, GetSetBounds)  // NOLINT
 
   constraint->SetBounds(bounds_vec);
   std::vector<ifopt::Bounds> results_vec = constraint->GetBounds();
-  for (size_t i = 0; i < bounds_vec.size(); i++)
+  for (std::size_t i = 0; i < bounds_vec.size(); i++)
   {
     EXPECT_EQ(bounds_vec[i].lower_, results_vec[i].lower_);
     EXPECT_EQ(bounds_vec[i].upper_, results_vec[i].upper_);
diff --git a/trajopt_ifopt/test/cartesian_position_constraint_unit.cpp b/trajopt_ifopt/test/cartesian_position_constraint_unit.cpp
index 85d3692d..ed0752fc 100644
--- a/trajopt_ifopt/test/cartesian_position_constraint_unit.cpp
+++ b/trajopt_ifopt/test/cartesian_position_constraint_unit.cpp
@@ -200,7 +200,7 @@ TEST_F(CartesianPositionConstraintUnit, GetSetBounds)  // NOLINT
 
     constraint_2->SetBounds(bounds_vec);
     std::vector<ifopt::Bounds> results_vec = constraint_2->GetBounds();
-    for (size_t i = 0; i < bounds_vec.size(); i++)
+    for (std::size_t i = 0; i < bounds_vec.size(); i++)
     {
       EXPECT_EQ(bounds_vec[i].lower_, results_vec[i].lower_);
       EXPECT_EQ(bounds_vec[i].upper_, results_vec[i].upper_);
diff --git a/trajopt_ifopt/test/collision_unit.cpp b/trajopt_ifopt/test/collision_unit.cpp
index 3c33270e..ce84e5ef 100644
--- a/trajopt_ifopt/test/collision_unit.cpp
+++ b/trajopt_ifopt/test/collision_unit.cpp
@@ -179,7 +179,7 @@ TEST_F(CollisionUnit, GetSetBounds)  // NOLINT
     std::vector<ifopt::Bounds> bounds_vec = std::vector<ifopt::Bounds>(1, bounds);
     constraint_2->SetBounds(bounds_vec);
     std::vector<ifopt::Bounds> results_vec = constraint_2->GetBounds();
-    for (size_t i = 0; i < bounds_vec.size(); i++)
+    for (std::size_t i = 0; i < bounds_vec.size(); i++)
     {
       EXPECT_EQ(bounds_vec[i].lower_, results_vec[i].lower_);
       EXPECT_EQ(bounds_vec[i].upper_, results_vec[i].upper_);
diff --git a/trajopt_ifopt/test/inverse_kinematics_constraint_unit.cpp b/trajopt_ifopt/test/inverse_kinematics_constraint_unit.cpp
index 4a3cb088..a903d016 100644
--- a/trajopt_ifopt/test/inverse_kinematics_constraint_unit.cpp
+++ b/trajopt_ifopt/test/inverse_kinematics_constraint_unit.cpp
@@ -160,7 +160,7 @@ TEST_F(InverseKinematicsConstraintUnit, GetSetBounds)  // NOLINT
 
     constraint_2->SetBounds(bounds_vec);
     std::vector<ifopt::Bounds> results_vec = constraint_2->GetBounds();
-    for (size_t i = 0; i < bounds_vec.size(); i++)
+    for (std::size_t i = 0; i < bounds_vec.size(); i++)
     {
       EXPECT_EQ(bounds_vec[i].lower_, results_vec[i].lower_);
       EXPECT_EQ(bounds_vec[i].upper_, results_vec[i].upper_);
diff --git a/trajopt_ifopt/test/variable_sets_unit.cpp b/trajopt_ifopt/test/variable_sets_unit.cpp
index 71e86f7b..f714868d 100644
--- a/trajopt_ifopt/test/variable_sets_unit.cpp
+++ b/trajopt_ifopt/test/variable_sets_unit.cpp
@@ -61,10 +61,10 @@ TEST(VariableSetsUnit, joint_position_1)  // NOLINT
 
   // Check that setting bounds works
   ifopt::Bounds bounds(-0.1234, 0.5678);
-  std::vector<ifopt::Bounds> bounds_vec = std::vector<ifopt::Bounds>(static_cast<size_t>(init.size()), bounds);
+  std::vector<ifopt::Bounds> bounds_vec = std::vector<ifopt::Bounds>(static_cast<std::size_t>(init.size()), bounds);
   position_var.SetBounds(bounds_vec);
   std::vector<ifopt::Bounds> results_vec = position_var.GetBounds();
-  for (size_t i = 0; i < bounds_vec.size(); i++)
+  for (std::size_t i = 0; i < bounds_vec.size(); i++)
   {
     EXPECT_EQ(bounds_vec[i].lower_, results_vec[i].lower_);
     EXPECT_EQ(bounds_vec[i].upper_, results_vec[i].upper_);
diff --git a/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp b/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp
index 81135583..271f4070 100644
--- a/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp
+++ b/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp
@@ -1,10 +1,8 @@
 #pragma once
 #include <trajopt_common/macros.h>
 TRAJOPT_IGNORE_WARNINGS_PUSH
-#include <string>
 #include <vector>
 #include <cassert>
-#include <iostream>
 #include <cstdlib>
 #include <unistd.h>
 TRAJOPT_IGNORE_WARNINGS_POP
diff --git a/trajopt_sco/include/trajopt_sco/gurobi_interface.hpp b/trajopt_sco/include/trajopt_sco/gurobi_interface.hpp
index 71ed10ad..c90bfbcc 100644
--- a/trajopt_sco/include/trajopt_sco/gurobi_interface.hpp
+++ b/trajopt_sco/include/trajopt_sco/gurobi_interface.hpp
@@ -29,7 +29,7 @@ class GurobiModel : public Model
   std::mutex m_mutex;
 
   GurobiModel();
-  ~GurobiModel();
+  ~GurobiModel() override;
 
   // Must be threadsafe
   Var addVar(const std::string& name) override;
diff --git a/trajopt_sco/include/trajopt_sco/modeling.hpp b/trajopt_sco/include/trajopt_sco/modeling.hpp
index 332879ba..b67eed8d 100644
--- a/trajopt_sco/include/trajopt_sco/modeling.hpp
+++ b/trajopt_sco/include/trajopt_sco/modeling.hpp
@@ -270,9 +270,9 @@ template <typename VecType>
 inline void setVec(DblVec& x, const VarVector& vars, const VecType& vals)
 {
   assert(vars.size() == vals.size());
-  for (size_t i = 0; i < vars.size(); ++i)
+  for (std::size_t i = 0; i < vars.size(); ++i)
   {
-    x[static_cast<size_t>(vars[i].var_rep->index)] = vals[i];
+    x[static_cast<std::size_t>(vars[i].var_rep->index)] = vals[i];
   }
 }
 template <typename OutVecType>
diff --git a/trajopt_sco/include/trajopt_sco/solver_interface.hpp b/trajopt_sco/include/trajopt_sco/solver_interface.hpp
index b2aaa841..f1e8d59c 100644
--- a/trajopt_sco/include/trajopt_sco/solver_interface.hpp
+++ b/trajopt_sco/include/trajopt_sco/solver_interface.hpp
@@ -4,7 +4,6 @@
 TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <cassert>
 #include <iosfwd>
-#include <limits>
 #include <string>
 #include <vector>
 #include <memory>
@@ -141,7 +140,7 @@ struct Var
   double value(const DblVec& x) const
   {
     assert(var_rep->index < x.size());
-    return x[static_cast<size_t>(var_rep->index)];
+    return x[static_cast<std::size_t>(var_rep->index)];
   }
 };
 
@@ -196,7 +195,7 @@ struct AffExpr
   explicit AffExpr(double a);
   explicit AffExpr(const Var& v);
 
-  size_t size() const;
+  std::size_t size() const;
   double value(const double* x) const;
   double value(const DblVec& x) const;
 };
@@ -213,7 +212,7 @@ struct QuadExpr
   explicit QuadExpr(double a);
   explicit QuadExpr(const Var& v);
   explicit QuadExpr(AffExpr aff);
-  size_t size() const;
+  std::size_t size() const;
   double value(const double* x) const;
   double value(const DblVec& x) const;
 };
diff --git a/trajopt_sco/include/trajopt_sco/solver_utils.hpp b/trajopt_sco/include/trajopt_sco/solver_utils.hpp
index 1c55b478..e25ae2e4 100644
--- a/trajopt_sco/include/trajopt_sco/solver_utils.hpp
+++ b/trajopt_sco/include/trajopt_sco/solver_utils.hpp
@@ -3,7 +3,6 @@
 TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <Eigen/Core>
 #include <Eigen/SparseCore>
-#include <iostream>
 TRAJOPT_IGNORE_WARNINGS_POP
 
 #include <trajopt_sco/solver_interface.hpp>
diff --git a/trajopt_sco/src/bpmpd_interface.cpp b/trajopt_sco/src/bpmpd_interface.cpp
index e37ce4ff..1629de88 100644
--- a/trajopt_sco/src/bpmpd_interface.cpp
+++ b/trajopt_sco/src/bpmpd_interface.cpp
@@ -284,7 +284,7 @@ void BPMPDModel::removeCnts(const CntVector& cnts)
 void BPMPDModel::update()
 {
   {
-    size_t inew = 0;
+    std::size_t inew = 0;
     for (unsigned iold = 0; iold < m_vars.size(); ++iold)
     {
       Var& var = m_vars[iold];
@@ -306,7 +306,7 @@ void BPMPDModel::update()
     m_ubs.resize(inew);
   }
   {
-    size_t inew = 0;
+    std::size_t inew = 0;
     for (unsigned iold = 0; iold < m_cnts.size(); ++iold)
     {
       Cnt& cnt = m_cnts[iold];
@@ -333,7 +333,7 @@ void BPMPDModel::setVarBounds(const VarVector& vars, const DblVec& lower, const
 {
   for (unsigned i = 0; i < vars.size(); ++i)
   {
-    auto varind = static_cast<size_t>(vars[i].var_rep->index);
+    auto varind = static_cast<std::size_t>(vars[i].var_rep->index);
     m_lbs[varind] = lower[i];
     m_ubs[varind] = upper[i];
   }
@@ -343,7 +343,7 @@ DblVec BPMPDModel::getVarValues(const VarVector& vars) const
   DblVec out(vars.size());
   for (unsigned i = 0; i < vars.size(); ++i)
   {
-    auto varind = static_cast<size_t>(vars[i].var_rep->index);
+    auto varind = static_cast<std::size_t>(vars[i].var_rep->index);
     out[i] = m_soln[varind];
   }
   return out;
@@ -363,15 +363,15 @@ CvxOptStatus BPMPDModel::optimize()
   // lbound[maxn+maxm],
   //        ubound[maxn+maxm], primal[maxn+maxm], dual[maxn+maxm], big, opt;
 
-  size_t n = m_vars.size();
-  size_t m = m_cnts.size();
+  std::size_t n = m_vars.size();
+  std::size_t m = m_cnts.size();
 
   IntVec acolcnt(n), acolidx, qcolcnt(n), qcolidx, status(m + n);
   DblVec acolnzs, qcolnzs, rhs(m), obj(n, 0), lbound(m + n), ubound(m + n), primal(m + n), dual(m + n);
 
   DBG(m_lbs);
   DBG(m_ubs);
-  for (size_t iVar = 0; iVar < n; ++iVar)
+  for (std::size_t iVar = 0; iVar < n; ++iVar)
   {
     lbound[iVar] = fmax(m_lbs[iVar], -BPMPD_BIG);
     ubound[iVar] = fmin(m_ubs[iVar], BPMPD_BIG);
@@ -379,17 +379,17 @@ CvxOptStatus BPMPDModel::optimize()
 
   std::vector<IntVec> var2cntinds(n);
   std::vector<DblVec> var2cntvals(n);
-  for (size_t iCnt = 0; iCnt < m; ++iCnt)
+  for (std::size_t iCnt = 0; iCnt < m; ++iCnt)
   {
     const AffExpr& aff = m_cntExprs[iCnt];
     // cout << "adding constraint " << aff << endl;
     SizeTVec inds;
     vars2inds(aff.vars, inds);
 
-    for (size_t i = 0; i < aff.vars.size(); ++i)
+    for (std::size_t i = 0; i < aff.vars.size(); ++i)
     {
-      var2cntinds[static_cast<size_t>(inds[i])].push_back(static_cast<int>(iCnt));
-      var2cntvals[static_cast<size_t>(inds[i])].push_back(aff.coeffs[i]);  // xxx maybe repeated/
+      var2cntinds[static_cast<std::size_t>(inds[i])].push_back(static_cast<int>(iCnt));
+      var2cntvals[static_cast<std::size_t>(inds[i])].push_back(aff.coeffs[i]);  // xxx maybe repeated/
     }
 
     lbound[n + iCnt] = (m_cntTypes[iCnt] == INEQ) ? -BPMPD_BIG : 0;
@@ -397,7 +397,7 @@ CvxOptStatus BPMPDModel::optimize()
     rhs[iCnt] = -aff.constant;
   }
 
-  for (size_t iVar = 0; iVar < n; ++iVar)
+  for (std::size_t iVar = 0; iVar < n; ++iVar)
   {
     simplify2(var2cntinds[iVar], var2cntvals[iVar]);
     acolcnt[iVar] = static_cast<int>(var2cntinds[iVar].size());
@@ -409,10 +409,10 @@ CvxOptStatus BPMPDModel::optimize()
 
   std::vector<DblVec> var2qcoeffs(n);
   std::vector<IntVec> var2qinds(n);
-  for (size_t i = 0; i < m_objective.size(); ++i)
+  for (std::size_t i = 0; i < m_objective.size(); ++i)
   {
-    auto idx1 = static_cast<size_t>(m_objective.vars1[i].var_rep->index);
-    auto idx2 = static_cast<size_t>(m_objective.vars2[i].var_rep->index);
+    auto idx1 = static_cast<std::size_t>(m_objective.vars1[i].var_rep->index);
+    auto idx2 = static_cast<std::size_t>(m_objective.vars2[i].var_rep->index);
     if (idx1 < idx2)
     {
       var2qinds[idx1].push_back(static_cast<int>(idx2));
@@ -430,7 +430,7 @@ CvxOptStatus BPMPDModel::optimize()
     }
   }
 
-  for (size_t iVar = 0; iVar < n; ++iVar)
+  for (std::size_t iVar = 0; iVar < n; ++iVar)
   {
     simplify2(var2qinds[iVar], var2qcoeffs[iVar]);
     qcolidx.insert(qcolidx.end(), var2qinds[iVar].begin(), var2qinds[iVar].end());
@@ -438,9 +438,9 @@ CvxOptStatus BPMPDModel::optimize()
     qcolcnt[iVar] = static_cast<int>(var2qinds[iVar].size());
   }
 
-  for (size_t i = 0; i < m_objective.affexpr.size(); ++i)
+  for (std::size_t i = 0; i < m_objective.affexpr.size(); ++i)
   {
-    obj[static_cast<size_t>(m_objective.affexpr.vars[i].var_rep->index)] += m_objective.affexpr.coeffs[i];
+    obj[static_cast<std::size_t>(m_objective.affexpr.vars[i].var_rep->index)] += m_objective.affexpr.coeffs[i];
   }
 
 #define VECINC(vec)                                                                                                    \
diff --git a/trajopt_sco/src/expr_ops.cpp b/trajopt_sco/src/expr_ops.cpp
index 8ddaf29a..d4082368 100644
--- a/trajopt_sco/src/expr_ops.cpp
+++ b/trajopt_sco/src/expr_ops.cpp
@@ -10,9 +10,9 @@ namespace sco
 QuadExpr exprMult(const AffExpr& affexpr1, const AffExpr& affexpr2)
 {
   QuadExpr out;
-  size_t naff1 = affexpr1.coeffs.size();
-  size_t naff2 = affexpr2.coeffs.size();
-  size_t nquad = naff1 * naff2;
+  std::size_t naff1 = affexpr1.coeffs.size();
+  std::size_t naff2 = affexpr2.coeffs.size();
+  std::size_t nquad = naff1 * naff2;
 
   // Multiply the constants of the two expr
   out.affexpr.constant = affexpr1.constant * affexpr2.constant;
@@ -22,18 +22,18 @@ QuadExpr exprMult(const AffExpr& affexpr1, const AffExpr& affexpr2)
   out.affexpr.vars.insert(out.affexpr.vars.end(), affexpr1.vars.begin(), affexpr1.vars.end());
   out.affexpr.vars.insert(out.affexpr.vars.end(), affexpr2.vars.begin(), affexpr2.vars.end());
   out.affexpr.coeffs.resize(naff1 + naff2);
-  for (size_t i = 0; i < naff1; ++i)
+  for (std::size_t i = 0; i < naff1; ++i)
     out.affexpr.coeffs[i] = affexpr2.constant * affexpr1.coeffs[i];
-  for (size_t i = 0; i < naff2; ++i)
+  for (std::size_t i = 0; i < naff2; ++i)
     out.affexpr.coeffs[i + naff1] = affexpr1.constant * affexpr2.coeffs[i];
 
   // Account for the vars in each expr that are multipled by another var in the other expr
   out.coeffs.reserve(nquad);
   out.vars1.reserve(nquad);
   out.vars2.reserve(nquad);
-  for (size_t i = 0; i < naff1; ++i)
+  for (std::size_t i = 0; i < naff1; ++i)
   {
-    for (size_t j = 0; j < naff2; ++j)
+    for (std::size_t j = 0; j < naff2; ++j)
     {
       out.vars1.push_back(affexpr1.vars[i]);
       out.vars2.push_back(affexpr2.vars[j]);
@@ -55,25 +55,25 @@ QuadExpr exprSquare(const Var& a)
 QuadExpr exprSquare(const AffExpr& affexpr)
 {
   QuadExpr out;
-  size_t naff = affexpr.coeffs.size();
-  size_t nquad = (naff * (naff + 1)) / 2;
+  std::size_t naff = affexpr.coeffs.size();
+  std::size_t nquad = (naff * (naff + 1)) / 2;
 
   out.affexpr.constant = sq(affexpr.constant);
 
   out.affexpr.vars = affexpr.vars;
   out.affexpr.coeffs.resize(naff);
-  for (size_t i = 0; i < naff; ++i)
+  for (std::size_t i = 0; i < naff; ++i)
     out.affexpr.coeffs[i] = 2 * affexpr.constant * affexpr.coeffs[i];
 
   out.coeffs.reserve(nquad);
   out.vars1.reserve(nquad);
   out.vars2.reserve(nquad);
-  for (size_t i = 0; i < naff; ++i)
+  for (std::size_t i = 0; i < naff; ++i)
   {
     out.vars1.push_back(affexpr.vars[i]);
     out.vars2.push_back(affexpr.vars[i]);
     out.coeffs.push_back(sq(affexpr.coeffs[i]));
-    for (size_t j = i + 1; j < naff; ++j)
+    for (std::size_t j = i + 1; j < naff; ++j)
     {
       out.vars1.push_back(affexpr.vars[i]);
       out.vars2.push_back(affexpr.vars[j]);
@@ -86,7 +86,7 @@ QuadExpr exprSquare(const AffExpr& affexpr)
 AffExpr cleanupAff(const AffExpr& a)
 {
   AffExpr out;
-  for (size_t i = 0; i < a.size(); ++i)
+  for (std::size_t i = 0; i < a.size(); ++i)
   {
     if (fabs(a.coeffs[i]) > 1e-7)
     {
diff --git a/trajopt_sco/src/gurobi_interface.cpp b/trajopt_sco/src/gurobi_interface.cpp
index fa81571a..5a83186a 100644
--- a/trajopt_sco/src/gurobi_interface.cpp
+++ b/trajopt_sco/src/gurobi_interface.cpp
@@ -3,11 +3,6 @@ TRAJOPT_IGNORE_WARNINGS_PUSH
 extern "C" {
 #include "gurobi_c.h"
 }
-#include <iostream>
-#include <map>
-#include <sstream>
-#include <stdexcept>
-#include <utility>
 TRAJOPT_IGNORE_WARNINGS_POP
 
 #include <trajopt_sco/gurobi_interface.hpp>
@@ -161,12 +156,12 @@ Cnt GurobiModel::addIneqCnt(const QuadExpr& qexpr, const std::string& name)
 
 void resetIndices(VarVector& vars)
 {
-  for (size_t i = 0; i < vars.size(); ++i)
+  for (std::size_t i = 0; i < vars.size(); ++i)
     vars[i].var_rep->index = i;
 }
 void resetIndices(CntVector& cnts)
 {
-  for (size_t i = 0; i < cnts.size(); ++i)
+  for (std::size_t i = 0; i < cnts.size(); ++i)
     cnts[i].cnt_rep->index = i;
 }
 
@@ -176,7 +171,7 @@ void GurobiModel::removeVars(const VarVector& vars)
   IntVec inds;
   vars2inds(vars, inds);
   ENSURE_SUCCESS(GRBdelvars(m_model, static_cast<int>(inds.size()), inds.data()));
-  for (size_t i = 0; i < vars.size(); ++i)
+  for (std::size_t i = 0; i < vars.size(); ++i)
     vars[i].var_rep->removed = true;
 }
 
@@ -186,7 +181,7 @@ void GurobiModel::removeCnts(const CntVector& cnts)
   IntVec inds;
   cnts2inds(cnts, inds);
   ENSURE_SUCCESS(GRBdelconstrs(m_model, static_cast<int>(inds.size()), inds.data()));
-  for (size_t i = 0; i < cnts.size(); ++i)
+  for (std::size_t i = 0; i < cnts.size(); ++i)
     cnts[i].cnt_rep->removed = true;
 }
 
@@ -264,8 +259,8 @@ void GurobiModel::setObjective(const AffExpr& expr)
   GRBgetintattr(m_model, GRB_INT_ATTR_NUMVARS, &nvars);
   assert(nvars == static_cast<int>(m_vars.size()));
 
-  DblVec obj(static_cast<size_t>(nvars), 0);
-  for (size_t i = 0; i < expr.size(); ++i)
+  DblVec obj(static_cast<std::size_t>(nvars), 0);
+  for (std::size_t i = 0; i < expr.size(); ++i)
   {
     obj[expr.vars[i].var_rep->index] += expr.coeffs[i];
   }
@@ -293,7 +288,7 @@ void GurobiModel::update()
   ENSURE_SUCCESS(GRBupdatemodel(m_model));
 
   {
-    size_t inew = 0;
+    std::size_t inew = 0;
     for (Var& var : m_vars)
     {
       if (!var.var_rep->removed)
@@ -310,7 +305,7 @@ void GurobiModel::update()
     m_vars.resize(inew);
   }
   {
-    size_t inew = 0;
+    std::size_t inew = 0;
     for (Cnt& cnt : m_cnts)
     {
       if (!cnt.cnt_rep->removed)
diff --git a/trajopt_sco/src/modeling.cpp b/trajopt_sco/src/modeling.cpp
index 9282209b..f0e23a8c 100644
--- a/trajopt_sco/src/modeling.cpp
+++ b/trajopt_sco/src/modeling.cpp
@@ -154,12 +154,12 @@ DblVec Constraint::violations(const DblVec& x)
 
   if (type() == EQ)
   {
-    for (size_t i = 0; i < val.size(); ++i)
+    for (std::size_t i = 0; i < val.size(); ++i)
       out[i] = fabs(val[i]);
   }
   else
   {  // type() == INEQ
-    for (size_t i = 0; i < val.size(); ++i)
+    for (std::size_t i = 0; i < val.size(); ++i)
       out[i] = pospart(val[i]);
   }
 
@@ -179,13 +179,13 @@ VarVector OptProb::createVariables(const std::vector<std::string>& names)
 
 VarVector OptProb::createVariables(const std::vector<std::string>& names, const DblVec& lb, const DblVec& ub)
 {
-  size_t n_add = names.size(), n_cur = vars_.size();
+  std::size_t n_add = names.size(), n_cur = vars_.size();
   assert(lb.size() == n_add);
   assert(ub.size() == n_add);
   vars_.reserve(n_cur + n_add);
   lower_bounds_.reserve(n_cur + n_add);
   upper_bounds_.reserve(n_cur + n_add);
-  for (size_t i = 0; i < names.size(); ++i)
+  for (std::size_t i = 0; i < names.size(); ++i)
   {
     vars_.push_back(model_->addVar(names[i], lb[i], ub[i]));
     lower_bounds_.push_back(lb[i]);
diff --git a/trajopt_sco/src/modeling_utils.cpp b/trajopt_sco/src/modeling_utils.cpp
index a0ad105b..71efc11c 100644
--- a/trajopt_sco/src/modeling_utils.cpp
+++ b/trajopt_sco/src/modeling_utils.cpp
@@ -90,19 +90,19 @@ ConvexObjective::Ptr CostFromFunc::convex(const DblVec& x, Model* model)
     quad.affexpr.vars = vars_;
     quad.affexpr.coeffs = trajopt_common::toDblVec(grad - pos_hess * x_eigen);
 
-    auto nquadterms = static_cast<size_t>((x_eigen.size() * (x_eigen.size() - 1)) / 2);
+    auto nquadterms = static_cast<std::size_t>((x_eigen.size() * (x_eigen.size() - 1)) / 2);
     quad.coeffs.reserve(nquadterms);
     quad.vars1.reserve(nquadterms);
     quad.vars2.reserve(nquadterms);
     for (long int i = 0, end = x_eigen.size(); i != end; ++i)
     {  // tricky --- eigen size() is signed
-      quad.vars1.push_back(vars_[static_cast<size_t>(i)]);
-      quad.vars2.push_back(vars_[static_cast<size_t>(i)]);
+      quad.vars1.push_back(vars_[static_cast<std::size_t>(i)]);
+      quad.vars2.push_back(vars_[static_cast<std::size_t>(i)]);
       quad.coeffs.push_back(pos_hess(i, i) / 2);
       for (long int j = i + 1; j != end; ++j)
       {  // tricky --- eigen size() is signed
-        quad.vars1.push_back(vars_[static_cast<size_t>(i)]);
-        quad.vars2.push_back(vars_[static_cast<size_t>(j)]);
+        quad.vars1.push_back(vars_[static_cast<std::size_t>(i)]);
+        quad.vars2.push_back(vars_[static_cast<std::size_t>(j)]);
         quad.coeffs.push_back(pos_hess(i, j));
       }
     }
@@ -270,7 +270,7 @@ ConvexConstraints::Ptr ConstraintFromErrFunc::convex(const DblVec& x, Model* mod
 std::string AffExprToString(const AffExpr& aff)
 {
   std::string out;
-  for (size_t i = 0; i < aff.vars.size(); i++)
+  for (std::size_t i = 0; i < aff.vars.size(); i++)
   {
     if (i != 0)
       out.append(" + ");
diff --git a/trajopt_sco/src/optimizers.cpp b/trajopt_sco/src/optimizers.cpp
index a47d3632..45d92c60 100644
--- a/trajopt_sco/src/optimizers.cpp
+++ b/trajopt_sco/src/optimizers.cpp
@@ -93,7 +93,7 @@ void BasicTrustRegionSQP::setTrustBoxConstraints(const DblVec& x)
   assert(vars.size() == x.size());
   const DblVec &lb = prob_->getLowerBounds(), ub = prob_->getUpperBounds();
   DblVec lbtrust(x.size()), ubtrust(x.size());
-  for (size_t i = 0; i < x.size(); ++i)
+  for (std::size_t i = 0; i < x.size(); ++i)
   {
     lbtrust[i] = fmax(x[i] - param_.trust_box_size, lb[i]);
     ubtrust[i] = fmin(x[i] + param_.trust_box_size, ub[i]);
@@ -108,7 +108,7 @@ void BasicTrustRegionSQP::setTrustBoxConstraints(const DblVec& x)
 DblVec BasicTrustRegionSQP::evaluateCosts(const std::vector<Cost::Ptr>& costs, const DblVec& x) const
 {
   DblVec out(costs.size());
-  for (size_t i = 0; i < costs.size(); ++i)
+  for (std::size_t i = 0; i < costs.size(); ++i)
     out[i] = costs[i]->value(x);
 
   return out;
@@ -117,7 +117,7 @@ DblVec BasicTrustRegionSQP::evaluateCosts(const std::vector<Cost::Ptr>& costs, c
 DblVec BasicTrustRegionSQP::evaluateConstraintViols(const std::vector<Constraint::Ptr>& cnts, const DblVec& x) const
 {
   DblVec out(cnts.size());
-  for (size_t i = 0; i < cnts.size(); ++i)
+  for (std::size_t i = 0; i < cnts.size(); ++i)
     out[i] = cnts[i]->violation(x);
 
   return out;
@@ -128,7 +128,7 @@ std::vector<ConvexObjective::Ptr> BasicTrustRegionSQP::convexifyCosts(const std:
                                                                       Model* model) const
 {
   std::vector<ConvexObjective::Ptr> out(costs.size());
-  for (size_t i = 0; i < costs.size(); ++i)
+  for (std::size_t i = 0; i < costs.size(); ++i)
     out[i] = costs[i]->convex(x, model);
 
   return out;
@@ -139,7 +139,7 @@ std::vector<ConvexConstraints::Ptr> BasicTrustRegionSQP::convexifyConstraints(co
                                                                               Model* model) const
 {
   std::vector<ConvexConstraints::Ptr> out(cnts.size());
-  for (size_t i = 0; i < cnts.size(); ++i)
+  for (std::size_t i = 0; i < cnts.size(); ++i)
     out[i] = cnts[i]->convex(x, model);
 
   return out;
@@ -148,7 +148,7 @@ std::vector<ConvexConstraints::Ptr> BasicTrustRegionSQP::convexifyConstraints(co
 DblVec BasicTrustRegionSQP::evaluateModelCosts(const std::vector<ConvexObjective::Ptr>& costs, const DblVec& x) const
 {
   DblVec out(costs.size());
-  for (size_t i = 0; i < costs.size(); ++i)
+  for (std::size_t i = 0; i < costs.size(); ++i)
     out[i] = costs[i]->value(x);
 
   return out;
@@ -158,7 +158,7 @@ DblVec BasicTrustRegionSQP::evaluateModelCntViols(const std::vector<ConvexConstr
                                                   const DblVec& x) const
 {
   DblVec out(cnts.size());
-  for (size_t i = 0; i < cnts.size(); ++i)
+  for (std::size_t i = 0; i < cnts.size(); ++i)
     out[i] = cnts[i]->violation(x);
 
   return out;
@@ -167,7 +167,7 @@ DblVec BasicTrustRegionSQP::evaluateModelCntViols(const std::vector<ConvexConstr
 std::vector<std::string> BasicTrustRegionSQP::getCostNames(const std::vector<Cost::Ptr>& costs) const
 {
   std::vector<std::string> out(costs.size());
-  for (size_t i = 0; i < costs.size(); ++i)
+  for (std::size_t i = 0; i < costs.size(); ++i)
     out[i] = costs[i]->name();
 
   return out;
@@ -176,7 +176,7 @@ std::vector<std::string> BasicTrustRegionSQP::getCostNames(const std::vector<Cos
 std::vector<std::string> BasicTrustRegionSQP::getCntNames(const std::vector<Constraint::Ptr>& cnts) const
 {
   std::vector<std::string> out(cnts.size());
-  for (size_t i = 0; i < cnts.size(); ++i)
+  for (std::size_t i = 0; i < cnts.size(); ++i)
     out[i] = cnts[i]->name();
   return out;
 }
@@ -375,7 +375,7 @@ void BasicTrustRegionSQPResults::print() const
               "dexact",
               "ratio");
   std::printf("| %s | COSTS\n", std::string(88, '-').c_str());
-  for (size_t i = 0; i < old_cost_vals.size(); ++i)
+  for (std::size_t i = 0; i < old_cost_vals.size(); ++i)
   {
     double approx_improve = old_cost_vals[i] - model_cost_vals[i];
     double exact_improve = old_cost_vals[i] - new_cost_vals[i];
@@ -414,7 +414,7 @@ void BasicTrustRegionSQPResults::print() const
   if (!cnt_names.empty())
   {
     std::printf("| %s | CONSTRAINTS\n", std::string(88, '-').c_str());
-    for (size_t i = 0; i < old_cnt_viols.size(); ++i)
+    for (std::size_t i = 0; i < old_cnt_viols.size(); ++i)
     {
       double approx_improve = old_cnt_viols[i] - model_cnt_viols[i];
       double exact_improve = old_cnt_viols[i] - new_cnt_viols[i];
@@ -508,14 +508,14 @@ void BasicTrustRegionSQPResults::writeCosts(std::FILE* stream, bool header) cons
     std::fprintf(stream, "\n");
 
     std::fprintf(stream, "%s", "DESCRIPTION");
-    for (size_t i = 0; i < cost_names.size(); ++i)
+    for (std::size_t i = 0; i < cost_names.size(); ++i)
       std::fprintf(stream, ",%s,%s,%s,%s", "oldexact", "dapprox", "dexact", "ratio");
 
     std::fprintf(stream, "\n");
   }
 
   std::fprintf(stream, "%s", "COSTS");
-  for (size_t i = 0; i < old_cost_vals.size(); ++i)
+  for (std::size_t i = 0; i < old_cost_vals.size(); ++i)
   {
     double approx_improve = old_cost_vals[i] - model_cost_vals[i];
     double exact_improve = old_cost_vals[i] - new_cost_vals[i];
@@ -544,14 +544,14 @@ void BasicTrustRegionSQPResults::writeConstraints(std::FILE* stream, bool header
     std::fprintf(stream, "\n");
 
     std::fprintf(stream, "%s", "DESCRIPTION");
-    for (size_t i = 0; i < cnt_names.size(); ++i)
+    for (std::size_t i = 0; i < cnt_names.size(); ++i)
       std::fprintf(stream, ",%s,%s,%s,%s", "oldexact", "dapprox", "dexact", "ratio");
 
     std::fprintf(stream, "\n");
   }
 
   std::fprintf(stream, "%s", "CONSTRAINTS");
-  for (size_t i = 0; i < old_cnt_viols.size(); ++i)
+  for (std::size_t i = 0; i < old_cnt_viols.size(); ++i)
   {
     double approx_improve = old_cnt_viols[i] - model_cnt_viols[i];
     double exact_improve = old_cnt_viols[i] - new_cnt_viols[i];
diff --git a/trajopt_sco/src/osqp_interface.cpp b/trajopt_sco/src/osqp_interface.cpp
index 3057a4c5..87121dab 100644
--- a/trajopt_sco/src/osqp_interface.cpp
+++ b/trajopt_sco/src/osqp_interface.cpp
@@ -108,7 +108,7 @@ void OSQPModel::removeCnts(const CntVector& cnts)
 
 void OSQPModel::updateObjective()
 {
-  const size_t n = vars_.size();
+  const std::size_t n = vars_.size();
   osqp_data_.n = static_cast<c_int>(n);
 
   Eigen::SparseMatrix<double> sm;
@@ -133,8 +133,8 @@ void OSQPModel::updateObjective()
 
 void OSQPModel::updateConstraints()
 {
-  const size_t n = vars_.size();
-  const size_t m = cnts_.size();
+  const std::size_t n = vars_.size();
+  const std::size_t m = cnts_.size();
   const auto n_int = static_cast<Eigen::Index>(n);
   const auto m_int = static_cast<Eigen::Index>(m);
 
@@ -158,7 +158,7 @@ void OSQPModel::updateConstraints()
   }
 
   std::vector<Eigen::Index> new_inner_sizes(n);
-  for (size_t k = 0; k < n; ++k)
+  for (std::size_t k = 0; k < n; ++k)
   {
     new_inner_sizes[k] = sm.innerVector(static_cast<Eigen::Index>(k)).nonZeros() + 1;
   }
diff --git a/trajopt_sco/src/qpoases_interface.cpp b/trajopt_sco/src/qpoases_interface.cpp
index ca84a2ad..7e1eccc0 100644
--- a/trajopt_sco/src/qpoases_interface.cpp
+++ b/trajopt_sco/src/qpoases_interface.cpp
@@ -104,8 +104,8 @@ void qpOASESModel::updateObjective()
 
 void qpOASESModel::updateConstraints()
 {
-  const size_t n = vars_.size();
-  const size_t m = cnts_.size();
+  const std::size_t n = vars_.size();
+  const std::size_t m = cnts_.size();
 
   lbA_.clear();
   lbA_.resize(m, -QPOASES_INFTY);
@@ -116,7 +116,7 @@ void qpOASESModel::updateConstraints()
   Eigen::VectorXd v;
   exprToEigen(cnt_exprs_, sm, v, static_cast<Eigen::Index>(n));
 
-  for (size_t i_cnt = 0; i_cnt < m; ++i_cnt)
+  for (std::size_t i_cnt = 0; i_cnt < m; ++i_cnt)
   {
     lbA_[i_cnt] = (cnt_types_[i_cnt] == INEQ) ? -QPOASES_INFTY : v[static_cast<Eigen::Index>(i_cnt)];
     ubA_[i_cnt] = v[static_cast<Eigen::Index>(i_cnt)];
@@ -152,8 +152,8 @@ void qpOASESModel::createSolver()
 void qpOASESModel::update()
 {
   {
-    size_t inew = 0;
-    for (size_t iold = 0; iold < vars_.size(); ++iold)
+    std::size_t inew = 0;
+    for (std::size_t iold = 0; iold < vars_.size(); ++iold)
     {
       Var& var = vars_[iold];
       if (!var.var_rep->removed)
@@ -174,8 +174,8 @@ void qpOASESModel::update()
     ub_.resize(inew, -QPOASES_INFTY);
   }
   {
-    size_t inew = 0;
-    for (size_t iold = 0; iold < cnts_.size(); ++iold)
+    std::size_t inew = 0;
+    for (std::size_t iold = 0; iold < cnts_.size(); ++iold)
     {
       Cnt& cnt = cnts_[iold];
       if (!cnt.cnt_rep->removed)
@@ -199,9 +199,9 @@ void qpOASESModel::update()
 
 void qpOASESModel::setVarBounds(const VarVector& vars, const DblVec& lower, const DblVec& upper)
 {
-  for (size_t i = 0; i < vars.size(); ++i)
+  for (std::size_t i = 0; i < vars.size(); ++i)
   {
-    const size_t varind = vars[i].var_rep->index;
+    const std::size_t varind = vars[i].var_rep->index;
     lb_[varind] = lower[i];
     ub_[varind] = upper[i];
   }
@@ -209,9 +209,9 @@ void qpOASESModel::setVarBounds(const VarVector& vars, const DblVec& lower, cons
 DblVec qpOASESModel::getVarValues(const VarVector& vars) const
 {
   DblVec out(vars.size());
-  for (size_t i = 0; i < vars.size(); ++i)
+  for (std::size_t i = 0; i < vars.size(); ++i)
   {
-    const size_t varind = vars[i].var_rep->index;
+    const std::size_t varind = vars[i].var_rep->index;
     out[i] = solution_[varind];
   }
   return out;
diff --git a/trajopt_sco/src/solver_interface.cpp b/trajopt_sco/src/solver_interface.cpp
index def65064..c37c13dd 100644
--- a/trajopt_sco/src/solver_interface.cpp
+++ b/trajopt_sco/src/solver_interface.cpp
@@ -16,28 +16,28 @@ const std::vector<std::string> ModelType::MODEL_NAMES_ = { "GUROBI", "BPMPD", "O
 void vars2inds(const VarVector& vars, SizeTVec& inds)
 {
   inds = SizeTVec(vars.size());
-  for (size_t i = 0; i < inds.size(); ++i)
+  for (std::size_t i = 0; i < inds.size(); ++i)
     inds[i] = vars[i].var_rep->index;
 }
 
 void vars2inds(const VarVector& vars, IntVec& inds)
 {
   inds = IntVec(vars.size());
-  for (size_t i = 0; i < inds.size(); ++i)
+  for (std::size_t i = 0; i < inds.size(); ++i)
     inds[i] = static_cast<int>(vars[i].var_rep->index);
 }
 
 void cnts2inds(const CntVector& cnts, SizeTVec& inds)
 {
   inds = SizeTVec(cnts.size());
-  for (size_t i = 0; i < inds.size(); ++i)
+  for (std::size_t i = 0; i < inds.size(); ++i)
     inds[i] = cnts[i].cnt_rep->index;
 }
 
 void cnts2inds(const CntVector& cnts, IntVec& inds)
 {
   inds = IntVec(cnts.size());
-  for (size_t i = 0; i < inds.size(); ++i)
+  for (std::size_t i = 0; i < inds.size(); ++i)
     inds[i] = static_cast<int>(cnts[i].cnt_rep->index);
 }
 
@@ -68,7 +68,7 @@ size_t AffExpr::size() const { return coeffs.size(); }
 double AffExpr::value(const double* x) const
 {
   double out = constant;
-  for (size_t i = 0; i < size(); ++i)
+  for (std::size_t i = 0; i < size(); ++i)
   {
     out += coeffs[i] * vars[i].value(x);
   }
@@ -77,7 +77,7 @@ double AffExpr::value(const double* x) const
 double AffExpr::value(const DblVec& x) const
 {
   double out = constant;
-  for (size_t i = 0; i < size(); ++i)
+  for (std::size_t i = 0; i < size(); ++i)
   {
     out += coeffs[i] * vars[i].value(x);
   }
@@ -92,7 +92,7 @@ size_t QuadExpr::size() const { return coeffs.size(); }
 double QuadExpr::value(const DblVec& x) const
 {
   double out = affexpr.value(x);
-  for (size_t i = 0; i < size(); ++i)
+  for (std::size_t i = 0; i < size(); ++i)
   {
     out += coeffs[i] * vars1[i].value(x) * vars2[i].value(x);
   }
@@ -101,7 +101,7 @@ double QuadExpr::value(const DblVec& x) const
 double QuadExpr::value(const double* x) const
 {
   double out = affexpr.value(x);
-  for (size_t i = 0; i < size(); ++i)
+  for (std::size_t i = 0; i < size(); ++i)
   {
     out += coeffs[i] * vars1[i].value(x) * vars2[i].value(x);
   }
@@ -162,7 +162,7 @@ std::ostream& operator<<(std::ostream& o, const AffExpr& e)
     sep = " + ";
   }
 
-  for (size_t i = 0; i < e.size(); ++i)
+  for (std::size_t i = 0; i < e.size(); ++i)
   {
     if (e.coeffs[i] != 0)
     {
@@ -186,7 +186,7 @@ std::ostream& operator<<(std::ostream& o, const QuadExpr& e)
   o << " + [ ";
 
   std::string op;
-  for (size_t i = 0; i < e.size(); ++i)
+  for (std::size_t i = 0; i < e.size(); ++i)
   {
     if (e.coeffs[i] != 0)
     {
@@ -212,12 +212,11 @@ std::ostream& operator<<(std::ostream& o, const QuadExpr& e)
 
 std::ostream& operator<<(std::ostream& os, const ModelType& cs)
 {
-  auto cs_ivalue_ = static_cast<size_t>(cs.value_);
+  auto cs_ivalue_ = static_cast<std::size_t>(cs.value_);
   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_ << std::endl;
     throw std::runtime_error(conversion_error.str());
   }
   os << ModelType::MODEL_NAMES_[cs_ivalue_];
@@ -273,15 +272,15 @@ std::vector<ModelType> availableSolvers()
 #ifdef HAVE_QPOASES
   has_solver[ModelType::QPOASES] = true;
 #endif
-  size_t n_available_solvers = 0;
+  std::size_t n_available_solvers = 0;
   for (auto i = 0; i < ModelType::AUTO_SOLVER; ++i)
-    if (has_solver[static_cast<size_t>(i)])
+    if (has_solver[static_cast<std::size_t>(i)])
       ++n_available_solvers;
   std::vector<ModelType> available_solvers(n_available_solvers, ModelType::AUTO_SOLVER);
 
-  size_t j = 0;
+  std::size_t j = 0;
   for (int i = 0; i < static_cast<int>(ModelType::AUTO_SOLVER); ++i)
-    if (has_solver[static_cast<size_t>(i)])
+    if (has_solver[static_cast<std::size_t>(i)])
       available_solvers[j++] = static_cast<ModelType>(i);
   return available_solvers;
 }
diff --git a/trajopt_sco/src/solver_utils.cpp b/trajopt_sco/src/solver_utils.cpp
index 12521af6..660b4621 100644
--- a/trajopt_sco/src/solver_utils.cpp
+++ b/trajopt_sco/src/solver_utils.cpp
@@ -17,7 +17,7 @@ void exprToEigen(const AffExpr& expr, Eigen::SparseVector<double>& sparse_vector
   std::vector<std::pair<int, double>> doublets;
   doublets.reserve(expr.size());
 
-  for (size_t i = 0; i < expr.size(); ++i)
+  for (std::size_t i = 0; i < expr.size(); ++i)
   {
     auto i_var_index = static_cast<int>(expr.vars[i].var_rep->index);
     if (i_var_index >= n_vars)
@@ -68,7 +68,7 @@ void exprToEigen(const QuadExpr& expr,
   thread_local std::vector<T, Eigen::aligned_allocator<T>> triplets;
   triplets.clear();
 
-  for (size_t i = 0; i < expr.coeffs.size(); ++i)
+  for (std::size_t i = 0; i < expr.coeffs.size(); ++i)
   {
     if (expr.coeffs[i] != 0.0)
     {
@@ -123,10 +123,10 @@ void exprToEigen(const AffExprVector& expr_vec,
 
   for (int i = 0; i < static_cast<int>(expr_vec.size()); ++i)
   {
-    const AffExpr& expr = expr_vec[static_cast<size_t>(i)];
+    const AffExpr& expr = expr_vec[static_cast<std::size_t>(i)];
     vector[i] = -expr.constant;
 
-    for (size_t j = 0; j < expr.size(); ++j)
+    for (std::size_t j = 0; j < expr.size(); ++j)
     {
       auto i_var_index = static_cast<int>(expr.vars[j].var_rep->index);
       if (i_var_index >= n_vars)
@@ -170,9 +170,9 @@ void eigenToTriplets(const Eigen::SparseMatrix<double>& sparse_matrix,
                      DblVec& values_ij)
 {
   const auto& sm = sparse_matrix;
-  rows_i.reserve(rows_i.size() + static_cast<size_t>(sm.nonZeros()));
-  cols_j.reserve(cols_j.size() + static_cast<size_t>(sm.nonZeros()));
-  values_ij.reserve(values_ij.size() + static_cast<size_t>(sm.nonZeros()));
+  rows_i.reserve(rows_i.size() + static_cast<std::size_t>(sm.nonZeros()));
+  cols_j.reserve(cols_j.size() + static_cast<std::size_t>(sm.nonZeros()));
+  values_ij.reserve(values_ij.size() + static_cast<std::size_t>(sm.nonZeros()));
   for (int k = 0; k < sm.outerSize(); ++k)
   {
     for (Eigen::SparseMatrix<double>::InnerIterator it(sm, k); it; ++it)
diff --git a/trajopt_sco/test/small-problems-unit.cpp b/trajopt_sco/test/small-problems-unit.cpp
index fd2eafe0..8d5ad92a 100644
--- a/trajopt_sco/test/small-problems-unit.cpp
+++ b/trajopt_sco/test/small-problems-unit.cpp
@@ -27,11 +27,11 @@ class SQP : public testing::TestWithParam<ModelType>
   SQP() = default;
 };
 
-void setupProblem(OptProb::Ptr& probptr, size_t nvars, ModelType convex_solver)
+void setupProblem(OptProb::Ptr& probptr, std::size_t nvars, ModelType convex_solver)
 {
   probptr = std::make_shared<OptProb>(convex_solver);
   vector<string> var_names;
-  for (size_t i = 0; i < nvars; ++i)
+  for (std::size_t i = 0; i < nvars; ++i)
   {
     var_names.push_back((boost::format("x_%i") % i).str());
   }
@@ -43,7 +43,7 @@ void expectAllNear(const DblVec& x, const DblVec& y, double abstol)
   EXPECT_EQ(x.size(), y.size());
   stringstream ss;
   LOG_INFO("checking %s ?= %s", CSTR(x), CSTR(y));
-  for (size_t i = 0; i < x.size(); ++i)
+  for (std::size_t i = 0; i < x.size(); ++i)
     EXPECT_NEAR(x[i], y[i], abstol);
 }
 
@@ -92,7 +92,7 @@ void testProblem(ScalarOfVector::Ptr f,
                  ModelType convex_solver)
 {
   OptProb::Ptr prob;
-  size_t n = init.size();
+  std::size_t n = init.size();
   assert(sol.size() == n);
   setupProblem(prob, n, convex_solver);
   prob->addCost(std::make_shared<CostFromFunc>(std::move(f), prob->getVars(), "f", true));
diff --git a/trajopt_sco/test/solver-interface-unit.cpp b/trajopt_sco/test/solver-interface-unit.cpp
index a4b77750..dadcece9 100644
--- a/trajopt_sco/test/solver-interface-unit.cpp
+++ b/trajopt_sco/test/solver-interface-unit.cpp
@@ -44,7 +44,7 @@ TEST_P(SolverInterface, setup_problem)  // NOLINT
 
   AffExpr aff;
   std::cout << aff << std::endl;
-  for (size_t i = 0; i < 3; ++i)
+  for (std::size_t i = 0; i < 3; ++i)
   {
     exprInc(aff, vars[i]);
     solver->setVarBounds(vars[i], 0, 10);
@@ -61,7 +61,7 @@ TEST_P(SolverInterface, setup_problem)  // NOLINT
   solver->optimize();
 
   DblVec soln(3);
-  for (size_t i = 0; i < 3; ++i)
+  for (std::size_t i = 0; i < 3; ++i)
   {
     soln[i] = solver->getVarValue(vars[i]);
   }
@@ -93,7 +93,7 @@ TEST_P(SolverInterface, DISABLED_ExprMult_test1)  // NOLINT // QuadExpr not PSD
 
   AffExpr aff1;
   std::cout << aff1 << std::endl;
-  for (size_t i = 0; i < 3; ++i)
+  for (std::size_t i = 0; i < 3; ++i)
   {
     exprInc(aff1, vars[i]);
     solver->setVarBounds(vars[i], 0, 10);
@@ -103,7 +103,7 @@ TEST_P(SolverInterface, DISABLED_ExprMult_test1)  // NOLINT // QuadExpr not PSD
 
   AffExpr aff2;
   std::cout << aff2 << std::endl;
-  for (size_t i = 3; i < 6; ++i)
+  for (std::size_t i = 3; i < 6; ++i)
   {
     exprInc(aff2, vars[i]);
     solver->setVarBounds(vars[i], 0, 10);
@@ -121,7 +121,7 @@ TEST_P(SolverInterface, DISABLED_ExprMult_test1)  // NOLINT // QuadExpr not PSD
   solver->optimize();
 
   DblVec soln(3);
-  for (size_t i = 0; i < 3; ++i)
+  for (std::size_t i = 0; i < 3; ++i)
   {
     soln[i] = solver->getVarValue(vars[i]);
   }
@@ -174,7 +174,7 @@ TEST_P(SolverInterface, ExprMult_test2)  // NOLINT
   solver->optimize();
 
   DblVec soln(2);
-  for (size_t i = 0; i < 2; ++i)
+  for (std::size_t i = 0; i < 2; ++i)
   {
     soln[i] = solver->getVarValue(vars[i]);
     std::cout << soln[i] << std::endl;
@@ -226,7 +226,7 @@ TEST_P(SolverInterface, ExprMult_test3)  // NOLINT
   solver->optimize();
 
   DblVec soln(2);
-  for (size_t i = 0; i < 2; ++i)
+  for (std::size_t i = 0; i < 2; ++i)
   {
     soln[i] = solver->getVarValue(vars[i]);
     std::cout << soln[i] << std::endl;