update after merge

Author: farbod-farshidian

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/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, const std::vector<Metrics>& metrics);
+
 }  // namespace multiple_shooting
 }  // namespace ocs2

ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h

@@ -115,9 +115,12 @@ inline PerformanceIndex operator*(const scalar_t c, PerformanceIndex rhs) {
 /** Swaps performance indices. */
 void swap(PerformanceIndex& lhs, PerformanceIndex& rhs);
 
-/** Computes the PerformanceIndex based on a given Metrics. */
+/** Computes the PerformanceIndex based on a given continuous-time Metrics. */
 PerformanceIndex toPerformanceIndex(const Metrics& m);
 
+/** Computes the PerformanceIndex based on a given discrete-time Metrics. */
+PerformanceIndex toPerformanceIndex(const Metrics& m, const scalar_t dt);
+
 /** Overloads the stream insertion operator. */
 std::ostream& operator<<(std::ostream& stream, const PerformanceIndex& performanceIndex);
 

ocs2_oc/src/multiple_shooting/Helpers.cpp

@@ -119,5 +119,29 @@ PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSche
   return primalSolution;
 }
 
+ProblemMetrics toProblemMetrics(const std::vector<AnnotatedTime>& time, const std::vector<Metrics>& metrics) {
+  assert(time.size() > 1);
+  assert(metrics.size() == time.size());
+
+  // Problem horizon
+  const int N = static_cast<int>(time.size()) - 1;
+
+  // resize
+  ProblemMetrics problemMetrics;
+  problemMetrics.intermediates.reserve(N);
+  problemMetrics.preJumps.reserve(8);  // the size is just a guess
+  problemMetrics.final = metrics.back();
+
+  for (int i = 0; i < N; ++i) {
+    if (time[i].event == AnnotatedTime::Event::PreEvent) {
+      problemMetrics.preJumps.push_back(metrics[i]);
+    } else {
+      problemMetrics.intermediates.push_back(metrics[i]);
+    }
+  }
+
+  return problemMetrics;
+}
+
 }  // namespace multiple_shooting
 }  // namespace ocs2

ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp

@@ -60,12 +60,7 @@ PerformanceIndex computeIntermediatePerformance(const Transcription& transcripti
 PerformanceIndex computeIntermediatePerformance(OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t,
                                                 scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u) {
   const auto metrics = computeIntermediateMetrics(optimalControlProblem, discretizer, t, dt, x, x_next, u);
-  auto performanceIndex = toPerformanceIndex(metrics);
-  //  performanceIndex.cost *= dt  no need since it is already considered in multiple_shooting::computeIntermediateMetrics()
-  performanceIndex.dynamicsViolationSSE *= dt;
-  performanceIndex.equalityConstraintsSSE *= dt;
-  performanceIndex.inequalityConstraintsSSE *= dt;
-  return performanceIndex;
+  return toPerformanceIndex(metrics, dt);
 }
 
 PerformanceIndex computeTerminalPerformance(const TerminalTranscription& transcription) {

ocs2_oc/src/oc_data/PerformanceIndex.cpp

@@ -98,6 +98,16 @@ PerformanceIndex toPerformanceIndex(const Metrics& m) {
   return performanceIndex;
 }
 
+PerformanceIndex toPerformanceIndex(const Metrics& m, const scalar_t dt) {
+  auto performanceIndex = toPerformanceIndex(m);
+  //  performanceIndex.cost *= dt  no need since it is already considered in multiple_shooting::computeIntermediateMetrics()
+  performanceIndex.dualFeasibilitiesSSE *= dt;
+  performanceIndex.dynamicsViolationSSE *= dt;
+  performanceIndex.equalityConstraintsSSE *= dt;
+  performanceIndex.inequalityConstraintsSSE *= dt;
+  return performanceIndex;
+}
+
 std::ostream& operator<<(std::ostream& stream, const PerformanceIndex& performanceIndex) {
   const size_t tabSpace = 12;
   const auto indentation = stream.width();

ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h

@@ -125,7 +125,7 @@ class SqpSolver : public SolverBase {
 
   /** Computes only the performance metrics at the current {t, x(t), u(t)} */
   PerformanceIndex computePerformance(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x,
-                                      const vector_array_t& u, ProblemMetrics& problemMetrics);
+                                      const vector_array_t& u, std::vector<Metrics>& metrics);
 
   /** Returns solution of the QP subproblem in delta coordinates: */
   struct OcpSubproblemSolution {
@@ -143,7 +143,8 @@ class SqpSolver : public SolverBase {
 
   /** Decides on the step to take and overrides given trajectories {x(t), u(t)} <- {x(t) + a*dx(t), u(t) + a*du(t)} */
   sqp::StepInfo takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, const vector_t& initState,
-                         const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, vector_array_t& u);
+                         const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, vector_array_t& u,
+                         std::vector<Metrics>& metrics);
 
   /** Determine convergence after a step */
   sqp::Convergence checkConvergence(int iteration, const PerformanceIndex& baseline, const sqp::StepInfo& stepInfo) const;
@@ -184,8 +185,6 @@ class SqpSolver : public SolverBase {
 
   // The ProblemMetrics associated to primalSolution_
   ProblemMetrics problemMetrics_;
-  // Memory used within the search strategy
-  ProblemMetrics problemMetricsNew_;
 
   // Benchmarking
   size_t numProblems_{0};

ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp

@@ -35,6 +35,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <ocs2_oc/multiple_shooting/Helpers.h>
 #include <ocs2_oc/multiple_shooting/Initialization.h>
+#include <ocs2_oc/multiple_shooting/MetricsComputation.h>
 #include <ocs2_oc/multiple_shooting/PerformanceIndexComputation.h>
 #include <ocs2_oc/multiple_shooting/Transcription.h>
 #include <ocs2_oc/oc_problem/OcpSize.h>
@@ -176,6 +177,7 @@ void SqpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f
 
   // Bookkeeping
   performanceIndeces_.clear();
+  std::vector<Metrics> metrics(timeDiscretization.size());
 
   int iter = 0;
   sqp::Convergence convergence = sqp::Convergence::FALSE;
@@ -197,7 +199,7 @@ void SqpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f
 
     // Apply step
     linesearchTimer_.startTimer();
-    const auto stepInfo = takeStep(baselinePerformance, timeDiscretization, initState, deltaSolution, x, u);
+    const auto stepInfo = takeStep(baselinePerformance, timeDiscretization, initState, deltaSolution, x, u, metrics);
     performanceIndeces_.push_back(stepInfo.performanceAfterStep);
     linesearchTimer_.endTimer();
 
@@ -211,6 +213,7 @@ void SqpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f
 
   computeControllerTimer_.startTimer();
   primalSolution_ = toPrimalSolution(timeDiscretization, std::move(x), std::move(u));
+  problemMetrics_ = multiple_shooting::toProblemMetrics(timeDiscretization, metrics);
   computeControllerTimer_.endTimer();
 
   ++numProblems_;
@@ -362,21 +365,11 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated
 }
 
 PerformanceIndex SqpSolver::computePerformance(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x,
-                                               const vector_array_t& u, ProblemMetrics& problemMetrics) {
-  // Problem horizon
+                                               const vector_array_t& u, std::vector<Metrics>& metrics) {
+  // Problem size
   const int N = static_cast<int>(time.size()) - 1;
-
-  // extract indices for setting up problemMetrics
-  int numEvents = 0, numIntermediates = 0;
-  std::vector<int> indices(N);
-  for (int i = 0; i < N; ++i) {
-    indices[i] = (time[i].event == AnnotatedTime::Event::PreEvent) ? numEvents++ : numIntermediates++;
-  }
-
-  // resize
-  problemMetrics.clear();
-  problemMetrics.preJumps.resize(numEvents);
-  problemMetrics.intermediates.resize(numIntermediates);
+  metrics.clear();
+  metrics.resize(N + 1);
 
   std::vector<PerformanceIndex> performance(settings_.nThreads, PerformanceIndex());
   std::atomic_int timeIndex{0};
@@ -386,34 +379,32 @@ PerformanceIndex SqpSolver::computePerformance(const std::vector<AnnotatedTime>&
 
     int i = timeIndex++;
     while (i < N) {
-      const auto problemMetricsIndex = indices[i];
       if (time[i].event == AnnotatedTime::Event::PreEvent) {
         // Event node
-        problemMetrics.preJumps[problemMetricsIndex] = multiple_shooting::computeEventMetrics(ocpDefinition, time[i].time, x[i], x[i + 1]);
-        performance[workerId] += toPerformanceIndex(problemMetrics.preJumps[problemMetricsIndex]);
+        metrics[i] = multiple_shooting::computeEventMetrics(ocpDefinition, time[i].time, x[i], x[i + 1]);
+        performance[workerId] += toPerformanceIndex(metrics[i]);
       } else {
         // Normal, intermediate node
         const scalar_t ti = getIntervalStart(time[i]);
         const scalar_t dt = getIntervalDuration(time[i], time[i + 1]);
-        problemMetrics.intermediates[problemMetricsIndex] =
-            multiple_shooting::computeIntermediateMetrics(ocpDefinition, discretizer_, ti, dt, x[i], x[i + 1], u[i]);
-        performance[workerId] += toPerformanceIndex(problemMetrics.intermediates[problemMetricsIndex], dt);
+        metrics[i] = multiple_shooting::computeIntermediateMetrics(ocpDefinition, discretizer_, ti, dt, x[i], x[i + 1], u[i]);
+        performance[workerId] += toPerformanceIndex(metrics[i], dt);
       }
 
       i = timeIndex++;
     }
 
     if (i == N) {  // Only one worker will execute this
       const scalar_t tN = getIntervalStart(time[N]);
-      problemMetrics.final = multiple_shooting::computeTerminalMetrics(ocpDefinition, tN, x[N]);
-      performance[workerId] += toPerformanceIndex(problemMetrics.final);
+      metrics[N] = multiple_shooting::computeTerminalMetrics(ocpDefinition, tN, x[N]);
+      performance[workerId] += toPerformanceIndex(metrics[N]);
     }
   };
   runParallel(std::move(parallelTask));
 
   // Account for initial state in performance
   const vector_t initDynamicsViolation = initState - x.front();
-  problemMetrics.intermediates.front().dynamicsViolation += initDynamicsViolation;
+  metrics.front().dynamicsViolation += initDynamicsViolation;
   performance.front().dynamicsViolationSSE += initDynamicsViolation.squaredNorm();
 
   // Sum performance of the threads
@@ -424,7 +415,7 @@ PerformanceIndex SqpSolver::computePerformance(const std::vector<AnnotatedTime>&
 
 sqp::StepInfo SqpSolver::takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization,
                                   const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x,
-                                  vector_array_t& u) {
+                                  vector_array_t& u, std::vector<Metrics>& metrics) {
   using StepType = FilterLinesearch::StepType;
 
   /*
@@ -450,13 +441,14 @@ sqp::StepInfo SqpSolver::takeStep(const PerformanceIndex& baseline, const std::v
   scalar_t alpha = 1.0;
   vector_array_t xNew(x.size());
   vector_array_t uNew(u.size());
+  std::vector<Metrics> metricsNew(x.size());
   do {
     // Compute step
     multiple_shooting::incrementTrajectory(u, du, alpha, uNew);
     multiple_shooting::incrementTrajectory(x, dx, alpha, xNew);
 
     // Compute cost and constraints
-    const PerformanceIndex performanceNew = computePerformance(timeDiscretization, initState, xNew, uNew);
+    const PerformanceIndex performanceNew = computePerformance(timeDiscretization, initState, xNew, uNew, metricsNew);
 
     // Step acceptance and record step type
     bool stepAccepted;
@@ -474,7 +466,7 @@ sqp::StepInfo SqpSolver::takeStep(const PerformanceIndex& baseline, const std::v
     if (stepAccepted) {  // Return if step accepted
       x = std::move(xNew);
       u = std::move(uNew);
-      problemMetrics.swap(problemMetricsNew_);
+      metrics = std::move(metricsNew);
 
       // Prepare step info
       sqp::StepInfo stepInfo;
@@ -508,8 +500,8 @@ sqp::StepInfo SqpSolver::takeStep(const PerformanceIndex& baseline, const std::v
   stepInfo.du_norm = 0.0;
   stepInfo.performanceAfterStep = baseline;
   stepInfo.totalConstraintViolationAfterStep = FilterLinesearch::totalConstraintViolation(baseline);
-
-  std::ignore = computePerformance(timeDiscretization, initState, xNew, uNew, problemMetrics);
+  // compute metrics for the baseline rollout
+  std::ignore = computePerformance(timeDiscretization, initState, x, u, metrics);
 
   if (settings_.printLinesearch) {
     std::cerr << "[Linesearch terminated] Step size: " << stepInfo.stepSize << ", Step Type: " << toString(stepInfo.stepType) << "\n";