refactored slack and dual initialization

mayataka · mayataka · commit 5cc9bba5cf72 · 2022-12-19T17:59:06.000+01:00
diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt
@@ -73,7 +73,7 @@ if(cmake_clang_tools_FOUND)
     TARGETS ${PROJECT_NAME}
     SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test
     CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
-    CF_WERROR
+    CF_FIX
   )
 endif(cmake_clang_tools_FOUND)
 
diff --git a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h
@@ -115,36 +115,20 @@ scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scala
  *
  * @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.
+ * @param stateIneq: The slack/dual variable trajectory of the state inequality constraints.
+ * @param stateInputIneq: The slack/dual variable trajectory of the state-input inequality constraints.
  * @return A dual solution.
  */
 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);
+                            const vector_array_t& stateIneq, const vector_array_t& stateInputIneq);
 
 /**
- * Extracts a slack variable of the state-only and state-input constraints from a MultiplierCollection
+ * Extracts slack/dual variables of the state-only and state-input constraints from a MultiplierCollection
  *
- * @param[in] multiplierCollection: The MultiplierCollection.
- * @param[out] slackStateIneq: The slack variable of the state inequality constraints.
- * @param[out] dualStateIneq: The dual variable of the state inequality constraints.
- * @param[out] slackStateInputIneq: The slack variable of the state-input inequality constraints.
- * @param[out] dualStateInputIneq: The dual variable of the state-input inequality constraints.
+ * @param multiplierCollection: The MultiplierCollection.
+ * @return slack/dual variables of the state-only (first) and state-input constraints (second).
  */
-void toSlackDual(const MultiplierCollection& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq,
-                 vector_t& slackStateInputIneq, vector_t& dualStateInputIneq);
-
-/**
- * Extracts a slack variable of the state-only constraints from a MultiplierCollection
- *
- * @param[in] multiplierCollection: The MultiplierCollection.
- * @param[out] slackStateIneq: The slack variable of the state inequality constraints.
- * @param[out] dualStateIneq: The dual variable of the state inequality constraints.
- */
-void toSlackDual(const MultiplierCollection& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq);
+std::pair<vector_t, vector_t> fromDualSolution(const MultiplierCollection& multiplierCollection);
 
 }  // namespace ipm
 }  // namespace ocs2
diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h
@@ -202,7 +202,8 @@ class IpmSolver : public SolverBase {
   PrimalSolution primalSolution_;
   vector_array_t costateTrajectory_;
   vector_array_t projectionMultiplierTrajectory_;
-  DualSolution slackDualTrajectory_;
+  DualSolution slackIneqTrajectory_;
+  DualSolution dualIneqTrajectory_;
 
   // Value function in absolute state coordinates (without the constant value)
   std::vector<ScalarFunctionQuadraticApproximation> valueFunction_;
diff --git a/ocs2_ipm/src/IpmHelpers.cpp b/ocs2_ipm/src/IpmHelpers.cpp
@@ -114,26 +114,23 @@ scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scala
 }
 
 namespace {
-MultiplierCollection toMultiplierCollection(const multiple_shooting::ConstraintsSize constraintsSize, const vector_t& slackStateIneq,
-                                            const vector_t& dualStateIneq) {
+MultiplierCollection toMultiplierCollection(const multiple_shooting::ConstraintsSize constraintsSize, const vector_t& stateIneq) {
   MultiplierCollection multiplierCollection;
   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));
+    multiplierCollection.stateIneq.emplace_back(0.0, stateIneq.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);
+MultiplierCollection toMultiplierCollection(const multiple_shooting::ConstraintsSize constraintsSize, const vector_t& stateIneq,
+                                            const vector_t& stateInputIneq) {
+  MultiplierCollection multiplierCollection = toMultiplierCollection(constraintsSize, stateIneq);
   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));
+    multiplierCollection.stateInputIneq.emplace_back(0.0, stateInputIneq.segment(head, size));
+    head += size;
   }
   return multiplierCollection;
 }
