Merged in feature/interior_point/trajectory_adjustment (pull request …

…#687)

Feature/interior point/trajectory adjustment

Approved-by: Farbod Farshidian

ocs2_doc/docs/intro.rst

@@ -7,10 +7,11 @@ Introduction
 **S**\ witched **S**\ ystems (OCS2). The toolbox provides an efficient 
 implementation of the following algorithms:
 
-* **SLQ**\: Continuous-time domain DDP
-* **iLQR**\: Discrete-time domain DDP
+* **SLQ**\: Continuous-time domain constrained DDP
+* **iLQR**\: Discrete-time domain constrained DDP
 * **SQP**\: Multiple-shooting algorithm based on `HPIPM <href="https://github.com/giaf/hpipm"/>`__
-* **PISOC**\: Path integral stochastic optimal control
+* **IPM**\: Multiple-shooting algorithm based on nonlinear interior point method
+* **SLP**\: Sequential Linear Programming based on `PIPG <href="https://arxiv.org/abs/2009.06980"/>`__
 
 OCS2 handles general path constraints through Augmented Lagrangian or 
 relaxed barrier methods. To facilitate the application of OCS2 in robotic 

ocs2_doc/docs/overview.rst

@@ -9,10 +9,11 @@ Overview
 for **S**\ witched **S**\ ystems (OCS2). The toolbox provides an 
 efficient implementation of the following algorithms:
 
-* **SLQ**\: Continuous-time domain DDP
-* **iLQR**\: Discrete-time domain DDP
+* **SLQ**\: Continuous-time domain constrained DDP
+* **iLQR**\: Discrete-time domain constrained DDP
 * **SQP**\: Multiple-shooting algorithm based on `HPIPM <href="https://github.com/giaf/hpipm"/>`__
-* **PISOC**\: Path integral stochastic optimal control
+* **IPM**\: Multiple-shooting algorithm based on nonlinear interior point method
+* **SLP**\: Sequential Linear Programming based on `PIPG <href="https://arxiv.org/abs/2009.06980"/>`__
 
 OCS2 handles general path constraints through Augmented Lagrangian or 
 relaxed barrier methods. To facilitate the application of OCS2 in robotic 
@@ -51,18 +52,20 @@ Credits
 The following people have been involved in the development of OCS2:
 
 **Project Manager**: 
-Farbod Farshidian (ETHZ).
+Farbod Farshidian.
 
 **Main Developers**: 
-Farbod Farshidian (ETHZ), 
-Ruben Grandia (ETHZ), 
-Michael Spieler (ETHZ), 
-Jan Carius (ETHZ), 
-Jean-Pierre Sleiman (ETHZ).
+Farbod Farshidian,
+Ruben Grandia,
+Michael Spieler,
+Jan Carius,
+Jean-Pierre Sleiman.
 
 **Other Developers**:
 Alexander Reske,
+Sotaro Katayama,
 Mayank Mittal,
+Jia-​Ruei Chiu,
 Johannes Pankert,
 Perry Franklin,
 Tom Lankhorst,
@@ -76,6 +79,25 @@ Edo Jelavic.
 project has continued to evolve at RSL, ETH Zurich. The RSL team now actively 
 supports the development of OCS2.
 
+Citing OCS2
+~~~~~~~~~~~
+To cite the toolbox in your academic research, please use the following:
+
+.. code-block::
+
+      @misc{OCS2,
+         title = {{OCS2}:  An  open  source  library  for  Optimal  Control  of  Switched Systems},
+         note = {[Online]. Available: \url{https://github.com/leggedrobotics/ocs2}},
+         author = {Farbod Farshidian and others}
+      }
+
+Tutorials
+~~~~~~~~~
+* Tutorial on OCS2 Toolbox, Farbod Farshidian, 
+MPC Workshop at RSS 2021 `link <href="https://youtu.be/RYmQN9GbFYg"/>`__
+* Real-time optimal control for legged locomotion & manipulation, Marco Hutter, 
+MPC Workshop at RSS 2021 `link <href="https://youtu.be/sjAENmtO4bA"/>`__
+
 
 Related publications
 ~~~~~~~~~~~~~~~~~~~~

ocs2_ipm/CMakeLists.txt

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

ocs2_ipm/include/ocs2_ipm/IpmHelpers.h

@@ -30,6 +30,9 @@ 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>
 
 namespace ocs2 {
@@ -107,5 +110,25 @@ vector_t retrieveDualDirection(scalar_t barrierParam, const vector_t& slack, con
  */
 scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scalar_t marginRate = 0.995);
 
+/**
+ * Convert the optimized slack or dual trajectories as a DualSolution.
+ *
+ * @param time: The time discretization.
+ * @param constraintsSize: The constraint tems size.
+ * @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& stateIneq, const vector_array_t& stateInputIneq);
+
+/**
+ * Extracts slack/dual variables of the state-only and state-input constraints from a MultiplierCollection
+ *
+ * @param multiplierCollection: The MultiplierCollection.
+ * @return slack/dual variables of the state-only (first) and state-input constraints (second).
+ */
+std::pair<vector_t, vector_t> fromMultiplierCollection(const MultiplierCollection& multiplierCollection);
+
 }  // namespace ipm
 }  // namespace ocs2

ocs2_ipm/include/ocs2_ipm/IpmInitialization.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/oc_problem/OptimalControlProblem.h>
 
 namespace ocs2 {
 namespace ipm {
@@ -44,7 +45,11 @@ namespace ipm {
  * @return Initialized slack variable.
  */
 inline vector_t initializeSlackVariable(const vector_t& ineqConstraint, scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate) {
-  return (1.0 + initialSlackMarginRate) * ineqConstraint.cwiseMax(initialSlackLowerBound);
+  if (ineqConstraint.size() > 0) {
+    return (1.0 + initialSlackMarginRate) * ineqConstraint.cwiseMax(initialSlackLowerBound);
+  } else {
+    return vector_t();
+  }
 }
 
 /**
@@ -59,8 +64,53 @@ inline vector_t initializeSlackVariable(const vector_t& ineqConstraint, scalar_t
  */
 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);
+  if (slack.size() > 0) {
+    return (1.0 + initialDualMarginRate) * (barrierParam * slack.cwiseInverse()).cwiseMax(initialDualLowerBound);
+  } else {
+    return vector_t();
+  }
 }
 
+/**
+ * Initializes the slack variable at a single intermediate node.
+ *
+ * @param ocpDefinition: Definition of the optimal control problem.
+ * @param time : Time of this node
+ * @param state : State
+ * @param input : Input
+ * @param initialSlackLowerBound : Lower bound of the initial slack variables. Corresponds to `slack_bound_push` option of IPOPT.
+ * @param initialSlackMarginRate : Additional margin rate of the initial slack variables. Corresponds to `slack_bound_frac` option of IPOPT.
+ * @return Initialized slack variables of the intermediate state-only (first) and state-input (second) constraints.
+ */
+std::pair<vector_t, vector_t> initializeIntermediateSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time,
+                                                                  const vector_t& state, const vector_t& input,
+                                                                  scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate);
+
+/**
+ * Initializes the slack variable at the terminal node.
+ *
+ * @param ocpDefinition: Definition of the optimal control problem.
+ * @param time : Time at the terminal node
+ * @param state : Terminal state
+ * @param initialSlackLowerBound : Lower bound of the initial slack variables. Corresponds to `slack_bound_push` option of IPOPT.
+ * @param initialSlackMarginRate : Additional margin rate of the initial slack variables. Corresponds to `slack_bound_frac` option of IPOPT.
+ * @return Initialized slack variable of the terminal state-only constraints.
+ */
+vector_t initializeTerminalSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state,
+                                         scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate);
+
+/**
+ * Initializes the slack variable at an event node.
+ *
+ * @param ocpDefinition: Definition of the optimal control problem.
+ * @param time : Time at the event node
+ * @param state : Pre-event state
+ * @param initialSlackLowerBound : Lower bound of the initial slack variables. Corresponds to `slack_bound_push` option of IPOPT.
+ * @param initialSlackMarginRate : Additional margin rate of the initial slack variables. Corresponds to `slack_bound_frac` option of IPOPT.
+ * @return Initialized slack variable of the pre-jump state-only constraints.
+ */
+vector_t initializeEventSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state,
+                                      scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate);
+
 }  // 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>
