Feature/hard inequality constraints

ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h

@@ -75,6 +75,8 @@ struct OptimalControlProblem {
   std::unique_ptr<StateConstraintCollection> preJumpEqualityConstraintPtr;
   /** Final equality constraints */
   std::unique_ptr<StateConstraintCollection> finalEqualityConstraintPtr;
+  /** (Hard) inequality constraints **/
+  std::unique_ptr<StateInputConstraintCollection> inequalityConstraintPtr;
 
   /* Lagrangians */
   /** Lagrangian for intermediate equality constraints */

ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h

@@ -56,6 +56,11 @@ struct PerformanceIndex {
    */
   scalar_t equalityConstraintsSSE = 0.0;
 
+  /** Sum of Squared Error (SSE) of hard inequality constraints, used by the SQP solver.
+   * Inequality constraints are satisfied if positive, so there is only error if the constraints are negative.
+   */
+  scalar_t inequalityConstraintsSSE = 0.0;
+
   /** Sum of equality Lagrangians:
    * - Final: penalty for violation in state equality constraints
    * - PreJumps: penalty for violation in state equality constraints
@@ -76,6 +81,7 @@ struct PerformanceIndex {
     this->cost += rhs.cost;
     this->dynamicsViolationSSE += rhs.dynamicsViolationSSE;
     this->equalityConstraintsSSE += rhs.equalityConstraintsSSE;
+    this->inequalityConstraintsSSE += rhs.inequalityConstraintsSSE;
     this->equalityLagrangian += rhs.equalityLagrangian;
     this->inequalityLagrangian += rhs.inequalityLagrangian;
     return *this;
@@ -93,6 +99,7 @@ inline void swap(PerformanceIndex& lhs, PerformanceIndex& rhs) {
   std::swap(lhs.cost, rhs.cost);
   std::swap(lhs.dynamicsViolationSSE, rhs.dynamicsViolationSSE);
   std::swap(lhs.equalityConstraintsSSE, rhs.equalityConstraintsSSE);
+  std::swap(lhs.inequalityConstraintsSSE, rhs.inequalityConstraintsSSE);
   std::swap(lhs.equalityLagrangian, rhs.equalityLagrangian);
   std::swap(lhs.inequalityLagrangian, rhs.inequalityLagrangian);
 }
@@ -109,6 +116,7 @@ inline std::ostream& operator<<(std::ostream& stream, const PerformanceIndex& pe
   stream << std::setw(indentation) << "";
   stream << "Dynamics violation SSE:     " << std::setw(tabSpace) << performanceIndex.dynamicsViolationSSE;
   stream << "Equality constraints SSE:   " << std::setw(tabSpace) << performanceIndex.equalityConstraintsSSE << '\n';
+  stream << "Inequality constraints SSE: " << std::setw(tabSpace) << performanceIndex.inequalityConstraintsSSE << '\n';
 
   stream << std::setw(indentation) << "";
   stream << "Equality Lagrangian:        " << std::setw(tabSpace) << performanceIndex.equalityLagrangian;

ocs2_oc/src/oc_problem/OptimalControlProblem.cpp

@@ -50,6 +50,8 @@ OptimalControlProblem::OptimalControlProblem()
       stateEqualityConstraintPtr(new StateConstraintCollection),
       preJumpEqualityConstraintPtr(new StateConstraintCollection),
       finalEqualityConstraintPtr(new StateConstraintCollection),
+      /* Inequality constraints */
+      inequalityConstraintPtr(new StateInputConstraintCollection),
       /* Lagrangians */
       equalityLagrangianPtr(new StateInputCostCollection),
       stateEqualityLagrangianPtr(new StateCostCollection),
@@ -82,6 +84,8 @@ OptimalControlProblem::OptimalControlProblem(const OptimalControlProblem& other)
       stateEqualityConstraintPtr(other.stateEqualityConstraintPtr->clone()),
       preJumpEqualityConstraintPtr(other.preJumpEqualityConstraintPtr->clone()),
       finalEqualityConstraintPtr(other.finalEqualityConstraintPtr->clone()),
+      /* Inequality constraints */
+      inequalityConstraintPtr(other.inequalityConstraintPtr->clone()),
       /* Lagrangians */
       equalityLagrangianPtr(other.equalityLagrangianPtr->clone()),
       stateEqualityLagrangianPtr(other.stateEqualityLagrangianPtr->clone()),
@@ -130,6 +134,9 @@ void OptimalControlProblem::swap(OptimalControlProblem& other) noexcept {
   preJumpEqualityConstraintPtr.swap(other.preJumpEqualityConstraintPtr);
   finalEqualityConstraintPtr.swap(other.finalEqualityConstraintPtr);
 
+  /* Inequality constraints */
+  inequalityConstraintPtr.swap(other.inequalityConstraintPtr);
+
   /* Lagrangians */
   equalityLagrangianPtr.swap(other.equalityLagrangianPtr);
   stateEqualityLagrangianPtr.swap(other.stateEqualityLagrangianPtr);

ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h

@@ -95,6 +95,7 @@ class LeggedRobotInterface final : public RobotInterface {
   std::pair<scalar_t, RelaxedBarrierPenalty::Config> loadFrictionConeSettings(const std::string& taskFile, bool verbose) const;
   std::unique_ptr<StateInputCost> getFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient,
                                                             const RelaxedBarrierPenalty::Config& barrierPenaltyConfig);
+  std::unique_ptr<StateInputConstraint> getHardFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient);
   std::unique_ptr<StateInputConstraint> getZeroForceConstraint(size_t contactPointIndex);
   std::unique_ptr<StateInputConstraint> getZeroVelocityConstraint(const EndEffectorKinematics<scalar_t>& eeKinematics,
                                                                   size_t contactPointIndex, bool useAnalyticalGradients);

ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp

@@ -174,8 +174,9 @@ void LeggedRobotInterface::setupOptimalConrolProblem(const std::string& taskFile
                                                                     modelSettings_.recompileLibrariesCppAd, modelSettings_.verboseCppAd));
     }
 
