diff --git a/trajopt_common/cmake/trajopt_macros.cmake b/trajopt_common/cmake/trajopt_macros.cmake
index 2c680d00..2368d7dd 100644
--- a/trajopt_common/cmake/trajopt_macros.cmake
+++ b/trajopt_common/cmake/trajopt_macros.cmake
@@ -82,5 +82,5 @@ macro(trajopt_variables)
     endif()
   endif()
 
-  set(TRAJOPT_CXX_VERSION 14)
+  set(TRAJOPT_CXX_VERSION 17)
 endmacro()
diff --git a/trajopt_ifopt/test/simple_collision_unit.cpp b/trajopt_ifopt/test/simple_collision_unit.cpp
index 127130d6..c9814368 100644
--- a/trajopt_ifopt/test/simple_collision_unit.cpp
+++ b/trajopt_ifopt/test/simple_collision_unit.cpp
@@ -109,7 +109,7 @@ TEST_F(SimpleCollisionTest, spheres)  // NOLINT
       std::make_shared<trajopt_ifopt::SingleTimestepCollisionEvaluator>(
           collision_cache, manip, env, trajopt_collision_config);
 
-  auto cnt = std::make_shared<trajopt_ifopt::DiscreteCollisionConstraint>(collision_evaluator, vars[0], 3);
+  auto cnt = std::make_shared<trajopt_ifopt::DiscreteCollisionConstraint>(collision_evaluator, vars[0], 3, true);
   nlp.AddConstraintSet(cnt);
 
   nlp.PrintCurrent();
diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/ifopt_qp_problem.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/ifopt_qp_problem.h
index 20c6722b..361a084f 100644
--- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/ifopt_qp_problem.h
+++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/ifopt_qp_problem.h
@@ -77,6 +77,8 @@ class IfoptQPProblem : public QPProblem
 
   void setBoxSize(const Eigen::Ref<const Eigen::VectorXd>& box_size) override;
 
+  void setConstraintMeritCoeff(const Eigen::Ref<const Eigen::VectorXd>& merit_coeff) override;
+
   Eigen::VectorXd getBoxSize() const override;
 
   /** @brief Prints all members to the terminal in a human readable form */
diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_problem.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_problem.h
index 1f94a7c7..ca567269 100644
--- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_problem.h
+++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_problem.h
@@ -129,6 +129,14 @@ class QPProblem
    * @param box_size New box size
    */
   virtual void setBoxSize(const Eigen::Ref<const Eigen::VectorXd>& box_size) = 0;
+
+  /**
+   * @brief Set the constraint merit coeff
+   * @details This controls the gradient for the constraint slack variables.
+   * @param merit_coeff
+   */
+  virtual void setConstraintMeritCoeff(const Eigen::Ref<const Eigen::VectorXd>& merit_coeff) = 0;
+
   /**
    * @brief Returns the box size
    * @return The box size for each variable
diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/trajopt_qp_problem.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/trajopt_qp_problem.h
index faf5fb92..2e936475 100644
--- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/trajopt_qp_problem.h
+++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/trajopt_qp_problem.h
@@ -51,6 +51,8 @@ class TrajOptQPProblem : public QPProblem
 
   void setBoxSize(const Eigen::Ref<const Eigen::VectorXd>& box_size) override;
 
+  void setConstraintMeritCoeff(const Eigen::Ref<const Eigen::VectorXd>& merit_coeff) override;
+
   Eigen::VectorXd getBoxSize() const override;
 
   void print() const override;
diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/trust_region_sqp_solver.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/trust_region_sqp_solver.h
index 068f3074..3af13eb3 100644
--- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/trust_region_sqp_solver.h
+++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/trust_region_sqp_solver.h
@@ -125,6 +125,8 @@ class TrustRegionSQPSolver
   SQPStatus status_{ SQPStatus::QP_SOLVER_ERROR };
   SQPResults results_;
   std::vector<SQPCallback::Ptr> callbacks_;
+
+  void constraintMeritCoeffChanged();
 };
 
 }  // namespace trajopt_sqp
diff --git a/trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp b/trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp
index befbe26a..99d6e5e0 100644
--- a/trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp
+++ b/trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp
@@ -485,6 +485,12 @@ void IfoptQPProblem::setBoxSize(const Eigen::Ref<const Eigen::VectorXd>& box_siz
   updateNLPVariableBounds();
 }
 
+void IfoptQPProblem::setConstraintMeritCoeff(const Eigen::Ref<const Eigen::VectorXd>& merit_coeff)
+{
+  assert(merit_coeff.size() == num_nlp_cnts_);
+  constraint_merit_coeff_ = merit_coeff;
+}
+
 Eigen::VectorXd IfoptQPProblem::getBoxSize() const { return box_size_; }
 
 void IfoptQPProblem::print() const
diff --git a/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp b/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp
index a9881c25..bc08e5af 100644
--- a/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp
+++ b/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp
@@ -116,7 +116,12 @@ bool OSQPEigenSolver::solve()
   }
 
   /** @todo Need to check if this is what we want in the new version */