@@ -66,6 +67,8 @@ class IpmSolver : public SolverBase {
 
   void getPrimalSolution(scalar_t finalTime, PrimalSolution* primalSolutionPtr) const override { *primalSolutionPtr = primalSolution_; }
 
+  const DualSolution* getDualSolution() const override { return &dualIneqTrajectory_; }
+
   const ProblemMetrics& getSolutionMetrics() const override { return problemMetrics_; }
 
   size_t getNumIterations() const override { return totalNumIterations_; }
@@ -84,9 +87,7 @@ class IpmSolver : public SolverBase {
 
   vector_t getStateInputEqualityConstraintLagrangian(scalar_t time, const vector_t& state) const override;
 
-  MultiplierCollection getIntermediateDualSolution(scalar_t time) const override {
-    throw std::runtime_error("[IpmSolver] getIntermediateDualSolution() not available yet.");
-  }
+  MultiplierCollection getIntermediateDualSolution(scalar_t time) const override;
 
  private:
   void runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) override;
@@ -123,12 +124,17 @@ class IpmSolver : public SolverBase {
   void initializeProjectionMultiplierTrajectory(const std::vector<AnnotatedTime>& timeDiscretization,
                                                 vector_array_t& projectionMultiplierTrajectory) const;
 
+  /** Initializes for the slack and dual trajectories of the hard inequality constraints */
+  void initializeSlackDualTrajectory(const std::vector<AnnotatedTime>& timeDiscretization, const vector_array_t& x, const vector_array_t& u,
+                                     scalar_t barrierParam, vector_array_t& slackStateIneq, vector_array_t& dualStateIneq,
+                                     vector_array_t& slackStateInputIneq, vector_array_t& dualStateInputIneq);
+
   /** Creates QP around t, x(t), u(t). Returns performance metrics at the current {t, x(t), u(t)} */
   PerformanceIndex setupQuadraticSubproblem(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x,
                                             const vector_array_t& u, const vector_array_t& lmd, const vector_array_t& nu,
-                                            scalar_t barrierParam, vector_array_t& slackStateIneq, vector_array_t& slackStateInputIneq,
-                                            vector_array_t& dualStateIneq, vector_array_t& dualStateInputIneq,
-                                            bool initializeSlackAndDualVariables, std::vector<Metrics>& metrics);
+                                            scalar_t barrierParam, const vector_array_t& slackStateIneq,
+                                            const vector_array_t& slackStateInputIneq, const vector_array_t& dualStateIneq,
+                                            const vector_array_t& dualStateInputIneq, std::vector<Metrics>& metrics);
 
   /** 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,
@@ -142,15 +148,15 @@ class IpmSolver : public SolverBase {
     vector_array_t deltaLmdSol;  // delta_lmd(t)
     vector_array_t deltaNuSol;   // delta_nu(t)
     vector_array_t deltaSlackStateIneq;
-    vector_array_t deltaSlackStateInputIneq;
     vector_array_t deltaDualStateIneq;
+    vector_array_t deltaSlackStateInputIneq;
     vector_array_t deltaDualStateInputIneq;
     scalar_t armijoDescentMetric;  // inner product of the cost gradient and decision variable step
     scalar_t maxPrimalStepSize;
     scalar_t maxDualStepSize;
   };
   OcpSubproblemSolution getOCPSolution(const vector_t& delta_x0, scalar_t barrierParam, const vector_array_t& slackStateIneq,
-                                       const vector_array_t& slackStateInputIneq, const vector_array_t& dualStateIneq,
+                                       const vector_array_t& dualStateIneq, const vector_array_t& slackStateInputIneq,
                                        const vector_array_t& dualStateInputIneq);
 
   /** Extract the value function based on the last solved QP */
@@ -196,10 +202,8 @@ class IpmSolver : public SolverBase {
   PrimalSolution primalSolution_;
   vector_array_t costateTrajectory_;
   vector_array_t projectionMultiplierTrajectory_;
-  vector_array_t slackStateIneqTrajectory_;
-  vector_array_t dualStateIneqTrajectory_;
-  vector_array_t slackStateInputIneqTrajectory_;
-  vector_array_t dualStateInputIneqTrajectory_;
+  DualSolution slackIneqTrajectory_;
+  DualSolution dualIneqTrajectory_;
 
   // Value function in absolute state coordinates (without the constant value)
   std::vector<ScalarFunctionQuadraticApproximation> valueFunction_;
@@ -212,6 +216,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

@@ -113,5 +113,72 @@ scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scala
   return alpha > 0.0 ? std::min(1.0 / alpha, 1.0) : 1.0;
 }
 
+namespace {
+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, stateIneq.segment(head, size));
+    head += size;
+  }
+  return multiplierCollection;
+}
+
+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, stateInputIneq.segment(head, size));
+    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
+
+DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, const std::vector<multiple_shooting::ConstraintsSize>& constraintsSize,
+                            const vector_array_t& stateIneq, const vector_array_t& stateInputIneq) {
+  // Problem horizon
+  const int N = static_cast<int>(time.size()) - 1;
+
+  DualSolution dualSolution;
+  dualSolution.timeTrajectory = toInterpolationTime(time);
+  dualSolution.postEventIndices = toPostEventIndices(time);
+
+  dualSolution.preJumps.reserve(dualSolution.postEventIndices.size());
+  dualSolution.intermediates.reserve(time.size());
+
+  for (int i = 0; i < N; ++i) {
+    if (time[i].event == AnnotatedTime::Event::PreEvent) {
+      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], stateIneq[i], stateInputIneq[i]));
+    }
+  }
+  dualSolution.final = toMultiplierCollection(constraintsSize[N], stateIneq[N]);
+  dualSolution.intermediates.push_back(dualSolution.intermediates.back());
+
+  return dualSolution;
+}
+
+std::pair<vector_t, vector_t> fromMultiplierCollection(const MultiplierCollection& multiplierCollection) {
+  return std::make_pair(extractLagrangian(multiplierCollection.stateIneq), extractLagrangian(multiplierCollection.stateInputIneq));
+}
+
 }  // namespace ipm
 }  // namespace ocs2