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_oc/test/include/ocs2_oc/test/circular_kinematics.h

@@ -117,6 +117,39 @@ class CircularKinematicsConstraints final : public StateInputConstraint {
   }
 };
 
+/******************************************************************************************************/
+/******************************************************************************************************/
+/******************************************************************************************************/
+/**
+ * Adds a constraint on the state, such that the first coordinate x(0) >= 0.75, which causes the
+ * trajectory to deviate from the unconstrained version.
+ */
+class CircularKinematicsInequalityConstraints final : public StateInputConstraint {
+ public:
+  CircularKinematicsInequalityConstraints() : StateInputConstraint(ConstraintOrder::Linear) {}
+  ~CircularKinematicsInequalityConstraints() override = default;
+
+  CircularKinematicsInequalityConstraints* clone() const override { return new CircularKinematicsInequalityConstraints(*this); }
+
+  size_t getNumConstraints(scalar_t time) const override { return 1; }
+
+  vector_t getValue(scalar_t t, const vector_t& x, const vector_t& u, const PreComputation&) const override {
+    vector_t e(1);
+    e << x(0) - 0.75;
+    return e;
+  }
+
+  VectorFunctionLinearApproximation getLinearApproximation(scalar_t t, const vector_t& x, const vector_t& u,
+                                                           const PreComputation& preComp) const override {
+    VectorFunctionLinearApproximation e;
+    e.f = getValue(t, x, u, preComp);
+    e.dfdx.setZero(1, 2);
+    e.dfdx(0, 0) = 1;
+    e.dfdu.setZero(1, 2);
+    return e;
+  }
+};
+
 /******************************************************************************************************/
 /******************************************************************************************************/
 /******************************************************************************************************/
@@ -136,4 +169,15 @@ inline OptimalControlProblem createCircularKinematicsProblem(const std::string&
   return problem;
 }
 
+/******************************************************************************************************/
+/******************************************************************************************************/
+/******************************************************************************************************/
+inline OptimalControlProblem createCircularKinematicsProblemWithInequality(const std::string& libraryFolder) {
+  // Add an inequality constraint to the problem
+  OptimalControlProblem problem = createCircularKinematicsProblem(libraryFolder);
+  std::unique_ptr<StateInputConstraint> constraint(new CircularKinematicsInequalityConstraints());
+  problem.inequalityConstraintPtr->add("ineqConstraint", std::move(constraint));
+  return problem;
+}
+
 }  // namespace ocs2

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/HpipmInterfaceSettings.h

@@ -54,9 +54,20 @@ struct Settings {
   int warm_start = 0;
   int pred_corr = 1;
   int ric_alg = 0;  // square root ricatti recursion
+
+  // Slacks - soften constraints
+  // TODO we probably want more control over which constraints get slacks,
+  // perhaps in its own structure
+  bool use_slack = false;
+  scalar_t slack_upper_L2_penalty = 1e2;
+  scalar_t slack_lower_L2_penalty = 1e2;
+  scalar_t slack_upper_L1_penalty = 0;
+  scalar_t slack_lower_L1_penalty = 0;
+  scalar_t slack_upper_low_bound = 0;
+  scalar_t slack_lower_low_bound = 0;
 };
 
 std::ostream& operator<<(std::ostream& stream, const Settings& settings);
 
 }  // namespace hpipm_interface
-}  // namespace ocs2
+}  // namespace ocs2

ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h

@@ -81,12 +81,16 @@ 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.
+ * @param useSlack : True if slack variables should be used.
  * @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,
+                                bool useSlack);
 
 }  // namespace hpipm_interface
-}  // namespace ocs2
+}  // namespace ocs2