-  if (solver_.solve() || solver_.workspace()->info->status_val == OSQP_SOLVED_INACCURATE)
+  const bool solved = solver_.solve();
+  const auto status = static_cast<int>(solver_.workspace()->info->status_val);
+  if (OSQP_COMPARE_DEBUG_MODE)
+    std::cout << "OSQP Status Value: " << solver_.workspace()->info->status_val << std::endl;
+
+  if (solved || status == OSQP_SOLVED_INACCURATE)
   {
     if (OSQP_COMPARE_DEBUG_MODE)
     {
diff --git a/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp b/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp
index bdd338aa..543d8157 100644
--- a/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp
+++ b/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp
@@ -768,6 +768,12 @@ void TrajOptQPProblem::setBoxSize(const Eigen::Ref<const Eigen::VectorXd>& box_s
   updateNLPVariableBounds();
 }
 
+void TrajOptQPProblem::setConstraintMeritCoeff(const Eigen::Ref<const Eigen::VectorXd>& merit_coeff)
+{
+  assert(merit_coeff.size() == getNumNLPConstraints());
+  constraint_merit_coeff_ = merit_coeff;
+}
+
 Eigen::VectorXd TrajOptQPProblem::getBoxSize() const { return box_size_; }
 
 void TrajOptQPProblem::print() const
diff --git a/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp b/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp
index 9efb7e6b..cd12dcdb 100644
--- a/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp
+++ b/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp
@@ -54,11 +54,8 @@ bool TrustRegionSQPSolver::init(QPProblem::Ptr qp_prob)
   // Evaluate exact constraint violations (expensive)
   results_.best_constraint_violations = qp_problem->getExactConstraintViolations();
 
-  // Calculate exact NLP merits (expensive) - TODO: Look into caching for qp_solver->Convexify()
-  results_.best_exact_merit =
-      results_.best_costs.sum() + results_.best_constraint_violations.dot(results_.merit_error_coeffs);
-
   setBoxSize(params.initial_trust_box_size);
+  constraintMeritCoeffChanged();
   return true;
 }
 
@@ -68,6 +65,15 @@ void TrustRegionSQPSolver::setBoxSize(double box_size)
   results_.box_size = qp_problem->getBoxSize();
 }
 
+void TrustRegionSQPSolver::constraintMeritCoeffChanged()
+{
+  qp_problem->setConstraintMeritCoeff(results_.merit_error_coeffs);
+
+  // Recalculate the best exact merit because merit coeffs may have changed
+  results_.best_exact_merit =
+      results_.best_costs.sum() + results_.best_constraint_violations.dot(results_.merit_error_coeffs);
+}
+
 void TrustRegionSQPSolver::registerCallback(const SQPCallback::Ptr& callback) { callbacks_.push_back(callback); }
 
 const SQPStatus& TrustRegionSQPSolver::getStatus() { return status_; }
@@ -186,6 +192,7 @@ void TrustRegionSQPSolver::adjustPenalty()
     results_.merit_error_coeffs *= params.merit_coeff_increase_ratio;
   }
   setBoxSize(fmax(results_.box_size[0], params.min_trust_box_size / params.trust_shrink_ratio * 1.5));
+  constraintMeritCoeffChanged();
 }
 
 bool TrustRegionSQPSolver::stepSQPSolver()
diff --git a/trajopt_sco/src/optimizers.cpp b/trajopt_sco/src/optimizers.cpp
index 8854b482..e22f7857 100644
--- a/trajopt_sco/src/optimizers.cpp
+++ b/trajopt_sco/src/optimizers.cpp
@@ -12,7 +12,6 @@ TRAJOPT_IGNORE_WARNINGS_POP
 #include <trajopt_sco/sco_common.hpp>
 #include <trajopt_sco/solver_interface.hpp>
 #include <trajopt_common/logging.hpp>
-#include <trajopt_common/macros.h>
 #include <trajopt_common/stl_to_string.hpp>
 
 namespace sco
diff --git a/trajopt_sco/src/osqp_interface.cpp b/trajopt_sco/src/osqp_interface.cpp
index 4a2d36eb..04253864 100644
--- a/trajopt_sco/src/osqp_interface.cpp
+++ b/trajopt_sco/src/osqp_interface.cpp
@@ -337,15 +337,16 @@ CvxOptStatus OSQPModel::optimize()
   {
     // opt += m_objective.affexpr.constant;
     solution_ = DblVec(osqp_workspace_->solution->x, osqp_workspace_->solution->x + vars_.size());
+    auto status = static_cast<int>(osqp_workspace_->info->status_val);
 
     if (OSQP_COMPARE_DEBUG_MODE)
     {
       Eigen::IOFormat format(5);
       Eigen::Map<Eigen::VectorXd> solution_vec(solution_.data(), static_cast<Eigen::Index>(solution_.size()));
       std::cout << "OSQP Solution: " << solution_vec.transpose().format(format) << std::endl;
+      std::cout << "OSQP Status Value: " << status << std::endl;
     }
 
-    auto status = static_cast<int>(osqp_workspace_->info->status_val);
     if (status == OSQP_SOLVED || status == OSQP_SOLVED_INACCURATE)
       return CVX_SOLVED;
     if (status == OSQP_PRIMAL_INFEASIBLE || status == OSQP_PRIMAL_INFEASIBLE_INACCURATE ||