diff --git a/ocs2_doc/docs/intro.rst b/ocs2_doc/docs/intro.rst index 2cab1805b..ffac40b6e 100644 --- a/ocs2_doc/docs/intro.rst +++ b/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 `__ -* **PISOC**\: Path integral stochastic optimal control +* **IPM**\: Multiple-shooting algorithm based on nonlinear interior point method +* **SLP**\: Sequential Linear Programming based on `PIPG `__ OCS2 handles general path constraints through Augmented Lagrangian or relaxed barrier methods. To facilitate the application of OCS2 in robotic diff --git a/ocs2_doc/docs/overview.rst b/ocs2_doc/docs/overview.rst index 6141cc135..8ce91d113 100644 --- a/ocs2_doc/docs/overview.rst +++ b/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 `__ -* **PISOC**\: Path integral stochastic optimal control +* **IPM**\: Multiple-shooting algorithm based on nonlinear interior point method +* **SLP**\: Sequential Linear Programming based on `PIPG `__ 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 `__ +* Real-time optimal control for legged locomotion & manipulation, Marco Hutter, +MPC Workshop at RSS 2021 `link `__ + Related publications ~~~~~~~~~~~~~~~~~~~~ diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt index bd02a70cc..798f35478 100644 --- a/ocs2_ipm/CMakeLists.txt +++ b/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 diff --git a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h index 9531582a9..d9f27dac5 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h +++ b/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 +#include +#include +#include #include 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& time, const std::vector& 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 fromMultiplierCollection(const MultiplierCollection& multiplierCollection); + } // namespace ipm } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h b/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h index 7e318323c..cdc50a4ec 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h +++ b/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 +#include 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 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 \ No newline at end of file diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h index 582bc77a3..56e7e282b 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h @@ -35,6 +35,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include #include #include @@ -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& timeDiscretization, vector_array_t& projectionMultiplierTrajectory) const; + /** Initializes for the slack and dual trajectories of the hard inequality constraints */ + void initializeSlackDualTrajectory(const std::vector& 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& 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); + 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); /** Computes only the performance metrics at the current {t, x(t), u(t)} */ PerformanceIndex computePerformance(const std::vector& 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 valueFunction_; @@ -212,6 +216,9 @@ class IpmSolver : public SolverBase { std::vector stateInputIneqConstraints_; std::vector constraintsProjection_; + // Constraint terms size + std::vector constraintsSize_; + // Lagrange multipliers std::vector projectionMultiplierCoefficients_; diff --git a/ocs2_ipm/src/IpmHelpers.cpp b/ocs2_ipm/src/IpmHelpers.cpp index ea7188006..f7ebcc812 100644 --- a/ocs2_ipm/src/IpmHelpers.cpp +++ b/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& 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& time, const std::vector& constraintsSize, + const vector_array_t& stateIneq, const vector_array_t& stateInputIneq) { + // Problem horizon + const int N = static_cast(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 fromMultiplierCollection(const MultiplierCollection& multiplierCollection) { + return std::make_pair(extractLagrangian(multiplierCollection.stateIneq), extractLagrangian(multiplierCollection.stateInputIneq)); +} + } // namespace ipm } // namespace ocs2 diff --git a/ocs2_ipm/src/IpmInitialization.cpp b/ocs2_ipm/src/IpmInitialization.cpp new file mode 100644 index 000000000..33f8b7aa1 --- /dev/null +++ b/ocs2_ipm/src/IpmInitialization.cpp @@ -0,0 +1,86 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_ipm/IpmInitialization.h" + +namespace ocs2 { +namespace ipm { + +std::pair initializeIntermediateSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, + const vector_t& state, const vector_t& input, + scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate) { + vector_t slackStateIneq, slackStateInputIneq; + + if (!ocpDefinition.stateInequalityConstraintPtr->empty() || !ocpDefinition.inequalityConstraintPtr->empty()) { + constexpr auto request = Request::Constraint; + ocpDefinition.preComputationPtr->request(request, time, state, input); + } + + if (!ocpDefinition.stateInequalityConstraintPtr->empty()) { + const auto ineqConstraint = + toVector(ocpDefinition.stateInequalityConstraintPtr->getValue(time, state, *ocpDefinition.preComputationPtr)); + slackStateIneq = initializeSlackVariable(ineqConstraint, initialSlackLowerBound, initialSlackMarginRate); + } + + if (!ocpDefinition.inequalityConstraintPtr->empty()) { + const auto ineqConstraint = + toVector(ocpDefinition.inequalityConstraintPtr->getValue(time, state, input, *ocpDefinition.preComputationPtr)); + slackStateInputIneq = initializeSlackVariable(ineqConstraint, initialSlackLowerBound, initialSlackMarginRate); + } + + return std::make_pair(std::move(slackStateIneq), std::move(slackStateInputIneq)); +} + +vector_t initializeTerminalSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state, + scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate) { + if (ocpDefinition.finalInequalityConstraintPtr->empty()) { + return vector_t(); + } + + constexpr auto request = Request::Constraint; + ocpDefinition.preComputationPtr->requestFinal(request, time, state); + const auto ineqConstraint = toVector(ocpDefinition.finalInequalityConstraintPtr->getValue(time, state, *ocpDefinition.preComputationPtr)); + return initializeSlackVariable(ineqConstraint, initialSlackLowerBound, initialSlackMarginRate); +} + +vector_t initializeEventSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state, + scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate) { + if (ocpDefinition.preJumpInequalityConstraintPtr->empty()) { + return vector_t(); + } + + constexpr auto request = Request::Constraint; + ocpDefinition.preComputationPtr->requestPreJump(request, time, state); + const auto ineqConstraint = + toVector(ocpDefinition.preJumpInequalityConstraintPtr->getValue(time, state, *ocpDefinition.preComputationPtr)); + return initializeSlackVariable(ineqConstraint, initialSlackLowerBound, initialSlackMarginRate); +} + +} // namespace ipm +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 55dc83b05..46ea304b7 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -39,8 +39,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include #include +#include #include "ocs2_ipm/IpmHelpers.h" #include "ocs2_ipm/IpmInitialization.h" @@ -100,15 +100,14 @@ void IpmSolver::reset() { primalSolution_ = PrimalSolution(); costateTrajectory_.clear(); projectionMultiplierTrajectory_.clear(); - slackStateIneqTrajectory_.clear(); - dualStateIneqTrajectory_.clear(); - slackStateInputIneqTrajectory_.clear(); - dualStateInputIneqTrajectory_.clear(); + slackIneqTrajectory_.clear(); + dualIneqTrajectory_.clear(); valueFunction_.clear(); performanceIndeces_.clear(); // reset timers totalNumIterations_ = 0; + initializationTimer_.reset(); linearQuadraticApproximationTimer_.reset(); solveQpTimer_.reset(); linesearchTimer_.reset(); @@ -116,12 +115,14 @@ void IpmSolver::reset() { } std::string IpmSolver::getBenchmarkingInformation() const { + const auto initializationTotal = initializationTimer_.getTotalInMilliseconds(); const auto linearQuadraticApproximationTotal = linearQuadraticApproximationTimer_.getTotalInMilliseconds(); const auto solveQpTotal = solveQpTimer_.getTotalInMilliseconds(); const auto linesearchTotal = linesearchTimer_.getTotalInMilliseconds(); const auto computeControllerTotal = computeControllerTimer_.getTotalInMilliseconds(); - const auto benchmarkTotal = linearQuadraticApproximationTotal + solveQpTotal + linesearchTotal + computeControllerTotal; + const auto benchmarkTotal = + initializationTotal + linearQuadraticApproximationTotal + solveQpTotal + linesearchTotal + computeControllerTotal; std::stringstream infoStream; if (benchmarkTotal > 0.0) { @@ -129,6 +130,8 @@ std::string IpmSolver::getBenchmarkingInformation() const { infoStream << "\n########################################################################\n"; infoStream << "The benchmarking is computed over " << totalNumIterations_ << " iterations. \n"; infoStream << "IPM Benchmarking\t :\tAverage time [ms] (% of total runtime)\n"; + infoStream << "\tInitialization :\t" << initializationTimer_.getAverageInMilliseconds() << " [ms] \t\t(" + << initializationTotal / benchmarkTotal * inPercent << "%)\n"; infoStream << "\tLQ Approximation :\t" << linearQuadraticApproximationTimer_.getAverageInMilliseconds() << " [ms] \t\t(" << linearQuadraticApproximationTotal / benchmarkTotal * inPercent << "%)\n"; infoStream << "\tSolve QP :\t" << solveQpTimer_.getAverageInMilliseconds() << " [ms] \t\t(" @@ -189,6 +192,14 @@ vector_t IpmSolver::getStateInputEqualityConstraintLagrangian(scalar_t time, con } } +MultiplierCollection IpmSolver::getIntermediateDualSolution(scalar_t time) const { + if (!dualIneqTrajectory_.timeTrajectory.empty()) { + return getIntermediateDualSolutionAtTime(dualIneqTrajectory_, time); + } else { + throw std::runtime_error("[IpmSolver] getIntermediateDualSolution() not available yet."); + } +} + void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) { if (settings_.printSolverStatus || settings_.printLinesearch) { std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; @@ -206,20 +217,35 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f ocpDefinition.targetTrajectoriesPtr = &targetTrajectories; } + // old and new mode schedules for the trajectory spreading + const auto oldModeSchedule = primalSolution_.modeSchedule_; + const auto& newModeSchedule = this->getReferenceManager().getModeSchedule(); + + initializationTimer_.startTimer(); // Initialize the state and input + if (!primalSolution_.timeTrajectory_.empty()) { + std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, primalSolution_); + } vector_array_t x, u; multiple_shooting::initializeStateInputTrajectories(initState, timeDiscretization, primalSolution_, *initializerPtr_, x, u); + // Initialize the slack and dual variables of the interior point method + 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; + initializeSlackDualTrajectory(timeDiscretization, x, u, barrierParam, slackStateIneq, dualStateIneq, slackStateInputIneq, + dualStateInputIneq); + // Initialize the costate and projection multiplier vector_array_t lmd, nu; if (settings_.computeLagrangeMultipliers) { initializeCostateTrajectory(timeDiscretization, x, lmd); initializeProjectionMultiplierTrajectory(timeDiscretization, nu); } - - // Interior point variables - scalar_t barrierParam = settings_.initialBarrierParameter; - vector_array_t slackStateIneq, slackStateInputIneq, dualStateIneq, dualStateInputIneq; + initializationTimer_.endTimer(); // Bookkeeping performanceIndeces_.clear(); @@ -234,17 +260,15 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f // Make QP approximation linearQuadraticApproximationTimer_.startTimer(); - const bool initializeSlackAndDualVariables = (iter == 0); - const auto baselinePerformance = - setupQuadraticSubproblem(timeDiscretization, initState, x, u, lmd, nu, barrierParam, slackStateIneq, slackStateInputIneq, - dualStateIneq, dualStateInputIneq, initializeSlackAndDualVariables, metrics); + const auto baselinePerformance = setupQuadraticSubproblem(timeDiscretization, initState, x, u, lmd, nu, barrierParam, slackStateIneq, + slackStateInputIneq, dualStateIneq, dualStateInputIneq, metrics); linearQuadraticApproximationTimer_.endTimer(); // Solve QP solveQpTimer_.startTimer(); const vector_t delta_x0 = initState - x[0]; const auto deltaSolution = - getOCPSolution(delta_x0, barrierParam, slackStateIneq, slackStateInputIneq, dualStateIneq, dualStateInputIneq); + getOCPSolution(delta_x0, barrierParam, slackStateIneq, dualStateIneq, slackStateInputIneq, dualStateInputIneq); extractValueFunction(timeDiscretization, x, lmd, deltaSolution.deltaXSol); solveQpTimer_.endTimer(); @@ -274,10 +298,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); - slackStateIneqTrajectory_ = std::move(slackStateIneq); - dualStateIneqTrajectory_ = std::move(dualStateIneq); - slackStateInputIneqTrajectory_ = std::move(slackStateInputIneq); - dualStateInputIneqTrajectory_ = std::move(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(); @@ -365,9 +387,90 @@ void IpmSolver::initializeProjectionMultiplierTrajectory(const std::vector& 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) { + const auto& oldTimeTrajectory = slackIneqTrajectory_.timeTrajectory; + const auto& oldPostEventIndices = slackIneqTrajectory_.postEventIndices; + const auto newTimeTrajectory = toInterpolationTime(timeDiscretization); + const auto newPostEventIndices = toPostEventIndices(timeDiscretization); + + // find the time period that we can interpolate the cached solution + const auto timePeriod = std::make_pair(newTimeTrajectory.front(), newTimeTrajectory.back()); + const auto interpolatableTimePeriod = + findIntersectionToExtendableInterval(oldTimeTrajectory, this->getReferenceManager().getModeSchedule().eventTimes, timePeriod); + const bool interpolateTillFinalTime = numerics::almost_eq(interpolatableTimePeriod.second, timePeriod.second); + const auto cacheEventIndexBias = [&]() -> size_t { + if (!newPostEventIndices.empty()) { + const auto firstEventTime = newTimeTrajectory[newPostEventIndices[0] - 1]; + return getNumberOfPrecedingEvents(oldTimeTrajectory, oldPostEventIndices, firstEventTime); + } else { + return 0; + } + }(); + + auto& ocpDefinition = ocpDefinitions_.front(); + const size_t N = static_cast(timeDiscretization.size()) - 1; // size of the input trajectory + slackStateIneq.resize(N + 1); + dualStateIneq.resize(N + 1); + slackStateInputIneq.resize(N); + dualStateInputIneq.resize(N); + + int eventIdx = 0; + for (size_t i = 0; i < N; i++) { + if (timeDiscretization[i].event == AnnotatedTime::Event::PreEvent) { + const auto cachedEventIndex = cacheEventIndexBias + eventIdx; + if (cachedEventIndex < slackIneqTrajectory_.preJumps.size()) { + std::tie(slackStateIneq[i], std::ignore) = ipm::fromMultiplierCollection(slackIneqTrajectory_.preJumps[cachedEventIndex]); + std::tie(dualStateIneq[i], std::ignore) = ipm::fromMultiplierCollection(dualIneqTrajectory_.preJumps[cachedEventIndex]); + } else { + scalar_t time = timeDiscretization[i].time; + slackStateIneq[i] = ipm::initializeEventSlackVariable(ocpDefinition, timeDiscretization[i].time, x[i], + settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); + dualStateIneq[i] = + ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); + } + slackStateInputIneq[i].resize(0); + dualStateInputIneq[i].resize(0); + ++eventIdx; + } else { + const scalar_t time = getIntervalStart(timeDiscretization[i]); + if (interpolatableTimePeriod.first <= time && time <= interpolatableTimePeriod.second) { + std::tie(slackStateIneq[i], slackStateInputIneq[i]) = + ipm::fromMultiplierCollection(getIntermediateDualSolutionAtTime(slackIneqTrajectory_, time)); + std::tie(dualStateIneq[i], dualStateInputIneq[i]) = + ipm::fromMultiplierCollection(getIntermediateDualSolutionAtTime(slackIneqTrajectory_, time)); + } else { + std::tie(slackStateIneq[i], slackStateInputIneq[i]) = ipm::initializeIntermediateSlackVariable( + ocpDefinition, time, x[i], u[i], settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); + dualStateIneq[i] = + ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); + dualStateInputIneq[i] = ipm::initializeDualVariable(slackStateInputIneq[i], barrierParam, settings_.initialDualLowerBound, + settings_.initialDualMarginRate); + } + } + } + + // Disable the state-only inequality constraints at the initial node + slackStateIneq[0].resize(0); + dualStateIneq[0].resize(0); + + if (interpolateTillFinalTime) { + std::tie(slackStateIneq[N], std::ignore) = ipm::fromMultiplierCollection(slackIneqTrajectory_.final); + std::tie(dualStateIneq[N], std::ignore) = ipm::fromMultiplierCollection(dualIneqTrajectory_.final); + } else { + slackStateIneq[N] = ipm::initializeTerminalSlackVariable(ocpDefinition, getIntervalStart(timeDiscretization[N]), x[N], + settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); + dualStateIneq[N] = + ipm::initializeDualVariable(slackStateIneq[N], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); + } +} + IpmSolver::OcpSubproblemSolution IpmSolver::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& dualStateInputIneq) { + const vector_array_t& slackStateIneq, const vector_array_t& dualStateIneq, + const vector_array_t& slackStateInputIneq, + const vector_array_t& dualStateInputIneq) { // Solve the QP OcpSubproblemSolution solution; auto& deltaXSol = solution.deltaXSol; @@ -394,14 +497,14 @@ IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta auto& deltaLmdSol = solution.deltaLmdSol; auto& deltaNuSol = solution.deltaNuSol; auto& deltaSlackStateIneq = solution.deltaSlackStateIneq; - auto& deltaSlackStateInputIneq = solution.deltaSlackStateInputIneq; auto& deltaDualStateIneq = solution.deltaDualStateIneq; + auto& deltaSlackStateInputIneq = solution.deltaSlackStateInputIneq; auto& deltaDualStateInputIneq = solution.deltaDualStateInputIneq; deltaLmdSol.resize(N + 1); deltaNuSol.resize(N); deltaSlackStateIneq.resize(N + 1); - deltaSlackStateInputIneq.resize(N); deltaDualStateIneq.resize(N + 1); + deltaSlackStateInputIneq.resize(N); deltaDualStateInputIneq.resize(N); scalar_array_t primalStepSizes(settings_.nThreads, 1.0); @@ -502,10 +605,9 @@ PrimalSolution IpmSolver::toPrimalSolution(const std::vector& tim PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector& 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) { + const vector_array_t& nu, 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) { // Problem horizon const int N = static_cast(time.size()) - 1; @@ -517,15 +619,9 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector= 0); } } + + // solve with shifted horizon + const scalar_array_t shiftTime = {0.05, 0.1, 0.3, 0.5, 0.8, 0.12, 0.16, 0.19}; + for (const auto e : shiftTime) { + solver.run(startTime + e, initState, finalTime + e); + } } \ No newline at end of file diff --git a/ocs2_ipm/test/Exp1Test.cpp b/ocs2_ipm/test/Exp1Test.cpp index 5a4cd0cb1..15cceb2d3 100644 --- a/ocs2_ipm/test/Exp1Test.cpp +++ b/ocs2_ipm/test/Exp1Test.cpp @@ -197,6 +197,12 @@ TEST(Exp1Test, Constrained) { ASSERT_TRUE(umax - u(0) >= 0); } } + + // solve with shifted horizon + const scalar_array_t shiftTime = {0.05, 0.1, 0.3, 0.5, 0.8, 0.12, 0.16, 0.19}; + for (const auto e : shiftTime) { + solver.run(startTime + e, initState, finalTime + e); + } } TEST(Exp1Test, MixedConstrained) { @@ -257,4 +263,10 @@ TEST(Exp1Test, MixedConstrained) { const auto constraintValue = stateInputIneqConstraintCloned->getValue(t, x, u, PreComputation()); ASSERT_TRUE(constraintValue.minCoeff() >= 0.0); } + + // solve with shifted horizon + const scalar_array_t shiftTime = {0.05, 0.1, 0.3, 0.5, 0.8, 0.12, 0.16, 0.19}; + for (const auto e : shiftTime) { + solver.run(startTime + e, initState, finalTime + e); + } } diff --git a/ocs2_ipm/test/testCircularKinematics.cpp b/ocs2_ipm/test/testCircularKinematics.cpp index a054ed01d..e05dc269b 100644 --- a/ocs2_ipm/test/testCircularKinematics.cpp +++ b/ocs2_ipm/test/testCircularKinematics.cpp @@ -238,6 +238,12 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_IneqConstraints) { // Feed forward part ASSERT_TRUE(u.isApprox(primalSolution.controllerPtr_->computeInput(t, x))); } + + // solve with shifted horizon + const scalar_array_t shiftTime = {0.05, 0.1, 0.3, 0.5, 0.8, 0.12, 0.16, 0.19}; + for (const auto e : shiftTime) { + solver.run(startTime + e, initState, finalTime + e); + } } TEST(test_circular_kinematics, solve_projected_EqConstraints_MixedIneqConstraints) { @@ -326,10 +332,15 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_MixedIneqConstraint // Check Lagrange multipliers for (int i = 0; i < primalSolution.timeTrajectory_.size() - 1; i++) { - std::cerr << "i: " << i << std::endl; const auto t = primalSolution.timeTrajectory_[i]; const auto& x = primalSolution.stateTrajectory_[i]; const auto& u = primalSolution.inputTrajectory_[i]; ASSERT_NO_THROW(const auto multiplier = solver.getStateInputEqualityConstraintLagrangian(t, x);); } + + // solve with shifted horizon + const scalar_array_t shiftTime = {0.05, 0.1, 0.3, 0.5, 0.8, 0.12, 0.16, 0.19}; + for (const auto e : shiftTime) { + solver.run(startTime + e, initState, finalTime + e); + } } \ No newline at end of file diff --git a/ocs2_oc/include/ocs2_oc/oc_data/TimeDiscretization.h b/ocs2_oc/include/ocs2_oc/oc_data/TimeDiscretization.h index 304cc3daf..dc0a70422 100644 --- a/ocs2_oc/include/ocs2_oc/oc_data/TimeDiscretization.h +++ b/ocs2_oc/include/ocs2_oc/oc_data/TimeDiscretization.h @@ -84,6 +84,14 @@ std::vector timeDiscretizationWithEvents(scalar_t initTime, scala */ scalar_array_t toTime(const std::vector& annotatedTime); +/** + * Extracts the time trajectory from the annotated time trajectory respecting interpolation rules around event times. + * + * @param annotatedTime : Annotated time trajectory. + * @return The time trajectory. + */ +scalar_array_t toInterpolationTime(const std::vector& annotatedTime); + /** * Extracts the array of indices indicating the post-event times from the annotated time trajectory. * diff --git a/ocs2_oc/src/oc_data/TimeDiscretization.cpp b/ocs2_oc/src/oc_data/TimeDiscretization.cpp index d4bbbb182..5b9f86ab3 100644 --- a/ocs2_oc/src/oc_data/TimeDiscretization.cpp +++ b/ocs2_oc/src/oc_data/TimeDiscretization.cpp @@ -122,6 +122,24 @@ scalar_array_t toTime(const std::vector& annotatedTime) { return timeTrajectory; } +scalar_array_t toInterpolationTime(const std::vector& annotatedTime) { + if (annotatedTime.empty()) { + return scalar_array_t(); + } + scalar_array_t timeTrajectory; + timeTrajectory.reserve(annotatedTime.size()); + timeTrajectory.push_back(annotatedTime.back().time - numeric_traits::limitEpsilon()); + for (int i = 1; i < annotatedTime.size() - 1; i++) { + if (annotatedTime[i].event == AnnotatedTime::Event::PostEvent) { + timeTrajectory.push_back(getInterpolationTime(annotatedTime[i])); + } else { + timeTrajectory.push_back(annotatedTime[i].time); + } + } + timeTrajectory.push_back(annotatedTime.back().time - numeric_traits::limitEpsilon()); + return timeTrajectory; +} + size_array_t toPostEventIndices(const std::vector& annotatedTime) { size_array_t postEventIndices; for (size_t i = 0; i < annotatedTime.size(); i++) { diff --git a/ocs2_slp/src/SlpSolver.cpp b/ocs2_slp/src/SlpSolver.cpp index 428bf9bad..81c2a6c9c 100644 --- a/ocs2_slp/src/SlpSolver.cpp +++ b/ocs2_slp/src/SlpSolver.cpp @@ -39,6 +39,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include "ocs2_slp/Helpers.h" @@ -172,6 +173,11 @@ void SlpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f ocpDefinition.targetTrajectoriesPtr = &targetTrajectories; } + // Trajectory spread of primalSolution_ + if (!primalSolution_.timeTrajectory_.empty()) { + std::ignore = trajectorySpread(primalSolution_.modeSchedule_, this->getReferenceManager().getModeSchedule(), primalSolution_); + } + // Initialize the state and input vector_array_t x, u; multiple_shooting::initializeStateInputTrajectories(initState, timeDiscretization, primalSolution_, *initializerPtr_, x, u); diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp index ff2f658e2..5e0e38379 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp @@ -39,6 +39,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include namespace ocs2 { @@ -170,6 +171,11 @@ void SqpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f ocpDefinition.targetTrajectoriesPtr = &targetTrajectories; } + // Trajectory spread of primalSolution_ + if (!primalSolution_.timeTrajectory_.empty()) { + std::ignore = trajectorySpread(primalSolution_.modeSchedule_, this->getReferenceManager().getModeSchedule(), primalSolution_); + } + // Initialize the state and input vector_array_t x, u; multiple_shooting::initializeStateInputTrajectories(initState, timeDiscretization, primalSolution_, *initializerPtr_, x, u);