Merged in feature/constraint_array (pull request #646)

Feature/constraint array

Approved-by: Jean-Pierre Sleiman
Approved-by: Sotaro Katayama

Author: farbod-farshidian

ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h

@@ -52,8 +52,8 @@ class StateConstraintCollection : public Collection<StateConstraint> {
   /** Returns the number of active constraints at given time. */
   virtual size_t getNumConstraints(scalar_t time) const;
 
-  /** Get the constraint vector value */
-  virtual vector_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const;
+  /** Get an array of all constraints */
+  virtual vector_array_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const;
 
   /** Get the constraint linear approximation */
   virtual VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state,
@@ -68,4 +68,4 @@ class StateConstraintCollection : public Collection<StateConstraint> {
   StateConstraintCollection(const StateConstraintCollection& other);
 };
 
-}  // namespace ocs2
+}  // namespace ocs2

ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h

@@ -52,8 +52,8 @@ class StateInputConstraintCollection : public Collection<StateInputConstraint> {
   /** Returns the number of active constraints at given time. */
   virtual size_t getNumConstraints(scalar_t time) const;
 
-  /** Get the constraint vector value */
-  virtual vector_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const;
+  /** Get an array of all constraints */
+  virtual vector_array_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const;
 
   /** Get the constraint linear approximation */
   virtual VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input,
@@ -68,4 +68,4 @@ class StateInputConstraintCollection : public Collection<StateInputConstraint> {
   StateInputConstraintCollection(const StateInputConstraintCollection& other);
 };
 
-}  // namespace ocs2
+}  // namespace ocs2

ocs2_core/include/ocs2_core/integration/TrapezoidalIntegration.h

