Merged in fix/sqp_feedback_numerics (pull request #570)

small changes to deal with numerics Approved-by: Farbod Farshidian
quentin-leboutet · Mar 2, 2022 · a579011 · a579011
2 parents e8c5fd4 + e5d9306
commit a579011
Show file tree

Hide file tree

Showing 3 changed files with 48 additions and 9 deletions.
diff --git a/ocs2_core/include/ocs2_core/misc/LinearAlgebra.h b/ocs2_core/include/ocs2_core/misc/LinearAlgebra.h
@@ -46,6 +46,20 @@ void makePsdCholesky(matrix_t& A, scalar_t minEigenvalue);
 void computeConstraintProjection(const matrix_t& Dm, const matrix_t& RmInvUmUmT, matrix_t& DmDagger, matrix_t& DmDaggerTRmDmDaggerUUT,
                                  matrix_t& RmInvConstrainedUUT);
 
+/**
+ *  Set the eigenvalues of a triangular matrix to a minimum magnitude (maintaining the sign).
+ */
+inline void setTriangularMinimumEigenvalues(matrix_t& Lr, scalar_t minEigenValue = numeric_traits::weakEpsilon<scalar_t>()) {
+  for (Eigen::Index i = 0; i < Lr.rows(); ++i) {
+    scalar_t& eigenValue = Lr(i, i);  // diagonal element is the eigenvalue
+    if (eigenValue < 0.0) {
+      eigenValue = std::min(-minEigenValue, eigenValue);
+    } else {
+      eigenValue = std::max(minEigenValue, eigenValue);
+    }
+  }
+}
+
 /**
  * Makes the input matrix PSD using a eigenvalue decomposition.
  *

diff --git a/ocs2_core/src/misc/LinearAlgebra.cpp b/ocs2_core/src/misc/LinearAlgebra.cpp
@@ -98,6 +98,7 @@ void computeConstraintProjection(const matrix_t& Dm, const matrix_t& RmInvUmUmT,
   Eigen::HouseholderQR<matrix_t> QRof_RmInvUmUmTT_DmT(RmInvUmUmT.transpose() * Dm.transpose());
 
   matrix_t QRof_RmInvUmUmTT_DmT_Rc = QRof_RmInvUmUmTT_DmT.matrixQR().topRows(numConstraints).triangularView<Eigen::Upper>();
+  setTriangularMinimumEigenvalues(QRof_RmInvUmUmTT_DmT_Rc);
 
   // Computes the inverse of Rc with an efficient in-place forward-backward substitution
   // Turns out that this is equal to the UUT decomposition of DmDagger^T * R * DmDagger after simplification

diff --git a/ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp b/ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp
@@ -29,6 +29,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "hpipm_catkin/HpipmInterface.h"
 
+#include <ocs2_core/misc/LinearAlgebra.h>
+
 extern "C" {
 #include <hpipm_d_ocp_qp.h>
 #include <hpipm_d_ocp_qp_dim.h>
@@ -285,30 +287,44 @@ class HpipmInterface::Impl {
       printStatus();
     }
 
-    getStateSolution(x0, stateTrajectory);
-    getInputSolution(inputTrajectory);
+    if (!getStateSolution(x0, stateTrajectory)) {
+      return hpipm_status::NAN_SOL;
+    }
+    if (!getInputSolution(inputTrajectory)) {
+      return hpipm_status::NAN_SOL;
+    }
 
     // Return solver status
     int hpipmStatus = -1;
     d_ocp_qp_ipm_get_status(&workspace_, &hpipmStatus);
     return hpipm_status(hpipmStatus);
   }
 
-  void getStateSolution(const vector_t& x0, vector_array_t& stateTrajectory) {
+  bool getStateSolution(const vector_t& x0, vector_array_t& stateTrajectory) {
     stateTrajectory.resize(ocpSize_.numStages + 1);
     stateTrajectory.front() = x0;
     for (int k = 1; k < (ocpSize_.numStages + 1); ++k) {
       stateTrajectory[k].resize(ocpSize_.numStates[k]);
       d_ocp_qp_sol_get_x(k, &qpSol_, stateTrajectory[k].data());
+
+      if (!stateTrajectory[k].allFinite()) {
+        return false;
+      }
     }
+    return true;
   }
 
-  void getInputSolution(vector_array_t& inputTrajectory) {
+  bool getInputSolution(vector_array_t& inputTrajectory) {
     inputTrajectory.resize(ocpSize_.numStages);
     for (int k = 0; k < ocpSize_.numStages; ++k) {
       inputTrajectory[k].resize(ocpSize_.numInputs[k]);
       d_ocp_qp_sol_get_u(k, &qpSol_, inputTrajectory[k].data());
+
+      if (!inputTrajectory[k].allFinite()) {
+        return false;
+      }
     }
+    return true;
   }
 
   matrix_array_t getRiccatiFeedback(const VectorFunctionLinearApproximation& dynamics0, const ScalarFunctionQuadraticApproximation& cost0) {
@@ -317,9 +333,11 @@ class HpipmInterface::Impl {
 
     // k = 0, state is not a decision variable. Reconstruct backward pass from k = 1
     matrix_t P1(ocpSize_.numStates[1], ocpSize_.numStates[1]);
-    matrix_t Lr(ocpSize_.numInputs[0], ocpSize_.numInputs[0]);
     d_ocp_qp_ipm_get_ric_P(&qp_, &arg_, &workspace_, 1, P1.data());
+
+    matrix_t Lr(ocpSize_.numInputs[0], ocpSize_.numInputs[0]);
     d_ocp_qp_ipm_get_ric_Lr(&qp_, &arg_, &workspace_, 0, Lr.data());  // Lr matrix is lower triangular
+    LinearAlgebra::setTriangularMinimumEigenvalues(Lr);
 
     // RiccatiFeedback[0] = - (inv(Lr)^T * inv(Lr)) * (S0 + B0^T * P1 * A0)
     RiccatiFeedback[0] = -cost0.dfdux;
@@ -335,8 +353,10 @@ class HpipmInterface::Impl {
       if (numInput > 0) {
         // RiccatiFeedback[k] = -(Ls * Lr.inverse()).transpose();
         Lr.resize(numInput, numInput);
-        Ls.resize(ocpSize_.numStates[k], numInput);
         d_ocp_qp_ipm_get_ric_Lr(&qp_, &arg_, &workspace_, k, Lr.data());  // Lr matrix is lower triangular
+        LinearAlgebra::setTriangularMinimumEigenvalues(Lr);
+
+        Ls.resize(ocpSize_.numStates[k], numInput);
         d_ocp_qp_ipm_get_ric_Ls(&qp_, &arg_, &workspace_, k, Ls.data());
         RiccatiFeedback[k].noalias() = -Lr.triangularView<Eigen::Lower>().transpose().solve(Ls.transpose());
       }
@@ -352,10 +372,13 @@ class HpipmInterface::Impl {
 
     // k = 0, state is not a decision variable. Reconstruct backward pass from k = 1
     matrix_t P1(ocpSize_.numStates[1], ocpSize_.numStates[1]);
-    matrix_t Lr(ocpSize_.numInputs[0], ocpSize_.numInputs[0]);
-    vector_t p1(ocpSize_.numStates[1]);
     d_ocp_qp_ipm_get_ric_P(&qp_, &arg_, &workspace_, 1, P1.data());
+
+    matrix_t Lr(ocpSize_.numInputs[0], ocpSize_.numInputs[0]);
     d_ocp_qp_ipm_get_ric_Lr(&qp_, &arg_, &workspace_, 0, Lr.data());
+    LinearAlgebra::setTriangularMinimumEigenvalues(Lr);
+
+    vector_t p1(ocpSize_.numStates[1]);
     d_ocp_qp_ipm_get_ric_p(&qp_, &arg_, &workspace_, 1, p1.data());
 
     // RiccatiFeedforward[0] = -(inv(Lr)^T * inv(Lr)) * (r0 + B0.transpose() * p1 + B0.transpose() * P1 * b0);
@@ -393,6 +416,7 @@ class HpipmInterface::Impl {
     // k = 0
     matrix_t Lr0(ocpSize_.numInputs[0], ocpSize_.numInputs[0]);
     d_ocp_qp_ipm_get_ric_Lr(&qp_, &arg_, &workspace_, 0, Lr0.data());
+    LinearAlgebra::setTriangularMinimumEigenvalues(Lr0);
 
     // Shorthand notation
     const matrix_t& A0 = dynamics0.dfdx;
@@ -527,4 +551,4 @@ vector_array_t HpipmInterface::getRiccatiFeedforward(const VectorFunctionLinearA
   return pImpl_->getRiccatiFeedforward(dynamics0, cost0);
 }
 
-}  // namespace ocs2
+}  // namespace ocs2