@@ -153,22 +150,8 @@ vector_t extractLagrangian(const std::vector<Multiplier>& termsMultiplier) {
 }
 }  // namespace
 
-void toSlackDual(const MultiplierCollection& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq,
-                 vector_t& slackStateInputIneq, vector_t& dualStateInputIneq) {
-  slackStateIneq = extractLagrangian(multiplierCollection.stateIneq);
-  dualStateIneq = extractLagrangian(multiplierCollection.stateEq);
-  slackStateInputIneq = extractLagrangian(multiplierCollection.stateInputIneq);
-  dualStateInputIneq = extractLagrangian(multiplierCollection.stateInputEq);
-}
-
-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, 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) {
+                            const vector_array_t& stateIneq, const vector_array_t& stateInputIneq) {
   // Problem horizon
   const int N = static_cast<int>(time.size()) - 1;
 
@@ -181,18 +164,21 @@ DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, const std::v
 
   for (int i = 0; i < N; ++i) {
     if (time[i].event == AnnotatedTime::Event::PreEvent) {
-      dualSolution.preJumps.emplace_back(toMultiplierCollection(constraintsSize[i], slackStateIneq[i], dualStateIneq[i]));
+      dualSolution.preJumps.emplace_back(toMultiplierCollection(constraintsSize[i], stateIneq[i]));
       dualSolution.intermediates.push_back(dualSolution.intermediates.back());  // no event at the initial node
     } else {
-      dualSolution.intermediates.emplace_back(
-          toMultiplierCollection(constraintsSize[i], slackStateIneq[i], dualStateIneq[i], slackStateInputIneq[i], dualStateInputIneq[i]));
+      dualSolution.intermediates.emplace_back(toMultiplierCollection(constraintsSize[i], stateIneq[i], stateInputIneq[i]));
     }
   }
-  dualSolution.final = toMultiplierCollection(constraintsSize[N], slackStateIneq[N], dualStateIneq[N]);
+  dualSolution.final = toMultiplierCollection(constraintsSize[N], stateIneq[N]);
   dualSolution.intermediates.push_back(dualSolution.intermediates.back());
 
   return dualSolution;
 }
 
+std::pair<vector_t, vector_t> fromDualSolution(const MultiplierCollection& multiplierCollection) {
+  return std::make_pair(extractLagrangian(multiplierCollection.stateIneq), extractLagrangian(multiplierCollection.stateInputIneq));
+}
+
 }  // namespace ipm
 }  // namespace ocs2
diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp
@@ -100,7 +100,8 @@ void IpmSolver::reset() {
   primalSolution_ = PrimalSolution();
   costateTrajectory_.clear();
   projectionMultiplierTrajectory_.clear();
-  slackDualTrajectory_.clear();
+  slackIneqTrajectory_.clear();
+  dualIneqTrajectory_.clear();
   valueFunction_.clear();
   performanceIndeces_.clear();
 
@@ -215,8 +216,9 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f
   multiple_shooting::initializeStateInputTrajectories(initState, timeDiscretization, primalSolution_, *initializerPtr_, x, u);
 
   // Initialize the slack and dual variables of the interior point method
-  if (!slackDualTrajectory_.timeTrajectory.empty()) {
-    std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, slackDualTrajectory_);
+  if (!slackIneqTrajectory_.timeTrajectory.empty()) {
+    std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, slackIneqTrajectory_);
+    std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, dualIneqTrajectory_);
   }
   scalar_t barrierParam = settings_.initialBarrierParameter;
   vector_array_t slackStateIneq, dualStateIneq, slackStateInputIneq, dualStateInputIneq;
@@ -281,8 +283,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, constraintsSize_, slackStateIneq, dualStateIneq, slackStateInputIneq, dualStateInputIneq);
+  slackIneqTrajectory_ = ipm::toDualSolution(timeDiscretization, constraintsSize_, slackStateIneq, slackStateInputIneq);
+  dualIneqTrajectory_ = ipm::toDualSolution(timeDiscretization, constraintsSize_, dualStateIneq, dualStateInputIneq);
   problemMetrics_ = multiple_shooting::toProblemMetrics(timeDiscretization, std::move(metrics));
   computeControllerTimer_.endTimer();
 
