modify the dual solution that stores the slack and dual trajectory

Author: mayataka

ocs2_ipm/include/ocs2_ipm/IpmHelpers.h

@@ -30,6 +30,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #pragma once
 
 #include <ocs2_core/Types.h>
+#include <ocs2_oc/multiple_shooting/Transcription.h>
 #include <ocs2_oc/oc_data/DualSolution.h>
 #include <ocs2_oc/oc_data/TimeDiscretization.h>
 #include <ocs2_oc/oc_problem/OptimalControlProblem.h>
@@ -113,14 +114,16 @@ scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scala
  * Convert the optimized slack or dual trajectories as a DualSolution.
  *
  * @param time: The time discretization.
+ * @param constraintsSize: The constraint tems size.
  * @param slackStateIneq: The slack variable trajectory of the state inequality constraints.
  * @param dualStateIneq: The dual variable trajectory of the state inequality constraints.
  * @param slackStateInputIneq: The slack variable trajectory of the state-input inequality constraints.
  * @param dualStateInputIneq: The dual variable trajectory of the state-input inequality constraints.
  * @return A dual solution.
  */
-DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& slackStateIneq, vector_array_t&& dualStateIneq,
-                            vector_array_t&& slackStateInputIneq, vector_array_t&& dualStateInputIneq);
+DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, const std::vector<multiple_shooting::ConstraintsSize>& constraintsSize,
+                            const vector_array_t& slackStateIneq, const vector_array_t& dualStateIneq,
+                            const vector_array_t& slackStateInputIneq, const vector_array_t& dualStateInputIneq);
 
 /**
  * Extracts a slack variable of the state-only and state-input constraints from a MultiplierCollection
@@ -131,7 +134,7 @@ DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, vector_array
  * @param[out] slackStateInputIneq: The slack variable of the state-input inequality constraints.
  * @param[out] dualStateInputIneq: The dual variable of the state-input inequality constraints.
  */
-void toSlackDual(MultiplierCollection&& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq,
+void toSlackDual(const MultiplierCollection& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq,
                  vector_t& slackStateInputIneq, vector_t& dualStateInputIneq);
 
 /**
@@ -141,7 +144,7 @@ void toSlackDual(MultiplierCollection&& multiplierCollection, vector_t& slackSta
  * @param[out] slackStateIneq: The slack variable of the state inequality constraints.
  * @param[out] dualStateIneq: The dual variable of the state inequality constraints.
  */
-void toSlackDual(MultiplierCollection&& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq);
+void toSlackDual(const MultiplierCollection& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq);
 
 }  // namespace ipm
 }  // namespace ocs2

ocs2_ipm/include/ocs2_ipm/IpmSolver.h

@@ -35,6 +35,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <ocs2_core/thread_support/ThreadPool.h>
 
 #include <ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h>
+#include <ocs2_oc/multiple_shooting/Transcription.h>
 #include <ocs2_oc/oc_data/TimeDiscretization.h>
 #include <ocs2_oc/oc_problem/OptimalControlProblem.h>
 #include <ocs2_oc/oc_solver/SolverBase.h>
