Merged in feature/sqp_with_metrics (pull request #652)

Feature/sqp with metrics

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

Author: farbod-farshidian

ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h

@@ -49,10 +49,13 @@ class StateConstraintCollection : public Collection<StateConstraint> {
   ~StateConstraintCollection() override = default;
   StateConstraintCollection* clone() const override;
 
-  /** Returns the number of active constraints at given time. */
-  virtual size_t getNumConstraints(scalar_t time) const;
+  /** Returns the number of active constraints at a given time. */
+  size_t getNumConstraints(scalar_t time) const;
 
-  /** Get an array of all constraints */
+  /** Returns the number of active constraints at a given time for each term. If a term is inactive, its size is zero. */
+  size_array_t getTermsSize(scalar_t time) const;
+
+  /** Get an array of all constraints. If a term is inactive, the corresponding element is a vector of size zero. */
   virtual vector_array_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const;
 
   /** Get the constraint linear approximation */

ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h

@@ -49,10 +49,13 @@ class StateInputConstraintCollection : public Collection<StateInputConstraint> {
   ~StateInputConstraintCollection() override = default;
   StateInputConstraintCollection* clone() const override;
 
-  /** Returns the number of active constraints at given time. */
-  virtual size_t getNumConstraints(scalar_t time) const;
+  /** Returns the number of active constraints at a given time. */
+  size_t getNumConstraints(scalar_t time) const;
 
-  /** Get an array of all constraints */
+  /** Returns the number of active constraints at a given time for each term. If a term is inactive, its size is zero. */
+  size_array_t getTermsSize(scalar_t time) const;
+
+  /** Get an array of all constraints. If a term is inactive, the corresponding element is a vector of size zero. */
   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 */

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

@@ -50,6 +50,7 @@ class LoopshapingStateConstraint final : public StateConstraintCollection {
   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,
                                                                  const PreComputation& preComp) const override;
 

ocs2_core/src/constraint/StateConstraintCollection.cpp

@@ -48,17 +48,27 @@ StateConstraintCollection* StateConstraintCollection::clone() const {
 /******************************************************************************************************/
 size_t StateConstraintCollection::getNumConstraints(scalar_t time) const {
   size_t numConstraints = 0;
-
-  // accumulate number of constraints for each constraintTerm
   for (const auto& constraintTerm : this->terms_) {
     if (constraintTerm->isActive(time)) {
       numConstraints += constraintTerm->getNumConstraints(time);
     }
   }
-
   return numConstraints;
 }
 
+/******************************************************************************************************/
+/******************************************************************************************************/
+/******************************************************************************************************/
+size_array_t StateConstraintCollection::getTermsSize(scalar_t time) const {
+  size_array_t termsSize(this->terms_.size(), 0);
+  for (size_t i = 0; i < this->terms_.size(); ++i) {
+    if (this->terms_[i]->isActive(time)) {
+      termsSize[i] = this->terms_[i]->getNumConstraints(time);
+    }
+  }
+  return termsSize;
+}
+
 /******************************************************************************************************/
 /******************************************************************************************************/
 /******************************************************************************************************/

ocs2_core/src/constraint/StateInputConstraintCollection.cpp

@@ -49,17 +49,27 @@ StateInputConstraintCollection* StateInputConstraintCollection::clone() const {
 /******************************************************************************************************/
 size_t StateInputConstraintCollection::getNumConstraints(scalar_t time) const {
   size_t numConstraints = 0;
-
-  // accumulate number of constraints for each constraintTerm
   for (const auto& constraintTerm : this->terms_) {
     if (constraintTerm->isActive(time)) {
       numConstraints += constraintTerm->getNumConstraints(time);
     }
   }
-
   return numConstraints;
 }
 
+/******************************************************************************************************/
+/******************************************************************************************************/
+/******************************************************************************************************/
+size_array_t StateInputConstraintCollection::getTermsSize(scalar_t time) const {
+  size_array_t termsSize(this->terms_.size(), 0);
+  for (size_t i = 0; i < this->terms_.size(); ++i) {
+    if (this->terms_[i]->isActive(time)) {
+      termsSize[i] = this->terms_[i]->getNumConstraints(time);
+    }
+  }
+  return termsSize;
+}
+
 /******************************************************************************************************/
 /******************************************************************************************************/
 /******************************************************************************************************/

ocs2_core/test/constraint/testConstraintCollection.cpp

@@ -27,6 +27,8 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 ******************************************************************************/
 
+#include <numeric>
+
 #include <gtest/gtest.h>
 
 #include <ocs2_core/constraint/StateConstraintCollection.h>
@@ -56,6 +58,37 @@ TEST(TestConstraintCollection, numberOfConstraints) {
   EXPECT_EQ(constraintCollection.getNumConstraints(0.0), addedConstraints);
 }
 
+TEST(TestConstraintCollection, termsSize) {
+  ocs2::StateInputConstraintCollection constraintCollection;
+
+  // Initially we have zero constraints for all types
+  auto termsSize = constraintCollection.getTermsSize(0.0);
+  EXPECT_EQ(termsSize.size(), 0);
+  EXPECT_EQ(std::accumulate(termsSize.begin(), termsSize.end(), 0), 0);
+
+  // Add 2 Linear inequality constraint term, which has 2 constraints
+  constraintCollection.add("Constraint1", std::make_unique<TestDummyConstraint>());
+  constraintCollection.add("Constraint2", std::make_unique<TestDummyConstraint>());
+  constraintCollection.add("Constraint3", std::make_unique<TestDummyConstraint>());
+  auto& constraint1 = constraintCollection.get<TestDummyConstraint>("Constraint1");
+  const size_t constraint1Size = constraint1.getNumConstraints(0.0);
+
+  // Check the right constraint size
+  termsSize = constraintCollection.getTermsSize(0.0);
+  EXPECT_EQ(termsSize.size(), 3);
+  if (termsSize.size() == 3) {
+    EXPECT_EQ(termsSize[0], constraint1Size);
+    EXPECT_EQ(termsSize[1], constraint1Size);
+    EXPECT_EQ(termsSize[2], constraint1Size);
+  }
+
+  // Deactivate constraint1
+  constraint1.setActivity(false);
+  termsSize = constraintCollection.getTermsSize(0.0);
+  EXPECT_EQ(termsSize.size(), 3);
+  EXPECT_EQ(std::accumulate(termsSize.begin(), termsSize.end(), 0), 2 * constraint1Size);
+}
+
 TEST(TestConstraintCollection, activatingConstraints) {
   ocs2::StateInputConstraintCollection constraintCollection;
 

ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h

@@ -89,7 +89,7 @@ class GaussNewtonDDP : public SolverBase {
 
   void getPrimalSolution(scalar_t finalTime, PrimalSolution* primalSolutionPtr) const final;
 
-  const DualSolution& getDualSolution() const override { return optimizedDualSolution_; }
+  const DualSolution* getDualSolution() const override { return &optimizedDualSolution_; }
 
   const ProblemMetrics& getSolutionMetrics() const override { return optimizedProblemMetrics_; }
 

ocs2_ddp/src/DDP_HelperFunctions.cpp

@@ -73,7 +73,7 @@ void computeRolloutMetrics(OptimalControlProblem& problem, const PrimalSolution&
   problemMetrics.intermediates.reserve(tTrajectory.size());
 
   auto nextPostEventIndexItr = postEventIndices.begin();
-  const auto request = Request::Cost + Request::Constraint + Request::SoftConstraint;
+  constexpr auto request = Request::Cost + Request::Constraint + Request::SoftConstraint;
   for (size_t k = 0; k < tTrajectory.size(); k++) {
     // intermediate time cost and constraints
     problem.preComputationPtr->request(request, tTrajectory[k], xTrajectory[k], uTrajectory[k]);

ocs2_oc/CMakeLists.txt

@@ -132,6 +132,7 @@ install(DIRECTORY test/include/${PROJECT_NAME}/
 
 catkin_add_gtest(test_${PROJECT_NAME}_multiple_shooting
   test/multiple_shooting/testProjectionMultiplierCoefficients.cpp
+  test/multiple_shooting/testTranscriptionMetrics.cpp
   test/multiple_shooting/testTranscriptionPerformanceIndex.cpp
 )
 add_dependencies(test_${PROJECT_NAME}_multiple_shooting

ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h

@@ -33,6 +33,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "ocs2_oc/oc_data/PerformanceIndex.h"
 #include "ocs2_oc/oc_data/PrimalSolution.h"
+#include "ocs2_oc/oc_data/ProblemMetrics.h"
 #include "ocs2_oc/oc_data/TimeDiscretization.h"
 
 namespace ocs2 {
@@ -101,5 +102,14 @@ PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSche
 PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSchedule&& modeSchedule, vector_array_t&& x, vector_array_t&& u,
                                 matrix_array_t&& KMatrices);
 
+/**
+ * Constructs a ProblemMetrics from an array of metrics.
+ *
+ * @param [in] time : The annotated time trajectory
+ * @param [in] metrics: The metrics array.
+ * @return The ProblemMetrics.
+ */
+ProblemMetrics toProblemMetrics(const std::vector<AnnotatedTime>& time, std::vector<Metrics>&& metrics);
+
 }  // namespace multiple_shooting
 }  // namespace ocs2