modify around computeLagrangeMultiplier option

Author: mayataka

ocs2_ipm/include/ocs2_ipm/IpmSettings.h

@@ -54,8 +54,9 @@ struct Settings {
   scalar_t gamma_c = 1e-6;       // (3): ELSE REQUIRE c{i+1} < (c{i} - gamma_c * g{i}) OR g{i+1} < (1-gamma_c) * g{i}
 
   // controller type
-  bool useFeedbackPolicy = true;     // true to use feedback, false to use feedforward
-  bool createValueFunction = false;  // true to store the value function, false to ignore it
+  bool useFeedbackPolicy = true;            // true to use feedback, false to use feedforward
+  bool createValueFunction = false;         // true to store the value function, false to ignore it
+  bool computeLagrangeMultipliers = false;  // true to compute the Lagrange multipliers
 
   // QP subproblem solver settings
   hpipm_interface::Settings hpipmSettings = hpipm_interface::Settings();
@@ -66,7 +67,6 @@ struct Settings {
 
   // Equality constraints
   bool projectStateInputEqualityConstraints = true;  // Use a projection method to resolve the state-input constraint Cx+Du+e
-  bool computeLagrangeMultipliers = true;            // compute the Lagrange multipliers to evaluate the SSE of the dual feasibilities
 
   // Barrier strategy of the primal-dual interior point method. Conventions follows Ipopt.
   scalar_t initialBarrierParameter = 1.0e-02;  // Initial value of the barrier parameter
@@ -107,7 +107,7 @@ struct Settings {
  * @param [in] verbose: Flag to determine whether to print out the loaded settings or not.
  * @return The settings
  */
-Settings loadSettings(const std::string& filename, const std::string& fieldName = "multiple_shooting", bool verbose = true);
+Settings loadSettings(const std::string& filename, const std::string& fieldName = "ipm", bool verbose = true);
 
 }  // namespace ipm
 }  // namespace ocs2

ocs2_ipm/src/IpmSettings.cpp

@@ -62,6 +62,7 @@ Settings loadSettings(const std::string& filename, const std::string& fieldName,
   loadData::loadPtreeValue(pt, settings.dt, fieldName + ".dt", verbose);
   loadData::loadPtreeValue(pt, settings.useFeedbackPolicy, fieldName + ".useFeedbackPolicy", verbose);
   loadData::loadPtreeValue(pt, settings.createValueFunction, fieldName + ".createValueFunction", verbose);
+  loadData::loadPtreeValue(pt, settings.computeLagrangeMultipliers, fieldName + ".computeLagrangeMultipliers", verbose);
   auto integratorName = sensitivity_integrator::toString(settings.integratorType);
   loadData::loadPtreeValue(pt, integratorName, fieldName + ".integratorType", verbose);
   settings.integratorType = sensitivity_integrator::fromString(integratorName);
@@ -74,7 +75,6 @@ Settings loadSettings(const std::string& filename, const std::string& fieldName,
   loadData::loadPtreeValue(pt, settings.fractionToBoundaryMargin, fieldName + ".fractionToBoundaryMargin", verbose);
   loadData::loadPtreeValue(pt, settings.usePrimalStepSizeForDual, fieldName + ".usePrimalStepSizeForDual", verbose);
   loadData::loadPtreeValue(pt, settings.projectStateInputEqualityConstraints, fieldName + ".projectStateInputEqualityConstraints", verbose);
-  loadData::loadPtreeValue(pt, settings.computeLagrangeMultipliers, fieldName + ".computeLagrangeMultipliers", verbose);
   loadData::loadPtreeValue(pt, settings.initialSlackLowerBound, fieldName + ".initialSlackLowerBound", verbose);
   loadData::loadPtreeValue(pt, settings.initialDualLowerBound, fieldName + ".initialDualLowerBound", verbose);
   loadData::loadPtreeValue(pt, settings.initialSlackMarginRate, fieldName + ".initialSlackMarginRate", verbose);

ocs2_ipm/src/IpmSolver.cpp

@@ -50,6 +50,10 @@ namespace ocs2 {
 
 namespace {
 ipm::Settings rectifySettings(const OptimalControlProblem& ocp, ipm::Settings&& settings) {
+  // We have to create the value function if we want to compute the Lagrange multipliers.
+  if (settings.computeLagrangeMultipliers) {
+    settings.createValueFunction = true;
+  }
   // True does not make sense if there are no constraints.
   if (ocp.equalityConstraintPtr->empty()) {
     settings.projectStateInputEqualityConstraints = false;
@@ -463,18 +467,20 @@ IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta
 
 void IpmSolver::extractValueFunction(const std::vector<AnnotatedTime>& time, const vector_array_t& x, const vector_array_t& lmd,
                                      const vector_array_t& deltaXSol, vector_array_t& deltaLmdSol) {
-  valueFunction_ = hpipmInterface_.getRiccatiCostToGo(dynamics_[0], lagrangian_[0]);
-  // Compute costate directions
-  deltaLmdSol.resize(deltaXSol.size());
-  for (int i = 0; i < time.size(); ++i) {
-    deltaLmdSol[i] = valueFunction_[i].dfdx;
-    deltaLmdSol[i].noalias() += valueFunction_[i].dfdxx * x[i];
-  }
-  // Correct for linearization state
-  for (int i = 0; i < time.size(); ++i) {
-    valueFunction_[i].dfdx.noalias() -= valueFunction_[i].dfdxx * x[i];
-    if (settings_.computeLagrangeMultipliers) {
-      valueFunction_[i].dfdx.noalias() += lmd[i];
+  if (settings_.createValueFunction) {
+    valueFunction_ = hpipmInterface_.getRiccatiCostToGo(dynamics_[0], lagrangian_[0]);
+    // Compute costate directions
+    deltaLmdSol.resize(deltaXSol.size());
+    for (int i = 0; i < time.size(); ++i) {
+      deltaLmdSol[i] = valueFunction_[i].dfdx;
+      deltaLmdSol[i].noalias() += valueFunction_[i].dfdxx * x[i];
+    }
+    // Correct for linearization state
+    for (int i = 0; i < time.size(); ++i) {
+      valueFunction_[i].dfdx.noalias() -= valueFunction_[i].dfdxx * x[i];
+      if (settings_.computeLagrangeMultipliers) {
+        valueFunction_[i].dfdx.noalias() += lmd[i];
+      }
     }
   }
 }
@@ -551,9 +557,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated
         }
 
         ipm::condenseIneqConstraints(barrierParam, slackStateIneq[i], dualStateIneq[i], stateIneqConstraints_[i], lagrangian_[i]);
-        if (settings_.computeLagrangeMultipliers) {
-          performance[workerId].dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[i]);
-        }
+        performance[workerId].dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[i]);
         performance[workerId].dualFeasibilitiesSSE +=
             ipm::evaluateComplementarySlackness(barrierParam, slackStateIneq[i], dualStateIneq[i]);
       } else {
@@ -597,9 +601,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated
         ipm::condenseIneqConstraints(barrierParam, slackStateIneq[i], dualStateIneq[i], stateIneqConstraints_[i], lagrangian_[i]);
         ipm::condenseIneqConstraints(barrierParam, slackStateInputIneq[i], dualStateInputIneq[i], stateInputIneqConstraints_[i],
                                      lagrangian_[i]);
-        if (settings_.computeLagrangeMultipliers) {
-          performance[workerId].dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[i]);
-        }
+        performance[workerId].dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[i]);
         performance[workerId].dualFeasibilitiesSSE +=
             ipm::evaluateComplementarySlackness(barrierParam, slackStateIneq[i], dualStateIneq[i]);
         performance[workerId].dualFeasibilitiesSSE +=
@@ -628,9 +630,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated
         lagrangian_[i] = std::move(result.cost);
       }
       ipm::condenseIneqConstraints(barrierParam, slackStateIneq[N], dualStateIneq[N], stateIneqConstraints_[N], lagrangian_[N]);
-      if (settings_.computeLagrangeMultipliers) {
-        performance[workerId].dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[N]);
-      }
+      performance[workerId].dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[N]);
       performance[workerId].dualFeasibilitiesSSE += ipm::evaluateComplementarySlackness(barrierParam, slackStateIneq[N], dualStateIneq[N]);
     }
   };