@@ -29,22 +29,32 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
+#include <cassert>
 #include <vector>
 
 namespace ocs2 {
 
-template <typename SCALAR_T>
-SCALAR_T trapezoidalIntegration(const std::vector<SCALAR_T>& timeTrajectory, const std::vector<SCALAR_T>& valueTrajectory) {
+/**
+ * Compute the trapezoidal integration of a trajectory of VALUE_T given the time stamp timeTrajectory and initial value initialValue.
+ *
+ * @note It requires that the VALUE_T has overwrite operator+(VALUE_T, VALUE_T) and define VALUE_T::operator*(SCALAR_T)
+ */
+template <typename SCALAR_T, typename VALUE_T>
+VALUE_T trapezoidalIntegration(const std::vector<SCALAR_T>& timeTrajectory, const std::vector<VALUE_T>& valueTrajectory,
+                               VALUE_T initialValue) {
+  assert(timeTrajectory.size() == valueTrajectory.size());
+
   if (timeTrajectory.size() < 2) {
-    return 0.0;
+    return initialValue;
   }
 
-  SCALAR_T areaUnderCurve = 0.0;
   for (size_t k = 1; k < timeTrajectory.size(); k++) {
-    areaUnderCurve += 0.5 * (valueTrajectory[k] + valueTrajectory[k - 1]) * (timeTrajectory[k] - timeTrajectory[k - 1]);
+    auto temp = valueTrajectory[k - 1] + valueTrajectory[k];
+    temp *= (0.5 * (timeTrajectory[k] - timeTrajectory[k - 1]));
+    initialValue += temp;
   }  // end of k loop
 
-  return areaUnderCurve;
+  return initialValue;
 }
 
 }  // namespace ocs2

ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateConstraint.h

@@ -47,7 +47,7 @@ class LoopshapingStateConstraint final : public StateConstraintCollection {
 
   LoopshapingStateConstraint* clone() const override { return new LoopshapingStateConstraint(*this); }
 
-  vector_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const override;
+  vector_array_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const override;
   VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state,
                                                            const PreComputation& preComp) const override;
   VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state,

ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateInputConstraint.h

@@ -41,7 +41,7 @@ class LoopshapingStateInputConstraint : public StateInputConstraintCollection {
  public:
   ~LoopshapingStateInputConstraint() override = default;
 
-  vector_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const override;
+  vector_array_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const override;
 
  protected:
   LoopshapingStateInputConstraint(const StateInputConstraintCollection& systemConstraint,

ocs2_core/include/ocs2_core/model_data/Metrics.h

@@ -53,83 +53,96 @@ struct LagrangianMetricsConstRef {
 };
 
 /**
- * The collection of cost, equality constraints, and LagrangianMetrics structure for all possible constraint terms (handled by
- * Lagrangian method) in a particular time point.
- * cost : The total cost in a particular time point.
- * stateEqConstraint : A vector of all active state equality constraints.
- * stateInputEqConstraint : A vector of all active state-input equality constraints.
- * stateEqLagrangian : An array of state equality constraint terms handled by Lagrangian method.
- * stateIneqLagrangian : An array of state inequality constraint terms handled by Lagrangian method.
- * stateInputEqLagrangian : An array of state-input equality constraint terms handled by Lagrangian method.
- * stateInputIneqLagrangian : An array of state-input inequality constraint terms handled by Lagrangian method.
+ * The collection of cost, dynamics violation, constraints, and LagrangianMetrics structure for all possible constraint
+ * terms (handled by Lagrangian method) in a point of time.
+ *     cost : The total cost in a particular time point.
+ *     dynamicsViolation : The vector of dynamics violation.
+ *     stateEqConstraint : An array of all state equality constraints.
+ *     stateInputEqConstraint : An array of all state-input equality constraints.
+ *     stateIneqConstraint : An array of all state inequality constraints.
+ *     stateInputIneqConstraint : An array of all state-input inequality constraints.
+ *     stateEqLagrangian : An array of state equality constraint terms handled by Lagrangian method.
+ *     stateIneqLagrangian : An array of state inequality constraint terms handled by Lagrangian method.
+ *     stateInputEqLagrangian : An array of state-input equality constraint terms handled by Lagrangian method.
+ *     stateInputIneqLagrangian : An array of state-input inequality constraint terms handled by Lagrangian method.
  */
-struct MetricsCollection {
+struct Metrics {
   // Cost
   scalar_t cost;
 
+  // Dynamics violation
+  vector_t dynamicsViolation;
+
   // Equality constraints
-  vector_t stateEqConstraint;
-  vector_t stateInputEqConstraint;
+  vector_array_t stateEqConstraint;
+  vector_array_t stateInputEqConstraint;
 
   // Inequality constraints
-  vector_t stateIneqConstraint;
-  vector_t stateInputIneqConstraint;
+  vector_array_t stateIneqConstraint;
+  vector_array_t stateInputIneqConstraint;
 
   // Lagrangians
   std::vector<LagrangianMetrics> stateEqLagrangian;
   std::vector<LagrangianMetrics> stateIneqLagrangian;
   std::vector<LagrangianMetrics> stateInputEqLagrangian;
   std::vector<LagrangianMetrics> stateInputIneqLagrangian;
 
-  /** Exchanges the values of MetricsCollection */
-  void swap(MetricsCollection& other) {
-    // Cost
-    std::swap(cost, other.cost);
-    // Equality constraints
-    stateEqConstraint.swap(other.stateEqConstraint);
-    stateInputEqConstraint.swap(other.stateInputEqConstraint);
-    // Inequality constraints
-    stateIneqConstraint.swap(other.stateIneqConstraint);
-    stateInputIneqConstraint.swap(other.stateInputIneqConstraint);
-    // Lagrangians
-    stateEqLagrangian.swap(other.stateEqLagrangian);
-    stateIneqLagrangian.swap(other.stateIneqLagrangian);
-    stateInputEqLagrangian.swap(other.stateInputEqLagrangian);
-    stateInputIneqLagrangian.swap(other.stateInputIneqLagrangian);
-  }
+  /** Exchanges the values of Metrics */
+  void swap(Metrics& other);
 
-  /** Clears the value of the MetricsCollection */
-  void clear() {
-    // Cost
-    cost = 0.0;
-    // Equality constraints
-    stateEqConstraint = vector_t();
-    stateInputEqConstraint = vector_t();
-    // Inequality constraints
-    stateIneqConstraint = vector_t();
-    stateInputIneqConstraint = vector_t();
-    // Lagrangians
-    stateEqLagrangian.clear();
-    stateIneqLagrangian.clear();
-    stateInputEqLagrangian.clear();
-    stateInputIneqLagrangian.clear();
-  }
+  /** Clears the value of the Metrics */
+  void clear();
+
+  /** Returns true if *this is approximately equal to other, within the precision determined by prec. */
+  bool isApprox(const Metrics& other, scalar_t prec = 1e-8) const;
 };
 
-/** Sums penalties of an array of LagrangianMetrics */
+/** Sums penalties of an array of LagrangianMetrics. */
 inline scalar_t sumPenalties(const std::vector<LagrangianMetrics>& metricsArray) {
   scalar_t s = 0.0;
   std::for_each(metricsArray.begin(), metricsArray.end(), [&s](const LagrangianMetrics& m) { s += m.penalty; });
   return s;
 }
 
-/** Computes the sum of squared norm of constraints of an array of LagrangianMetrics */
+/** Computes the sum of squared norm of constraints of an array of LagrangianMetrics. */
 inline scalar_t constraintsSquaredNorm(const std::vector<LagrangianMetrics>& metricsArray) {
   scalar_t s = 0.0;
   std::for_each(metricsArray.begin(), metricsArray.end(), [&s](const LagrangianMetrics& m) { s += m.constraint.squaredNorm(); });
   return s;
 }
 
+/** Computes the sum of squared norm of a vector of equality constraints violation. */
+inline scalar_t getEqConstraintsSSE(const vector_t& eqConstraint) {
+  if (eqConstraint.size() == 0) {
+    return 0.0;
+  } else {
+    return eqConstraint.squaredNorm();
+  }
+}
+
+/** Computes the sum of squared norm of a vector of inequality constraints violation. */
+inline scalar_t getIneqConstraintsSSE(const vector_t& ineqConstraint) {
+  if (ineqConstraint.size() == 0) {
+    return 0.0;
+  } else {
+    return ineqConstraint.cwiseMin(0.0).squaredNorm();
+  }
+}
+
+/** Computes the sum of squared norm of an array of equality constraints violation. */
+inline scalar_t getEqConstraintsSSE(const vector_array_t& eqConstraint) {
+  scalar_t s = 0.0;
+  std::for_each(eqConstraint.begin(), eqConstraint.end(), [&s](const vector_t& v) { s += getEqConstraintsSSE(v); });
+  return s;
+}
+
+/** Computes the sum of squared norm of an array of inequality constraints violation. */
+inline scalar_t getIneqConstraintsSSE(const vector_array_t& ineqConstraint) {
+  scalar_t s = 0.0;
+  std::for_each(ineqConstraint.begin(), ineqConstraint.end(), [&s](const vector_t& v) { s += getIneqConstraintsSSE(v); });
+  return s;
+}
+
 /**
  * Serializes an array of LagrangianMetrics structures associated to an array of constraint terms.
  *
@@ -138,14 +151,40 @@ inline scalar_t constraintsSquaredNorm(const std::vector<LagrangianMetrics>& met
  */
 vector_t toVector(const std::vector<LagrangianMetrics>& termsLagrangianMetrics);
 
+/**
+ * Serializes an array of constraint terms.
+ *
+ * @ param [in] constraintArray : An array of constraint terms.
+ * @return Serialized vector of the format : (...,  constraintArray[i], ...).
+ */
+vector_t toVector(const vector_array_t& constraintArray);
+
 /**
  * Gets the size of constraint terms.
  *
+ * @ param [in] constraintArray : An array of constraint terms.
+ * @return An array of constraint terms size. It has the same size as the input array.
+ */
+size_array_t getSizes(const vector_array_t& constraintArray);
+
+/**
+ * Gets the size of constraint Lagrangian terms.
+ *
  * @ param [in] termsLagrangianMetrics : LagrangianMetrics associated to an array of constraint terms.
  * @return An array of constraint terms size. It has the same size as the input array.
  */
 size_array_t getSizes(const std::vector<LagrangianMetrics>& termsLagrangianMetrics);
 
+/**
+ * Deserializes the vector to an array of constraint terms.
+ *
+ * @param [in] termsSize : An array of constraint terms size. It as the same size as the output array.
+ * @param [in] vec : Serialized array of constraint terms of the format :
+ *                   (..., constraintArray[i], ...)
+ * @return An array of constraint terms.
+ */
+vector_array_t toConstraintArray(const size_array_t& termsSize, const vector_t& vec);
+
 /**
  * Deserializes the vector to an array of LagrangianMetrics structures based on size of constraint terms.
  *
@@ -171,13 +210,13 @@ namespace LinearInterpolation {
 LagrangianMetrics interpolate(const index_alpha_t& indexAlpha, const std::vector<LagrangianMetricsConstRef>& dataArray);
 
 /**
- * Linearly interpolates a trajectory of MetricsCollection.
+ * Linearly interpolates a trajectory of Metrics.
  *
  * @param [in] indexAlpha : index and interpolation coefficient (alpha) pair.
- * @param [in] dataArray : A trajectory of MetricsCollection.
- * @return The interpolated MetricsCollection at indexAlpha.
+ * @param [in] dataArray : A trajectory of Metrics.
+ * @return The interpolated Metrics at indexAlpha.
  */
-MetricsCollection interpolate(const index_alpha_t& indexAlpha, const std::vector<MetricsCollection>& dataArray);
+Metrics interpolate(const index_alpha_t& indexAlpha, const std::vector<Metrics>& dataArray);
 
 }  // namespace LinearInterpolation
 }  // namespace ocs2

ocs2_core/src/constraint/StateConstraintCollection.cpp

@@ -62,20 +62,13 @@ size_t StateConstraintCollection::getNumConstraints(scalar_t time) const {
 /******************************************************************************************************/
 /******************************************************************************************************/
 /******************************************************************************************************/
-vector_t StateConstraintCollection::getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const {
-  vector_t constraintValues;
-  constraintValues.resize(getNumConstraints(time));
-
-  // append vectors of constraint values from each constraintTerm
-  size_t i = 0;
-  for (const auto& constraintTerm : this->terms_) {
-    if (constraintTerm->isActive(time)) {
-      const auto constraintTermValues = constraintTerm->getValue(time, state, preComp);
-      constraintValues.segment(i, constraintTermValues.rows()) = constraintTermValues;
-      i += constraintTermValues.rows();
+vector_array_t StateConstraintCollection::getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const {
+  vector_array_t constraintValues(this->terms_.size());
+  for (size_t i = 0; i < this->terms_.size(); ++i) {
+    if (this->terms_[i]->isActive(time)) {
+      constraintValues[i] = this->terms_[i]->getValue(time, state, preComp);
     }
-  }
-
+  }  // end of i loop
   return constraintValues;
 }
 

ocs2_core/src/constraint/StateInputConstraintCollection.cpp

@@ -63,21 +63,14 @@ size_t StateInputConstraintCollection::getNumConstraints(scalar_t time) const {
 /******************************************************************************************************/
 /******************************************************************************************************/
 /******************************************************************************************************/
-vector_t StateInputConstraintCollection::getValue(scalar_t time, const vector_t& state, const vector_t& input,
-                                                  const PreComputation& preComp) const {
-  vector_t constraintValues;
-  constraintValues.resize(getNumConstraints(time));
-
-  // append vectors of constraint values from each constraintTerm
-  size_t i = 0;
-  for (const auto& constraintTerm : this->terms_) {
-    if (constraintTerm->isActive(time)) {
-      const auto constraintTermValues = constraintTerm->getValue(time, state, input, preComp);
-      constraintValues.segment(i, constraintTermValues.rows()) = constraintTermValues;
-      i += constraintTermValues.rows();
+vector_array_t StateInputConstraintCollection::getValue(scalar_t time, const vector_t& state, const vector_t& input,
+                                                        const PreComputation& preComp) const {
+  vector_array_t constraintValues(this->terms_.size());
+  for (size_t i = 0; i < this->terms_.size(); ++i) {
+    if (this->terms_[i]->isActive(time)) {
+      constraintValues[i] = this->terms_[i]->getValue(time, state, input, preComp);
     }
-  }
-
+  }  // end of i loop
   return constraintValues;
 }
 

ocs2_core/src/loopshaping/constraint/LoopshapingStateConstraint.cpp

@@ -38,9 +38,9 @@ namespace ocs2 {
 /******************************************************************************************************/
 /******************************************************************************************************/
 /******************************************************************************************************/
-vector_t LoopshapingStateConstraint::getValue(scalar_t t, const vector_t& x, const PreComputation& preComp) const {
+vector_array_t LoopshapingStateConstraint::getValue(scalar_t t, const vector_t& x, const PreComputation& preComp) const {
   if (this->empty()) {
-    return vector_t::Zero(0);
+    return vector_array_t();
   }
 
   const LoopshapingPreComputation& preCompLS = cast<LoopshapingPreComputation>(preComp);

ocs2_core/src/loopshaping/constraint/LoopshapingStateInputConstraint.cpp

@@ -38,9 +38,10 @@ namespace ocs2 {
 /******************************************************************************************************/
 /******************************************************************************************************/
 /******************************************************************************************************/
-vector_t LoopshapingStateInputConstraint::getValue(scalar_t t, const vector_t& x, const vector_t& u, const PreComputation& preComp) const {
+vector_array_t LoopshapingStateInputConstraint::getValue(scalar_t t, const vector_t& x, const vector_t& u,
+                                                         const PreComputation& preComp) const {
   if (this->empty()) {
-    return vector_t::Zero(0);
+    return vector_array_t();
   }
 
   const LoopshapingPreComputation& preCompLS = cast<LoopshapingPreComputation>(preComp);