-    problemPtr_->softConstraintPtr->add(footName + "_frictionCone",
-                                        getFrictionConeConstraint(i, frictionCoefficient, barrierPenaltyConfig));
+    // problemPtr_->softConstraintPtr->add(footName + "_frictionCone",
+    //                                     getFrictionConeConstraint(i, frictionCoefficient, barrierPenaltyConfig));
+    problemPtr_->inequalityConstraintPtr->add(footName + "_frictionCone", getHardFrictionConeConstraint(i, frictionCoefficient));
     problemPtr_->equalityConstraintPtr->add(footName + "_zeroForce", getZeroForceConstraint(i));
     problemPtr_->equalityConstraintPtr->add(footName + "_zeroVelocity",
                                             getZeroVelocityConstraint(*eeKinematicsPtr, i, useAnalyticalGradientsConstraints));
@@ -321,6 +322,13 @@ std::unique_ptr<StateInputCost> LeggedRobotInterface::getFrictionConeConstraint(
   return std::unique_ptr<StateInputCost>(new StateInputSoftConstraint(std::move(frictionConeConstraintPtr), std::move(penalty)));
 }
 
+std::unique_ptr<StateInputConstraint> LeggedRobotInterface::getHardFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient) {
+  FrictionConeConstraint::Config frictionConeConConfig(frictionCoefficient);
+  std::unique_ptr<FrictionConeConstraint> frictionConeConstraintPtr(
+      new FrictionConeConstraint(*referenceManagerPtr_, std::move(frictionConeConConfig), contactPointIndex, centroidalModelInfo_));
+  return frictionConeConstraintPtr;
+}
+
 /******************************************************************************************************/
 /******************************************************************************************************/
 /******************************************************************************************************/

ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h

