refactor Ipm

Author: mayataka

ocs2_ipm/CMakeLists.txt

@@ -49,7 +49,6 @@ include_directories(
 
 # Multiple shooting solver library
 add_library(${PROJECT_NAME}
-  src/IpmInitialization.cpp
   src/IpmHelpers.cpp
   src/IpmPerformanceIndexComputation.cpp
   src/IpmSettings.cpp

ocs2_ipm/include/ocs2_ipm/IpmInitialization.h

@@ -43,7 +43,9 @@ namespace ipm {
  * @param initialSlackMarginRate : Additional margin rate of the initial slack variables. Corresponds to `slack_bound_frac` option of IPOPT.
  * @return Initialized slack variable.
  */
-vector_t initializeSlackVariable(const vector_t& ineqConstraint, scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate);
+inline vector_t initializeSlackVariable(const vector_t& ineqConstraint, scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate) {
+  return (1.0 + initialSlackMarginRate) * ineqConstraint.cwiseMax(initialSlackLowerBound);
+}
 
 /**
  * Initializes the dual variable. The initialization of dual variables follows IPOPT option `bound_mult_init_method`: `mu-based`
@@ -55,8 +57,10 @@ vector_t initializeSlackVariable(const vector_t& ineqConstraint, scalar_t initia
  * @param initialDualMarginRate : Additional margin rate of the initial dual variables.
  * @return Initialized dual variable.
  */
-vector_t initializeDualVariable(const vector_t& slack, scalar_t barrierParam, scalar_t initialSlackLowerBound,
-                                scalar_t initialSlackMarginRate);
+inline vector_t initializeDualVariable(const vector_t& slack, scalar_t barrierParam, scalar_t initialDualLowerBound,
+                                       scalar_t initialDualMarginRate) {
+  return (1.0 + initialDualMarginRate) * (barrierParam * slack.cwiseInverse()).cwiseMax(initialDualLowerBound);
+}
 
 }  // namespace ipm
 }  // namespace ocs2

ocs2_ipm/src/IpmInitialization.cpp

@@ -43,10 +43,10 @@ PerformanceIndex computePerformanceIndex(const multiple_shooting::Transcription&
   auto performance = multiple_shooting::computePerformanceIndex(transcription, dt);
 
   if (slackStateIneq.size() > 0) {
-    performance.cost += (-dt * barrierParam * slackStateIneq.array().log().sum());
+    performance.cost -= dt * barrierParam * slackStateIneq.array().log().sum();
   }
   if (slackStateInputIneq.size() > 0) {
-    performance.cost += (-dt * barrierParam * slackStateInputIneq.array().log().sum());
+    performance.cost -= dt * barrierParam * slackStateInputIneq.array().log().sum();
   }
 
   if (transcription.stateIneqConstraints.f.size() > 0) {
@@ -64,7 +64,7 @@ PerformanceIndex computePerformanceIndex(const multiple_shooting::TerminalTransc
   auto performance = multiple_shooting::computePerformanceIndex(transcription);
 
   if (slackIneq.size() > 0) {
-    performance.cost += (-barrierParam * slackIneq.array().log().sum());
+    performance.cost -= barrierParam * slackIneq.array().log().sum();
   }
 
   if (transcription.ineqConstraints.f.size() > 0) {
@@ -79,7 +79,7 @@ PerformanceIndex computePerformanceIndex(const multiple_shooting::EventTranscrip
   auto performance = multiple_shooting::computePerformanceIndex(transcription);
 
   if (slackIneq.size() > 0) {
-    performance.cost += (-barrierParam * slackIneq.array().log().sum());
+    performance.cost -= barrierParam * slackIneq.array().log().sum();
   }
 
   if (transcription.ineqConstraints.f.size() > 0) {
@@ -114,12 +114,12 @@ PerformanceIndex toPerformanceIndex(const Metrics& metrics, scalar_t dt, scalar_
   PerformanceIndex performance = toPerformanceIndex(metrics, dt);
 
   if (slackStateIneq.size() > 0) {
-    performance.cost += (-dt * barrierParam * slackStateIneq.array().log().sum());
+    performance.cost -= dt * barrierParam * slackStateIneq.array().log().sum();
     performance.equalityConstraintsSSE += dt * (toVector(metrics.stateIneqConstraint) - slackStateIneq).squaredNorm();
   }
 
   if (slackStateInputIneq.size() > 0 && enableStateInequalityConstraints) {
-    performance.cost += (-dt * barrierParam * slackStateInputIneq.array().log().sum());
+    performance.cost -= dt * barrierParam * slackStateInputIneq.array().log().sum();
     performance.equalityConstraintsSSE += dt * (toVector(metrics.stateInputIneqConstraint) - slackStateInputIneq).squaredNorm();
   }
 
@@ -130,7 +130,7 @@ PerformanceIndex toPerformanceIndex(const Metrics& metrics, scalar_t barrierPara
   PerformanceIndex performance = toPerformanceIndex(metrics);
 
   if (slackIneq.size() > 0) {
-    performance.cost += (-barrierParam * slackIneq.array().log().sum());
+    performance.cost -= barrierParam * slackIneq.array().log().sum();
     performance.equalityConstraintsSSE += (toVector(metrics.stateIneqConstraint) - slackIneq).squaredNorm();
   }