Fix TrajOpt Ifopt handling of constraint merit coefficient

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()

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();

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 */

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

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;

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

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

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)
     {

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

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()

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

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 ||