Merge pull request #1307 from aristotpap/correct_inequality_feasibility

Add Bounds to the Inequality Feasibility Calculation
loco-3d · Sep 25, 2024 · 73abb31 · 73abb31
2 parents 7749b50 + 80654e1
commit 73abb31
Show file tree

Hide file tree

Showing 3 changed files with 36 additions and 7 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -6,6 +6,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
 ## [Unreleased]
 
+* Fixed the inequality constraints' feasibility computation by incorporating bounds into the calculation in https://github.com/loco-3d/crocoddyl/pull/1307
 * Improved the action factory used for unit testing in https://github.com/loco-3d/crocoddyl/pull/1300
 * Ignore ruff issues in ipython notebook files in https://github.com/loco-3d/crocoddyl/pull/1297
 * Improved efficiency for computing impulse-dynamics derivatives in https://github.com/loco-3d/crocoddyl/pull/1294

diff --git a/include/crocoddyl/core/solver-base.hpp b/include/crocoddyl/core/solver-base.hpp
@@ -474,6 +474,7 @@ class SolverAbstract {
                                    //!< dynamics and constraints feasibility
   std::size_t iter_;  //!< Number of iteration performed by the solver
   double tmp_feas_;   //!< Temporal variables used for computed the feasibility
+  std::vector<Eigen::VectorXd> g_adj_;  //!< Adjusted inequality bound
 };
 
 /**

diff --git a/src/core/solver-base.cpp b/src/core/solver-base.cpp
@@ -47,34 +47,45 @@ SolverAbstract::SolverAbstract(boost::shared_ptr<ShootingProblem> problem)
   // Allocate common data
   const std::size_t ndx = problem_->get_ndx();
   const std::size_t T = problem_->get_T();
+  const std::size_t ng_T = problem_->get_terminalModel()->get_ng_T();
   xs_.resize(T + 1);
   us_.resize(T);
   fs_.resize(T + 1);
+  g_adj_.resize(T + 1);
   const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
       problem_->get_runningModels();
   for (std::size_t t = 0; t < T; ++t) {
     const boost::shared_ptr<ActionModelAbstract>& model = models[t];
     const std::size_t nu = model->get_nu();
+    const std::size_t ng = model->get_ng();
     xs_[t] = model->get_state()->zero();
     us_[t] = Eigen::VectorXd::Zero(nu);
     fs_[t] = Eigen::VectorXd::Zero(ndx);
+    g_adj_[t] = Eigen::VectorXd::Zero(ng);
   }
   xs_.back() = problem_->get_terminalModel()->get_state()->zero();
   fs_.back() = Eigen::VectorXd::Zero(ndx);
+  g_adj_.back() = Eigen::VectorXd::Zero(ng_T);
 }
 
 SolverAbstract::~SolverAbstract() {}
 
 void SolverAbstract::resizeData() {
   START_PROFILER("SolverAbstract::resizeData");
   const std::size_t T = problem_->get_T();
+  const std::size_t ng_T = problem_->get_terminalModel()->get_ng_T();
   const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
       problem_->get_runningModels();
   for (std::size_t t = 0; t < T; ++t) {
     const boost::shared_ptr<ActionModelAbstract>& model = models[t];
     const std::size_t nu = model->get_nu();
+    const std::size_t ng = model->get_ng();
     us_[t].conservativeResize(nu);
+    g_adj_[t].conservativeResize(ng);
   }
+
+  g_adj_.back().conservativeResize(ng_T);
+
   STOP_PROFILER("SolverAbstract::resizeData");
 }
 
@@ -120,28 +131,44 @@ double SolverAbstract::computeInequalityFeasibility() {
       problem_->get_runningModels();
   const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
       problem_->get_runningDatas();
+
   switch (feasnorm_) {
     case LInf:
       for (std::size_t t = 0; t < T; ++t) {
         if (models[t]->get_ng() > 0) {
-          tmp_feas_ =
-              std::max(tmp_feas_, datas[t]->g.lpNorm<Eigen::Infinity>());
+          g_adj_[t] = datas[t]
+                          ->g.cwiseMax(models[t]->get_g_lb())
+                          .cwiseMin(models[t]->get_g_ub());
+          tmp_feas_ = std::max(
+              tmp_feas_, (datas[t]->g - g_adj_[t]).lpNorm<Eigen::Infinity>());
         }
       }
       if (problem_->get_terminalModel()->get_ng_T() > 0) {
-        tmp_feas_ =
-            std::max(tmp_feas_,
-                     problem_->get_terminalData()->g.lpNorm<Eigen::Infinity>());
+        g_adj_.back() =
+            problem_->get_terminalData()
+                ->g.cwiseMax(problem_->get_terminalModel()->get_g_lb())
+                .cwiseMin(problem_->get_terminalModel()->get_g_ub());
+        tmp_feas_ += (problem_->get_terminalData()->g - g_adj_.back())
+                         .lpNorm<Eigen::Infinity>();
       }
       break;
     case L1:
       for (std::size_t t = 0; t < T; ++t) {
         if (models[t]->get_ng() > 0) {
-          tmp_feas_ += datas[t]->g.lpNorm<1>();
+          g_adj_[t] = datas[t]
+                          ->g.cwiseMax(models[t]->get_g_lb())
+                          .cwiseMin(models[t]->get_g_ub());
+          tmp_feas_ =
+              std::max(tmp_feas_, (datas[t]->g - g_adj_[t]).lpNorm<1>());
         }
       }
       if (problem_->get_terminalModel()->get_ng_T() > 0) {
-        tmp_feas_ += problem_->get_terminalData()->g.lpNorm<1>();
+        g_adj_.back() =
+            problem_->get_terminalData()
+                ->g.cwiseMax(problem_->get_terminalModel()->get_g_lb())
+                .cwiseMin(problem_->get_terminalModel()->get_g_ub());
+        tmp_feas_ +=
+            (problem_->get_terminalData()->g - g_adj_.back()).lpNorm<1>();
       }
       break;
   }