@@ -374,8 +376,8 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector<AnnotatedTime>&
                                               const vector_array_t& u, scalar_t barrierParam, vector_array_t& slackStateIneq,
                                               vector_array_t& dualStateIneq, vector_array_t& slackStateInputIneq,
                                               vector_array_t& dualStateInputIneq) {
-  const auto& oldTimeTrajectory = slackDualTrajectory_.timeTrajectory;
-  const auto& oldPostEventIndices = slackDualTrajectory_.postEventIndices;
+  const auto& oldTimeTrajectory = slackIneqTrajectory_.timeTrajectory;
+  const auto& oldPostEventIndices = slackIneqTrajectory_.postEventIndices;
   const auto newTimeTrajectory = toInterpolationTime(timeDiscretization);
   const auto newPostEventIndices = toPostEventIndices(timeDiscretization);
 
@@ -404,8 +406,9 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector<AnnotatedTime>&
   for (size_t i = 0; i < N; i++) {
     if (timeDiscretization[i].event == AnnotatedTime::Event::PreEvent) {
       const auto cachedEventIndex = cacheEventIndexBias + eventIdx;
-      if (cachedEventIndex < slackDualTrajectory_.preJumps.size()) {
-        ipm::toSlackDual(std::move(slackDualTrajectory_.preJumps[cachedEventIndex]), slackStateIneq[i], dualStateIneq[i]);
+      if (cachedEventIndex < slackIneqTrajectory_.preJumps.size()) {
+        std::tie(slackStateIneq[i], std::ignore) = ipm::fromDualSolution(slackIneqTrajectory_.preJumps[cachedEventIndex]);
+        std::tie(dualStateIneq[i], std::ignore) = ipm::fromDualSolution(dualIneqTrajectory_.preJumps[cachedEventIndex]);
       } else {
         const auto time = getIntervalStart(timeDiscretization[i]);
         const auto& state = x[i];
@@ -429,8 +432,10 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector<AnnotatedTime>&
     } else {
       const auto time = getIntervalStart(timeDiscretization[i]);
       if (interpolatableTimePeriod.first <= time && time <= interpolatableTimePeriod.second) {
-        ipm::toSlackDual(getIntermediateDualSolutionAtTime(slackDualTrajectory_, time), slackStateIneq[i], dualStateIneq[i],
-                         slackStateInputIneq[i], dualStateInputIneq[i]);
+        std::tie(slackStateIneq[i], slackStateInputIneq[i]) =
+            ipm::fromDualSolution(getIntermediateDualSolutionAtTime(slackIneqTrajectory_, time));
+        std::tie(dualStateIneq[i], dualStateInputIneq[i]) =
+            ipm::fromDualSolution(getIntermediateDualSolutionAtTime(slackIneqTrajectory_, time));
       } else {
         constexpr auto request = Request::Constraint;
         const auto& state = x[i];
@@ -463,7 +468,8 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector<AnnotatedTime>&
   }
 
   if (interpolateTillFinalTime) {
-    ipm::toSlackDual(std::move(slackDualTrajectory_.final), slackStateIneq[N], dualStateIneq[N]);
+    std::tie(slackStateIneq[N], std::ignore) = ipm::fromDualSolution(slackIneqTrajectory_.final);
+    std::tie(dualStateIneq[N], std::ignore) = ipm::fromDualSolution(dualIneqTrajectory_.final);
   } else {
     const auto time = newTimeTrajectory[N];
     const auto& state = x[N];

Original file line number	Diff line number	Diff line change
`@@ -73,7 +73,7 @@ if(cmake_clang_tools_FOUND)`
`73`	`73`	`TARGETS ${PROJECT_NAME}`
`74`	`74`	`SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test`
`75`	`75`	`CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include`
`76`		`- CF_WERROR`
	`76`	`+ CF_FIX`
`77`	`77`	`)`
`78`	`78`	`endif(cmake_clang_tools_FOUND)`
`79`	`79`