@@ -72,7 +72,8 @@ class HpipmInterface {
    * @param x0 : Initial state (deviation).
    * @param dynamics : Linearized approximation of the discrete dynamics.
    * @param cost : Quadratic approximation of the cost.
-   * @param constraints : Linearized approximation of constraints, all constraints are mapped to inequality constraints in HPIPM.
+   * @param constraints : Linearized approximation of equality constraints, all constraints are mapped to inequality constraints in HPIPM.
+   * @param ineqConstraints : Linearized approximation of inequality constraints.
    * @param [out] stateTrajectory : Solution state (deviation) trajectory.
    * @param [out] inputTrajectory : Solution input (deviation) trajectory.
    * @param verbose : Prints the HPIPM iteration statistics if true.
@@ -85,7 +86,8 @@ class HpipmInterface {
    */
   hpipm_status solve(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics,
                      std::vector<ScalarFunctionQuadraticApproximation>& cost, std::vector<VectorFunctionLinearApproximation>* constraints,
-                     vector_array_t& stateTrajectory, vector_array_t& inputTrajectory, bool verbose = false);
+                     std::vector<VectorFunctionLinearApproximation>* ineqConstraints, vector_array_t& stateTrajectory,
+                     vector_array_t& inputTrajectory, bool verbose = false);
 
   /**
    * Return the Riccati cost-to-go for the previously solved problem.

ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h

@@ -81,12 +81,14 @@ bool operator==(const OcpSize& lhs, const OcpSize& rhs) noexcept;
  *
  * @param dynamics : Linearized approximation of the discrete dynamics.
  * @param cost : Quadratic approximation of the cost.
- * @param constraints : Linearized approximation of constraints, all constraints are mapped to inequality constraints in HPIPM.
+ * @param constraints : Linearized approximation of equality constraints, all constraints are mapped to inequality constraints in HPIPM.
+ * @param ineqConstraints : Linearized approximation of inequality constraints.
  * @return Derived sizes
  */
 OcpSize extractSizesFromProblem(const std::vector<VectorFunctionLinearApproximation>& dynamics,
                                 const std::vector<ScalarFunctionQuadraticApproximation>& cost,
-                                const std::vector<VectorFunctionLinearApproximation>* constraints);
+                                const std::vector<VectorFunctionLinearApproximation>* constraints,
+                                const std::vector<VectorFunctionLinearApproximation>* ineqConstraints);
 
 }  // namespace hpipm_interface
 }  // namespace ocs2

ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp

@@ -145,7 +145,8 @@ class HpipmInterface::Impl {
 
   void verifySizes(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics,
                    std::vector<ScalarFunctionQuadraticApproximation>& cost,
-                   std::vector<VectorFunctionLinearApproximation>* constraints) const {
+                   std::vector<VectorFunctionLinearApproximation>* constraints,
+                   std::vector<VectorFunctionLinearApproximation>* ineqConstraints) const {
     if (dynamics.size() != ocpSize_.numStages) {
       throw std::runtime_error("[HpipmInterface] Inconsistent size of dynamics: " + std::to_string(dynamics.size()) + " with " +
                                std::to_string(ocpSize_.numStages) + " number of stages.");
@@ -156,7 +157,13 @@ class HpipmInterface::Impl {
     }
     if (constraints != nullptr) {
       if (constraints->size() != ocpSize_.numStages + 1) {
-        throw std::runtime_error("[HpipmInterface] Inconsistent size of constraints: " + std::to_string(constraints->size()) + " with " +
+        throw std::runtime_error("[HpipmInterface] Inconsistent size of equality constraints: " + std::to_string(constraints->size()) + " with " +
+                                 std::to_string(ocpSize_.numStages + 1) + " nodes.");
+      }
+    }
+    if (ineqConstraints != nullptr) {
+      if (ineqConstraints->size() != ocpSize_.numStages + 1) {
+        throw std::runtime_error("[HpipmInterface] Inconsistent size of inequality constraints: " + std::to_string(ineqConstraints->size()) + " with " +
                                  std::to_string(ocpSize_.numStages + 1) + " nodes.");
       }
     }
@@ -165,9 +172,10 @@ class HpipmInterface::Impl {
 
   hpipm_status solve(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics,
                      std::vector<ScalarFunctionQuadraticApproximation>& cost, std::vector<VectorFunctionLinearApproximation>* constraints,
-                     vector_array_t& stateTrajectory, vector_array_t& inputTrajectory, bool verbose) {
+                     std::vector<VectorFunctionLinearApproximation>* ineqConstraints, vector_array_t& stateTrajectory,
+                     vector_array_t& inputTrajectory, bool verbose) {
     const int N = ocpSize_.numStages;
-    verifySizes(x0, dynamics, cost, constraints);
+    verifySizes(x0, dynamics, cost, constraints, ineqConstraints);
 
     // === Dynamics ===
     std::vector<scalar_t*> AA(N, nullptr);
@@ -221,48 +229,114 @@ class HpipmInterface::Impl {
     qq[N] = cost[N].dfdx.data();
 
     // === Constraints ===
-    // for ocs2 --> C*dx + D*du + e = 0
-    // for hpipm --> ug >= C*dx + D*du >= lg
-    std::vector<scalar_t*> CC(N + 1, nullptr);
-    std::vector<scalar_t*> DD(N + 1, nullptr);
-    std::vector<scalar_t*> llg(N + 1, nullptr);
-    std::vector<scalar_t*> uug(N + 1, nullptr);
-    std::vector<ocs2::vector_t> boundData;  // Declare at this scope to keep the data alive while HPIPM has the pointers
+
+    // Equality constraints for each stage:
+    // C * x + D * u == d
+    // Declare at this scope to keep data alive while HPIPM has the pointers
+    std::vector<matrix_t> C_eq(N + 1);
+    std::vector<matrix_t> D_eq(N + 1);
+    std::vector<vector_t> d_eq(N + 1);
 
     if (constraints != nullptr) {
       auto& constr = *constraints;
-      boundData.resize(N + 1);
 
-      // k = 0, eliminate initial state
-      // numState[0] = 0 --> No need to specify C[0] here
+      // k = 0
+      // No need to specify C_eq[0] since initial state is given
       if (constr[0].f.size() > 0) {
-        boundData[0] = -constr[0].f;
-        boundData[0].noalias() -= constr[0].dfdx * x0;
-        llg[0] = boundData[0].data();
-        uug[0] = boundData[0].data();
-        DD[0] = constr[0].dfdu.data();
+        d_eq[0] = -constr[0].f;
+        d_eq[0].noalias() -= constr[0].dfdx * x0;
+        D_eq[0] = constr[0].dfdu;
       }
 
       // k = 1 -> (N-1)
       for (int k = 1; k < N; k++) {
         if (constr[k].f.size() > 0) {
-          CC[k] = constr[k].dfdx.data();
-          DD[k] = constr[k].dfdu.data();
-          boundData[k] = -constr[k].f;
-          llg[k] = boundData[k].data();
-          uug[k] = boundData[k].data();
+          d_eq[k] = -constr[k].f;
+          C_eq[k] = constr[k].dfdx;
+          D_eq[k] = constr[k].dfdu;
         }
       }
 
       // k = N, no inputs
       if (constr[N].f.size() > 0) {
-        CC[N] = constr[N].dfdx.data();
-        boundData[N] = -constr[N].f;
-        llg[N] = boundData[N].data();
-        uug[N] = boundData[N].data();
+        d_eq[N] = -constr[N].f;
+        C_eq[N] = constr[N].dfdx;
       }
     }
 
+    // Inequality constraints for each stage:
+    // C * x + D * u >= d
+    std::vector<matrix_t> C_ineq(N + 1);
+    std::vector<matrix_t> D_ineq(N + 1);
+    std::vector<vector_t> d_ineq(N + 1);
+
+    if (ineqConstraints != nullptr) {
+      auto& constr = *ineqConstraints;
+
+      // k = 0
+      if (constr[0].f.size() > 0) {
+        d_ineq[0] = -constr[0].f;
+        d_ineq[0].noalias() -= constr[0].dfdx * x0;
+        D_ineq[0] = constr[0].dfdu;
+      }
+
+      // k = 1 -> (N-1)
+      for (int k = 1; k < N; k++) {
+        if (constr[k].f.size() > 0) {
+          C_ineq[k] = constr[k].dfdx;
+          D_ineq[k] = constr[k].dfdu;
+          d_ineq[k] = -constr[k].f;
+        }
+      }
+
+      // k = N
+      if (constr[N].f.size() > 0) {
+        d_ineq[N] = -constr[N].f;
+        C_ineq[N] = constr[N].dfdx;
+      }
+    }
+
+    // Combined equality and inequality constraints for each stage
+    std::vector<matrix_t> C_con(N + 1);
+    std::vector<matrix_t> D_con(N + 1);
+    std::vector<vector_t> d_con(N + 1);
+    std::vector<vector_t> upper_bound_mask(N + 1);
+
+    // Raw data for each stage
+    std::vector<scalar_t*> CC(N + 1, nullptr);
+    std::vector<scalar_t*> DD(N + 1, nullptr);
+    std::vector<scalar_t*> llg(N + 1, nullptr);
+    std::vector<scalar_t*> uug(N + 1, nullptr);
+
+    // Combine equality and inequality constraints and extract raw data
+    for (int k = 0; k <= N; k++) {
+        // Make block diagonal C = [[C_eq, 0     ],
+        //                          [0,    C_ineq]]
+        C_con[k].setZero(C_eq[k].rows() + C_ineq[k].rows(), C_eq[k].cols() + C_ineq[k].cols());
+        C_con[k].topLeftCorner(C_eq[k].rows(), C_eq[k].cols()) = C_eq[k];
+        C_con[k].bottomRightCorner(C_ineq[k].rows(), C_ineq[k].cols()) = C_ineq[k];
+        CC[k] = C_con[k].data();
+
+        // Make block diagonal D
+        D_con[k].setZero(D_eq[k].rows() + D_ineq[k].rows(), D_eq[k].cols() + D_ineq[k].cols());
+        D_con[k].topLeftCorner(D_eq[k].rows(), D_eq[k].cols()) = D_eq[k];
+        D_con[k].bottomRightCorner(D_ineq[k].rows(), D_ineq[k].cols()) = D_ineq[k];
+        DD[k] = D_con[k].data();
+
+        // Equality constraints have both upper and lower bounds (which are the
+        // same). The inequality constraints have only lower bounds: we set it
+        // the same as the lower bound, but then mask out the inequalities
+        // constraints to make them unbounded.
+        d_con[k].setZero(d_eq[k].rows() + d_ineq[k].rows());
+        d_con[k] << d_eq[k], d_ineq[k];
+        llg[k] = d_con[k].data();
+        uug[k] = d_con[k].data();
+
+        upper_bound_mask[k].setZero(d_eq[k].rows() + d_ineq[k].rows());
+        upper_bound_mask[k].head(d_eq[k].rows()).setOnes();
+        d_ocp_qp_set_ug_mask(k, upper_bound_mask[k].data(), &qp_);
+    }
+
     // === Unused ===
     int** hidxbx = nullptr;
     scalar_t** hlbx = nullptr;
@@ -533,9 +607,11 @@ void HpipmInterface::resize(OcpSize ocpSize) {
 
 hpipm_status HpipmInterface::solve(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics,
                                    std::vector<ScalarFunctionQuadraticApproximation>& cost,
-                                   std::vector<VectorFunctionLinearApproximation>* constraints, vector_array_t& stateTrajectory,
+                                   std::vector<VectorFunctionLinearApproximation>* constraints,
+                                   std::vector<VectorFunctionLinearApproximation>* ineqConstraints,
+                                   vector_array_t& stateTrajectory,
                                    vector_array_t& inputTrajectory, bool verbose) {
-  return pImpl_->solve(x0, dynamics, cost, constraints, stateTrajectory, inputTrajectory, verbose);
+  return pImpl_->solve(x0, dynamics, cost, constraints, ineqConstraints, stateTrajectory, inputTrajectory, verbose);
 }
 
 std::vector<ScalarFunctionQuadraticApproximation> HpipmInterface::getRiccatiCostToGo(const VectorFunctionLinearApproximation& dynamics0,

ocs2_sqp/hpipm_catkin/src/OcpSize.cpp

@@ -48,7 +48,8 @@ bool operator==(const OcpSize& lhs, const OcpSize& rhs) noexcept {
 
 OcpSize extractSizesFromProblem(const std::vector<VectorFunctionLinearApproximation>& dynamics,
                                 const std::vector<ScalarFunctionQuadraticApproximation>& cost,
-                                const std::vector<VectorFunctionLinearApproximation>* constraints) {
+                                const std::vector<VectorFunctionLinearApproximation>* constraints,
+                                const std::vector<VectorFunctionLinearApproximation>* ineqConstraints) {
   const int numStages = dynamics.size();
 
   OcpSize problemSize(dynamics.size());
@@ -67,6 +68,11 @@ OcpSize extractSizesFromProblem(const std::vector<VectorFunctionLinearApproximat
       problemSize.numIneqConstraints[k] = (*constraints)[k].f.size();
     }
   }
+  if (ineqConstraints != nullptr) {
+    for (int k = 0; k < numStages + 1; k++) {
+      problemSize.numIneqConstraints[k] += (*ineqConstraints)[k].f.size();
+    }
+  }
 
   return problemSize;
 }