@@ -214,6 +215,9 @@ class IpmSolver : public SolverBase {
   std::vector<VectorFunctionLinearApproximation> stateInputIneqConstraints_;
   std::vector<VectorFunctionLinearApproximation> constraintsProjection_;
 
+  // Constraint terms size
+  std::vector<multiple_shooting::ConstraintsSize> constraintsSize_;
+
   // Lagrange multipliers
   std::vector<multiple_shooting::ProjectionMultiplierCoefficients> projectionMultiplierCoefficients_;
 

ocs2_ipm/src/IpmHelpers.cpp

@@ -114,32 +114,61 @@ scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scala
 }
 
 namespace {
-MultiplierCollection toMultiplierCollection(vector_t&& slackStateIneq, vector_t&& dualStateIneq,
-                                            vector_t&& slackStateInputIneq = vector_t(), vector_t&& dualStateInputIneq = vector_t()) {
+MultiplierCollection toMultiplierCollection(const multiple_shooting::ConstraintsSize constraintsSize, const vector_t& slackStateIneq,
+                                            const vector_t& dualStateIneq) {
   MultiplierCollection multiplierCollection;
-  multiplierCollection.stateIneq.emplace_back(0.0, std::move(slackStateIneq));
-  multiplierCollection.stateEq.emplace_back(0.0, std::move(dualStateIneq));
-  multiplierCollection.stateInputIneq.emplace_back(0.0, std::move(slackStateInputIneq));
-  multiplierCollection.stateInputEq.emplace_back(0.0, std::move(dualStateInputIneq));
+  size_t head = 0;
+  for (const size_t size : constraintsSize.stateIneq) {
+    multiplierCollection.stateIneq.emplace_back(0.0, slackStateIneq.segment(head, size));
+    multiplierCollection.stateEq.emplace_back(0.0, dualStateIneq.segment(head, size));
+    head += size;
+  }
+  return multiplierCollection;
+}
+
+MultiplierCollection toMultiplierCollection(const multiple_shooting::ConstraintsSize constraintsSize, const vector_t& slackStateIneq,
+                                            const vector_t& dualStateIneq, const vector_t& slackStateInputIneq,
+                                            const vector_t& dualStateInputIneq) {
+  MultiplierCollection multiplierCollection = toMultiplierCollection(constraintsSize, slackStateIneq, dualStateIneq);
+  size_t head = 0;
+  for (const size_t size : constraintsSize.stateInputIneq) {
+    multiplierCollection.stateInputIneq.emplace_back(0.0, slackStateInputIneq.segment(head, size));
+    multiplierCollection.stateInputEq.emplace_back(0.0, dualStateInputIneq.segment(head, size));
+  }
   return multiplierCollection;
 }
+
+vector_t extractLagrangian(const std::vector<Multiplier>& termsMultiplier) {
+  size_t n = 0;
+  std::for_each(termsMultiplier.begin(), termsMultiplier.end(), [&](const Multiplier& m) { n += m.lagrangian.size(); });
+
+  vector_t vec(n);
+  size_t head = 0;
+  for (const auto& m : termsMultiplier) {
+    vec.segment(head, m.lagrangian.size()) = m.lagrangian;
+    head += m.lagrangian.size();
+  }  // end of i loop
+
+  return vec;
+}
 }  // namespace
 
-void toSlackDual(MultiplierCollection&& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq,
+void toSlackDual(const MultiplierCollection& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq,
                  vector_t& slackStateInputIneq, vector_t& dualStateInputIneq) {
-  slackStateIneq = std::move(multiplierCollection.stateIneq.front().lagrangian);
-  dualStateIneq = std::move(multiplierCollection.stateEq.front().lagrangian);
-  slackStateInputIneq = std::move(multiplierCollection.stateInputIneq.front().lagrangian);
-  dualStateInputIneq = std::move(multiplierCollection.stateInputEq.front().lagrangian);
+  slackStateIneq = extractLagrangian(multiplierCollection.stateIneq);
+  dualStateIneq = extractLagrangian(multiplierCollection.stateEq);
+  slackStateInputIneq = extractLagrangian(multiplierCollection.stateInputIneq);
+  dualStateInputIneq = extractLagrangian(multiplierCollection.stateInputEq);
 }
 
-void toSlackDual(MultiplierCollection&& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq) {
-  slackStateIneq = std::move(multiplierCollection.stateIneq.front().lagrangian);
-  dualStateIneq = std::move(multiplierCollection.stateEq.front().lagrangian);
+void toSlackDual(const MultiplierCollection& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq) {
+  slackStateIneq = extractLagrangian(multiplierCollection.stateIneq);
+  dualStateIneq = extractLagrangian(multiplierCollection.stateEq);
 }
 
-DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& slackStateIneq, vector_array_t&& dualStateIneq,
-                            vector_array_t&& slackStateInputIneq, vector_array_t&& dualStateInputIneq) {
+DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, const std::vector<multiple_shooting::ConstraintsSize>& constraintsSize,
+                            const vector_array_t& slackStateIneq, const vector_array_t& dualStateIneq,
+                            const vector_array_t& slackStateInputIneq, const vector_array_t& dualStateInputIneq) {
   // Problem horizon
   const int N = static_cast<int>(time.size()) - 1;
 
@@ -152,14 +181,14 @@ DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, vector_array
 
   for (int i = 0; i < N; ++i) {
     if (time[i].event == AnnotatedTime::Event::PreEvent) {
-      dualSolution.preJumps.emplace_back(toMultiplierCollection(std::move(slackStateIneq[i]), std::move(dualStateIneq[i])));
+      dualSolution.preJumps.emplace_back(toMultiplierCollection(constraintsSize[i], slackStateIneq[i], dualStateIneq[i]));
       dualSolution.intermediates.push_back(dualSolution.intermediates.back());  // no event at the initial node
     } else {
-      dualSolution.intermediates.emplace_back(toMultiplierCollection(std::move(slackStateIneq[i]), std::move(dualStateIneq[i]),
-                                                                     std::move(slackStateInputIneq[i]), std::move(dualStateInputIneq[i])));
+      dualSolution.intermediates.emplace_back(
+          toMultiplierCollection(constraintsSize[i], slackStateIneq[i], dualStateIneq[i], slackStateInputIneq[i], dualStateInputIneq[i]));
     }
   }
-  dualSolution.final = toMultiplierCollection(std::move(slackStateIneq[N]), std::move(dualStateIneq[N]));
+  dualSolution.final = toMultiplierCollection(constraintsSize[N], slackStateIneq[N], dualStateIneq[N]);
   dualSolution.intermediates.push_back(dualSolution.intermediates.back());
 
   return dualSolution;

ocs2_ipm/src/IpmSolver.cpp

@@ -39,7 +39,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <ocs2_oc/multiple_shooting/LagrangianEvaluation.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>
 #include <ocs2_oc/trajectory_adjustment/TrajectorySpreadingHelperFunctions.h>
 
@@ -282,8 +281,8 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f
   primalSolution_ = toPrimalSolution(timeDiscretization, std::move(x), std::move(u));
   costateTrajectory_ = std::move(lmd);
   projectionMultiplierTrajectory_ = std::move(nu);
-  slackDualTrajectory_ = ipm::toDualSolution(timeDiscretization, std::move(slackStateIneq), std::move(dualStateIneq),
-                                             std::move(slackStateInputIneq), std::move(dualStateInputIneq));
+  slackDualTrajectory_ =
+      ipm::toDualSolution(timeDiscretization, constraintsSize_, slackStateIneq, dualStateIneq, slackStateInputIneq, dualStateInputIneq);
   problemMetrics_ = multiple_shooting::toProblemMetrics(timeDiscretization, std::move(metrics));
   computeControllerTimer_.endTimer();
 
@@ -635,6 +634,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated
   stateInputIneqConstraints_.resize(N + 1);
   constraintsProjection_.resize(N);
   projectionMultiplierCoefficients_.resize(N);
+  constraintsSize_.resize(N + 1);
   metrics.resize(N + 1);
 
   std::atomic_int timeIndex{0};
@@ -655,6 +655,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated
         stateInputIneqConstraints_[i].resize(0, x[i].size());
         constraintsProjection_[i].resize(0, x[i].size());
         projectionMultiplierCoefficients_[i] = multiple_shooting::ProjectionMultiplierCoefficients();
+        constraintsSize_[i] = std::move(result.constraintsSize);
         if (settings_.computeLagrangeMultipliers) {
           lagrangian_[i] = multiple_shooting::evaluateLagrangianEventNode(lmd[i], lmd[i + 1], std::move(result.cost), dynamics_[i]);
         } else {
@@ -684,6 +685,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated
         stateInputIneqConstraints_[i] = std::move(result.stateInputIneqConstraints);
         constraintsProjection_[i] = std::move(result.constraintsProjection);
         projectionMultiplierCoefficients_[i] = std::move(result.projectionMultiplierCoefficients);
+        constraintsSize_[i] = std::move(result.constraintsSize);
         if (settings_.computeLagrangeMultipliers) {
           lagrangian_[i] = multiple_shooting::evaluateLagrangianIntermediateNode(lmd[i], lmd[i + 1], nu[i], std::move(result.cost),
                                                                                  dynamics_[i], stateInputEqConstraints_[i]);
@@ -711,6 +713,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated
       performance[workerId] += ipm::computePerformanceIndex(result, barrierParam, slackStateIneq[N]);
       stateInputEqConstraints_[i].resize(0, x[i].size());
       stateIneqConstraints_[i] = std::move(result.ineqConstraints);
+      constraintsSize_[i] = std::move(result.constraintsSize);
       if (settings_.computeLagrangeMultipliers) {
         lagrangian_[i] = multiple_shooting::evaluateLagrangianTerminalNode(lmd[i], std::move(result.cost));
       } else {