From 27ccaaa8ef34c97eb4d04c64d729155c42cb3523 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Thu, 15 Dec 2022 11:04:38 +0100 Subject: [PATCH 01/21] add IpmTrajectoryAdjustment --- ocs2_ipm/CMakeLists.txt | 1 + ocs2_ipm/include/ocs2_ipm/IpmSolver.h | 8 +- .../ocs2_ipm/IpmTrajectoryAdjustment.h | 73 +++++++ ocs2_ipm/src/IpmSolver.cpp | 45 ++--- ocs2_ipm/src/IpmTrajectoryAdjustment.cpp | 180 ++++++++++++++++++ 5 files changed, 276 insertions(+), 31 deletions(-) create mode 100644 ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h create mode 100644 ocs2_ipm/src/IpmTrajectoryAdjustment.cpp diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt index 9822ddb8b..e8e6021bf 100644 --- a/ocs2_ipm/CMakeLists.txt +++ b/ocs2_ipm/CMakeLists.txt @@ -54,6 +54,7 @@ add_library(${PROJECT_NAME} src/IpmPerformanceIndexComputation.cpp src/IpmSettings.cpp src/IpmSolver.cpp + src/IpmTrajectoryAdjustment.cpp ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h index 30f15befe..2d7e28be1 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h @@ -130,7 +130,7 @@ class IpmSolver : public SolverBase { 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); + 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, @@ -191,10 +191,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_; diff --git a/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h b/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h new file mode 100644 index 000000000..3097800e0 --- /dev/null +++ b/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h @@ -0,0 +1,73 @@ +/****************************************************************************** +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. +******************************************************************************/ + +#pragma once + +#include + +#include +#include + +namespace ocs2 { +namespace ipm { + +/** + * Convert the optimized slack or dual trajectories as a DualSolution. + * + * @param time: The time discretization. + * @param stateIneq: The slack/dual of the state inequality constraints. + * @param stateInputIneq: The slack/dual of the state-input inequality constraints. + * @return A dual solution. + */ +DualSolution toDualSolution(const std::vector& time, vector_array_t&& stateIneq, vector_array_t&& stateInputIneq); + +/** + * Convert the optimized slack or dual trajectories as a DualSolution. + * + * @param time: The time discretization. + * @param dualSolution: The dual solution. + * @return The slack/dual of the state inequality constraints (first) and state-input inequality constraints (second). + */ +std::pair fromDualSolution(const std::vector& time, DualSolution&& dualSolution); + +/** + * Initializes the interior point trajectory. + * + * @param time: The time discretization. + * @param stateIneq: The slack/dual of the state inequality constraints. + * @param stateInputIneq: The slack/dual of the state-input inequality constraints. + * @return A dual solution. + */ +std::pair initializeInteriorPointTrajectory(const ModeSchedule& oldModeSchedule, + const ModeSchedule& newModeSchedule, + const std::vector& time, + DualSolution&& oldDualSolution); + +} // 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 c88eecf1c..4cdff7dd4 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -45,6 +45,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ipm/IpmHelpers.h" #include "ocs2_ipm/IpmInitialization.h" #include "ocs2_ipm/IpmPerformanceIndexComputation.h" +#include "ocs2_ipm/IpmTrajectoryAdjustment.h" namespace ocs2 { @@ -100,10 +101,8 @@ 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(); @@ -200,6 +199,10 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f // Interior point variables vector_array_t slackStateIneq, slackStateInputIneq, dualStateIneq, dualStateInputIneq; + std::tie(slackStateIneq, slackStateInputIneq) = ipm::initializeInteriorPointTrajectory( + primalSolution_.modeSchedule_, this->getReferenceManager().getModeSchedule(), timeDiscretization, std::move(slackIneqTrajectory_)); + std::tie(dualStateIneq, dualStateInputIneq) = ipm::initializeInteriorPointTrajectory( + primalSolution_.modeSchedule_, this->getReferenceManager().getModeSchedule(), timeDiscretization, std::move(dualIneqTrajectory_)); scalar_t barrierParam = settings_.initialBarrierParameter; @@ -216,10 +219,8 @@ 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 @@ -274,10 +275,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, std::move(slackStateIneq), std::move(slackStateInputIneq)); + dualIneqTrajectory_ = ipm::toDualSolution(timeDiscretization, std::move(dualStateIneq), std::move(dualStateInputIneq)); problemMetrics_ = multiple_shooting::toProblemMetrics(timeDiscretization, std::move(metrics)); computeControllerTimer_.endTimer(); @@ -506,8 +505,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector& metrics) { + vector_array_t& dualStateInputIneq, std::vector& metrics) { // Problem horizon const int N = static_cast(time.size()) - 1; @@ -521,13 +519,6 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector +#include + +namespace ocs2 { +namespace { + +MultiplierCollection toMultiplierCollection(vector_t&& stateIneq, vector_t&& stateInputIneq = vector_t()) { + MultiplierCollection multiplierCollection; + multiplierCollection.stateIneq.emplace_back(0.0, std::move(stateIneq)); + if (stateInputIneq.size() > 0) { + multiplierCollection.stateInputIneq.emplace_back(0.0, std::move(stateInputIneq)); + } + return multiplierCollection; +} + +std::pair fromMultiplierCollection(MultiplierCollection&& multiplierCollection) { + if (multiplierCollection.stateInputIneq.empty()) { + return std::make_pair(std::move(multiplierCollection.stateIneq.front().lagrangian), vector_t()); + } else { + return std::make_pair(std::move(multiplierCollection.stateIneq.front().lagrangian), + std::move(multiplierCollection.stateInputIneq.front().lagrangian)); + } +} +} // namespace +} // namespace ocs2 + +namespace ocs2 { +namespace ipm { + +DualSolution toDualSolution(const std::vector& time, vector_array_t&& stateIneq, vector_array_t&& stateInputIneq) { + // Problem horizon + const int N = static_cast(time.size()) - 1; + + DualSolution dualSolution; + dualSolution.timeTrajectory = toTime(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(std::move(stateIneq[i]))); + dualSolution.intermediates.push_back(dualSolution.intermediates.back()); // no event at the initial node + } else { + dualSolution.intermediates.emplace_back(toMultiplierCollection(std::move(stateIneq[i]), std::move(stateInputIneq[i]))); + } + } + dualSolution.final = toMultiplierCollection(std::move(stateIneq[N])); + dualSolution.intermediates.push_back(dualSolution.intermediates.back()); + + return dualSolution; +} + +std::pair fromDualSolution(const std::vector& time, DualSolution&& dualSolution) { + // Problem horizon + const int N = static_cast(time.size()) - 1; + + vector_array_t stateIneq; + vector_array_t stateInputIneq; + stateIneq.reserve(N + 1); + stateInputIneq.reserve(N + 1); + + int preJumpIdx = 0; + for (int i = 0; i < N; ++i) { + if (time[i].event == AnnotatedTime::Event::PreEvent) { + auto result = fromMultiplierCollection(std::move(dualSolution.preJumps[preJumpIdx])); + stateIneq.emplace_back(std::move(result.first)); + stateInputIneq.emplace_back(std::move(result.second)); + ++preJumpIdx; + } else { + auto result = fromMultiplierCollection(std::move(dualSolution.intermediates[i])); + stateIneq.emplace_back(std::move(result.first)); + stateInputIneq.emplace_back(std::move(result.second)); + } + } + + auto result = fromMultiplierCollection(std::move(dualSolution.final)); + stateIneq.emplace_back(std::move(result.first)); + stateInputIneq.emplace_back(std::move(result.second)); + + return std::make_pair(std::move(stateIneq), std::move(stateInputIneq)); +} + +std::pair initializeInteriorPointTrajectory(const ModeSchedule& oldModeSchedule, + const ModeSchedule& newModeSchedule, + const std::vector& time, + DualSolution&& oldDualSolution) { + const auto oldTimeTrajectory = oldDualSolution.timeTrajectory; + const auto oldPostEventIndices = oldDualSolution.postEventIndices; + + if (!oldTimeTrajectory.empty()) { + std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, oldDualSolution); + } + + const auto newTimeTrajectory = toTime(time); + const auto newPostEventIndices = toPostEventIndices(time); + + // 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, newModeSchedule.eventTimes, timePeriod); + const bool interpolateTillFinalTime = numerics::almost_eq(interpolatableTimePeriod.second, timePeriod.second); + + DualSolution newDualSolution; + + // set time and post-event indices + newDualSolution.timeTrajectory = std::move(newTimeTrajectory); + newDualSolution.postEventIndices = std::move(newPostEventIndices); + + // final + if (interpolateTillFinalTime) { + newDualSolution.final = std::move(oldDualSolution.final); + } else { + newDualSolution.final = toMultiplierCollection(vector_t()); + } + + // pre-jumps + newDualSolution.preJumps.resize(newDualSolution.postEventIndices.size()); + if (!newDualSolution.postEventIndices.empty()) { + const auto firstEventTime = newDualSolution.timeTrajectory[newDualSolution.postEventIndices[0] - 1]; + const auto cacheEventIndexBias = getNumberOfPrecedingEvents(oldTimeTrajectory, oldPostEventIndices, firstEventTime); + + for (size_t i = 0; i < newDualSolution.postEventIndices.size(); i++) { + const auto cachedTimeIndex = cacheEventIndexBias + i; + if (cachedTimeIndex < oldDualSolution.preJumps.size()) { + newDualSolution.preJumps[i] = std::move(oldDualSolution.preJumps[cachedTimeIndex]); + } else { + newDualSolution.preJumps[i] = toMultiplierCollection(vector_t()); + } + } + } + + // intermediates + newDualSolution.intermediates.resize(newDualSolution.timeTrajectory.size() - 1); + for (size_t i = 0; i < newDualSolution.timeTrajectory.size() - 1; i++) { + const auto time = newDualSolution.timeTrajectory[i]; + + if (interpolatableTimePeriod.first <= time && time <= interpolatableTimePeriod.second) { + newDualSolution.intermediates[i] = getIntermediateDualSolutionAtTime(oldDualSolution, time); + } else { + newDualSolution.intermediates[i] = toMultiplierCollection(vector_t(), vector_t()); + } + } + + return fromDualSolution(time, std::move(newDualSolution)); +} + +} // namespace ipm +} // namespace ocs2 From 496142c372bb9e3b41076fac102be98bd0c65113 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Thu, 15 Dec 2022 11:15:51 +0100 Subject: [PATCH 02/21] fix doc --- ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h b/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h index 3097800e0..a44b93f71 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h @@ -57,7 +57,8 @@ DualSolution toDualSolution(const std::vector& time, vector_array std::pair fromDualSolution(const std::vector& time, DualSolution&& dualSolution); /** - * Initializes the interior point trajectory. + * Initializes the interior point trajectory based on the stored trajectory. The part of the new trajectory that is uncovered by the stored + * trajectory is set by zero-sized vectors. * * @param time: The time discretization. * @param stateIneq: The slack/dual of the state inequality constraints. From 5c587761f30a81c39927c60e337de83180fec238 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Thu, 15 Dec 2022 17:14:58 +0100 Subject: [PATCH 03/21] fix bugs --- ocs2_ipm/CMakeLists.txt | 2 +- .../ocs2_ipm/IpmTrajectoryAdjustment.h | 21 ++++++++++--------- ocs2_ipm/src/IpmSolver.cpp | 4 ++-- ocs2_ipm/src/IpmTrajectoryAdjustment.cpp | 12 +++++------ .../ocs2_oc/oc_data/TimeDiscretization.h | 8 +++++++ ocs2_oc/src/oc_data/TimeDiscretization.cpp | 13 ++++++++++++ 6 files changed, 41 insertions(+), 19 deletions(-) diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt index beec07e7f..655a7a8ac 100644 --- a/ocs2_ipm/CMakeLists.txt +++ b/ocs2_ipm/CMakeLists.txt @@ -74,7 +74,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_FIX + CF_WERROR ) endif(cmake_clang_tools_FOUND) diff --git a/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h b/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h index a44b93f71..cea8a41ae 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h @@ -57,18 +57,19 @@ DualSolution toDualSolution(const std::vector& time, vector_array std::pair fromDualSolution(const std::vector& time, DualSolution&& dualSolution); /** - * Initializes the interior point trajectory based on the stored trajectory. The part of the new trajectory that is uncovered by the stored - * trajectory is set by zero-sized vectors. + * Interpolates the interior point trajectory using the stored interior point trajectory. The part of the new trajectory that is not + * covered by the stored trajectory is set by vectors of zero size. * - * @param time: The time discretization. - * @param stateIneq: The slack/dual of the state inequality constraints. - * @param stateInputIneq: The slack/dual of the state-input inequality constraints. - * @return A dual solution. + * @param oldModeSchedule: Mode schedule of the previous optimization. + * @param newModeSchedule: Mode schedule of the new optimization. + * @param time: Time discretization. + * @param oldDualSolution: The old dual solution that stored the previous interior point trajectory. + * @return Interpolated interior point trajectory. */ -std::pair initializeInteriorPointTrajectory(const ModeSchedule& oldModeSchedule, - const ModeSchedule& newModeSchedule, - const std::vector& time, - DualSolution&& oldDualSolution); +std::pair interpolateInteriorPointTrajectory(const ModeSchedule& oldModeSchedule, + const ModeSchedule& newModeSchedule, + const std::vector& time, + DualSolution&& oldDualSolution); } // 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 5365e9266..c8cad176e 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -200,9 +200,9 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f // Interior point variables scalar_t barrierParam = settings_.initialBarrierParameter; vector_array_t slackStateIneq, slackStateInputIneq, dualStateIneq, dualStateInputIneq; - std::tie(slackStateIneq, slackStateInputIneq) = ipm::initializeInteriorPointTrajectory( + std::tie(slackStateIneq, slackStateInputIneq) = ipm::interpolateInteriorPointTrajectory( primalSolution_.modeSchedule_, this->getReferenceManager().getModeSchedule(), timeDiscretization, std::move(slackIneqTrajectory_)); - std::tie(dualStateIneq, dualStateInputIneq) = ipm::initializeInteriorPointTrajectory( + std::tie(dualStateIneq, dualStateInputIneq) = ipm::interpolateInteriorPointTrajectory( primalSolution_.modeSchedule_, this->getReferenceManager().getModeSchedule(), timeDiscretization, std::move(dualIneqTrajectory_)); // Bookkeeping diff --git a/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp b/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp index eaf2645cc..7225965e5 100644 --- a/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp +++ b/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp @@ -63,7 +63,7 @@ DualSolution toDualSolution(const std::vector& time, vector_array const int N = static_cast(time.size()) - 1; DualSolution dualSolution; - dualSolution.timeTrajectory = toTime(time); + const auto newTimeTrajectory = toInterpolationTime(time); dualSolution.postEventIndices = toPostEventIndices(time); dualSolution.preJumps.reserve(dualSolution.postEventIndices.size()); @@ -113,10 +113,10 @@ std::pair fromDualSolution(const std::vector initializeInteriorPointTrajectory(const ModeSchedule& oldModeSchedule, - const ModeSchedule& newModeSchedule, - const std::vector& time, - DualSolution&& oldDualSolution) { +std::pair interpolateInteriorPointTrajectory(const ModeSchedule& oldModeSchedule, + const ModeSchedule& newModeSchedule, + const std::vector& time, + DualSolution&& oldDualSolution) { const auto oldTimeTrajectory = oldDualSolution.timeTrajectory; const auto oldPostEventIndices = oldDualSolution.postEventIndices; @@ -124,7 +124,7 @@ std::pair initializeInteriorPointTrajectory(cons std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, oldDualSolution); } - const auto newTimeTrajectory = toTime(time); + const auto newTimeTrajectory = toInterpolationTime(time); const auto newPostEventIndices = toPostEventIndices(time); // find the time period that we can interpolate the cached solution 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..466f68b48 100644 --- a/ocs2_oc/src/oc_data/TimeDiscretization.cpp +++ b/ocs2_oc/src/oc_data/TimeDiscretization.cpp @@ -122,6 +122,19 @@ scalar_array_t toTime(const std::vector& annotatedTime) { return timeTrajectory; } +scalar_array_t toInterpolationTime(const std::vector& annotatedTime) { + scalar_array_t timeTrajectory; + timeTrajectory.reserve(annotatedTime.size()); + for (size_t i = 0; i < annotatedTime.size(); i++) { + if (annotatedTime[i].event == AnnotatedTime::Event::PostEvent) { + timeTrajectory.push_back(getInterpolationTime(annotatedTime[i])); + } else { + timeTrajectory.push_back(annotatedTime[i].time); + } + } + return timeTrajectory; +} + size_array_t toPostEventIndices(const std::vector& annotatedTime) { size_array_t postEventIndices; for (size_t i = 0; i < annotatedTime.size(); i++) { From 7be7c496fbb570b51e4fab98556ec8025105b6a5 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Sun, 18 Dec 2022 09:38:27 +0100 Subject: [PATCH 04/21] modify trajectory spreading --- ocs2_ipm/src/IpmSolver.cpp | 32 +++++++++++++++++------- ocs2_ipm/src/IpmTrajectoryAdjustment.cpp | 12 +++------ 2 files changed, 26 insertions(+), 18 deletions(-) diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 6566cd09e..45170db0b 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -41,6 +41,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include "ocs2_ipm/IpmHelpers.h" #include "ocs2_ipm/IpmInitialization.h" @@ -205,7 +206,28 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f ocpDefinition.targetTrajectoriesPtr = &targetTrajectories; } - // Initialize the state and input + // Interior point variables + scalar_t barrierParam = settings_.initialBarrierParameter; + vector_array_t slackStateIneq, slackStateInputIneq, dualStateIneq, dualStateInputIneq; + if (!slackIneqTrajectory_.timeTrajectory.empty()) { + const auto& newModeSchedule = this->getReferenceManager().getModeSchedule(); + std::tie(slackStateIneq, slackStateInputIneq) = ipm::interpolateInteriorPointTrajectory( + primalSolution_.modeSchedule_, newModeSchedule, timeDiscretization, std::move(slackIneqTrajectory_)); + std::tie(dualStateIneq, dualStateInputIneq) = ipm::interpolateInteriorPointTrajectory( + primalSolution_.modeSchedule_, newModeSchedule, timeDiscretization, std::move(dualIneqTrajectory_)); + } else { + slackStateIneq.resize(timeDiscretization.size()); + slackStateInputIneq.resize(timeDiscretization.size() - 1); + dualStateIneq.resize(timeDiscretization.size()); + dualStateInputIneq.resize(timeDiscretization.size() - 1); + } + + // Trajectory spread of primalSolution_ after trajectory spread of the interior point variables + if (!primalSolution_.timeTrajectory_.empty()) { + std::ignore = trajectorySpread(primalSolution_.modeSchedule_, this->getReferenceManager().getModeSchedule(), primalSolution_); + } + + // Initialize the state and input after trajectory spread of primalSolution_ vector_array_t x, u; multiple_shooting::initializeStateInputTrajectories(initState, timeDiscretization, primalSolution_, *initializerPtr_, x, u); @@ -216,14 +238,6 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f initializeProjectionMultiplierTrajectory(timeDiscretization, nu); } - // Interior point variables - scalar_t barrierParam = settings_.initialBarrierParameter; - vector_array_t slackStateIneq, slackStateInputIneq, dualStateIneq, dualStateInputIneq; - std::tie(slackStateIneq, slackStateInputIneq) = ipm::interpolateInteriorPointTrajectory( - primalSolution_.modeSchedule_, this->getReferenceManager().getModeSchedule(), timeDiscretization, std::move(slackIneqTrajectory_)); - std::tie(dualStateIneq, dualStateInputIneq) = ipm::interpolateInteriorPointTrajectory( - primalSolution_.modeSchedule_, this->getReferenceManager().getModeSchedule(), timeDiscretization, std::move(dualIneqTrajectory_)); - // Bookkeeping performanceIndeces_.clear(); std::vector metrics; diff --git a/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp b/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp index 3a9a2cdb3..9176ac3d7 100644 --- a/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp +++ b/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp @@ -38,19 +38,13 @@ namespace { MultiplierCollection toMultiplierCollection(vector_t&& stateIneq, vector_t&& stateInputIneq = vector_t()) { MultiplierCollection multiplierCollection; multiplierCollection.stateIneq.emplace_back(0.0, std::move(stateIneq)); - if (stateInputIneq.size() > 0) { - multiplierCollection.stateInputIneq.emplace_back(0.0, std::move(stateInputIneq)); - } + multiplierCollection.stateInputIneq.emplace_back(0.0, std::move(stateInputIneq)); return multiplierCollection; } std::pair fromMultiplierCollection(MultiplierCollection&& multiplierCollection) { - if (multiplierCollection.stateInputIneq.empty()) { - return std::make_pair(std::move(multiplierCollection.stateIneq.front().lagrangian), vector_t()); - } else { - return std::make_pair(std::move(multiplierCollection.stateIneq.front().lagrangian), - std::move(multiplierCollection.stateInputIneq.front().lagrangian)); - } + return std::make_pair(std::move(multiplierCollection.stateIneq.front().lagrangian), + std::move(multiplierCollection.stateInputIneq.front().lagrangian)); } } // namespace } // namespace ocs2 From f42ec7fb3184e2f312e5087802b45a2376dc6f1c Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Sun, 18 Dec 2022 12:19:03 +0100 Subject: [PATCH 05/21] refactored trajectory spread of Ipm variables --- ocs2_ipm/src/IpmSolver.cpp | 11 +++++++---- ocs2_ipm/src/IpmTrajectoryAdjustment.cpp | 5 ----- 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 45170db0b..9b2651f01 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -210,11 +210,14 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f scalar_t barrierParam = settings_.initialBarrierParameter; vector_array_t slackStateIneq, slackStateInputIneq, dualStateIneq, dualStateInputIneq; if (!slackIneqTrajectory_.timeTrajectory.empty()) { + const auto& oldModeSchedule = primalSolution_.modeSchedule_; const auto& newModeSchedule = this->getReferenceManager().getModeSchedule(); - std::tie(slackStateIneq, slackStateInputIneq) = ipm::interpolateInteriorPointTrajectory( - primalSolution_.modeSchedule_, newModeSchedule, timeDiscretization, std::move(slackIneqTrajectory_)); - std::tie(dualStateIneq, dualStateInputIneq) = ipm::interpolateInteriorPointTrajectory( - primalSolution_.modeSchedule_, newModeSchedule, timeDiscretization, std::move(dualIneqTrajectory_)); + std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, slackIneqTrajectory_); + std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, dualIneqTrajectory_); + std::tie(slackStateIneq, slackStateInputIneq) = + ipm::interpolateInteriorPointTrajectory(oldModeSchedule, newModeSchedule, timeDiscretization, std::move(slackIneqTrajectory_)); + std::tie(dualStateIneq, dualStateInputIneq) = + ipm::interpolateInteriorPointTrajectory(oldModeSchedule, newModeSchedule, timeDiscretization, std::move(dualIneqTrajectory_)); } else { slackStateIneq.resize(timeDiscretization.size()); slackStateInputIneq.resize(timeDiscretization.size() - 1); diff --git a/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp b/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp index 9176ac3d7..646c3c2dc 100644 --- a/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp +++ b/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp @@ -112,11 +112,6 @@ std::pair interpolateInteriorPointTrajectory(con DualSolution&& oldDualSolution) { const auto oldTimeTrajectory = oldDualSolution.timeTrajectory; const auto oldPostEventIndices = oldDualSolution.postEventIndices; - - if (!oldTimeTrajectory.empty()) { - std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, oldDualSolution); - } - const auto newTimeTrajectory = toInterpolationTime(time); const auto newPostEventIndices = toPostEventIndices(time); From 95e0aacd3959aad31d50a852c9e4e56e556bbc32 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Sun, 18 Dec 2022 12:23:19 +0100 Subject: [PATCH 06/21] add trajectorySpread of primalSolution --- ocs2_slp/src/SlpSolver.cpp | 6 ++++++ ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp | 6 ++++++ 2 files changed, 12 insertions(+) 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); From 4a81c9e1b78104884282569d290999c747981031 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Sun, 18 Dec 2022 12:27:50 +0100 Subject: [PATCH 07/21] move functions to IpmHelpers --- ocs2_ipm/CMakeLists.txt | 1 - ocs2_ipm/include/ocs2_ipm/IpmHelpers.h | 36 ++++ .../ocs2_ipm/IpmTrajectoryAdjustment.h | 75 -------- ocs2_ipm/src/IpmHelpers.cpp | 129 ++++++++++++++ ocs2_ipm/src/IpmSolver.cpp | 1 - ocs2_ipm/src/IpmTrajectoryAdjustment.cpp | 168 ------------------ 6 files changed, 165 insertions(+), 245 deletions(-) delete mode 100644 ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h delete mode 100644 ocs2_ipm/src/IpmTrajectoryAdjustment.cpp diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt index 655a7a8ac..bd02a70cc 100644 --- a/ocs2_ipm/CMakeLists.txt +++ b/ocs2_ipm/CMakeLists.txt @@ -53,7 +53,6 @@ add_library(${PROJECT_NAME} src/IpmPerformanceIndexComputation.cpp src/IpmSettings.cpp src/IpmSolver.cpp - src/IpmTrajectoryAdjustment.cpp ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} diff --git a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h index 9531582a9..24d2dad39 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h @@ -30,6 +30,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once #include +#include +#include #include namespace ocs2 { @@ -107,5 +109,39 @@ 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 stateIneq: The slack/dual of the state inequality constraints. + * @param stateInputIneq: The slack/dual of the state-input inequality constraints. + * @return A dual solution. + */ +DualSolution toDualSolution(const std::vector& time, vector_array_t&& stateIneq, vector_array_t&& stateInputIneq); + +/** + * Convert the optimized slack or dual trajectories as a DualSolution. + * + * @param time: The time discretization. + * @param dualSolution: The dual solution. + * @return The slack/dual of the state inequality constraints (first) and state-input inequality constraints (second). + */ +std::pair fromDualSolution(const std::vector& time, DualSolution&& dualSolution); + +/** + * Interpolates the interior point trajectory using the stored interior point trajectory. The part of the new trajectory that is not + * covered by the stored trajectory is set by vectors of zero size. + * + * @param oldModeSchedule: Mode schedule of the previous optimization. + * @param newModeSchedule: Mode schedule of the new optimization. + * @param time: Time discretization. + * @param oldDualSolution: The old dual solution that stored the previous interior point trajectory. + * @return Interpolated interior point trajectory. + */ +std::pair interpolateInteriorPointTrajectory(const ModeSchedule& oldModeSchedule, + const ModeSchedule& newModeSchedule, + const std::vector& time, + DualSolution&& oldDualSolution); + } // namespace ipm } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h b/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h deleted file mode 100644 index cea8a41ae..000000000 --- a/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h +++ /dev/null @@ -1,75 +0,0 @@ -/****************************************************************************** -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. -******************************************************************************/ - -#pragma once - -#include - -#include -#include - -namespace ocs2 { -namespace ipm { - -/** - * Convert the optimized slack or dual trajectories as a DualSolution. - * - * @param time: The time discretization. - * @param stateIneq: The slack/dual of the state inequality constraints. - * @param stateInputIneq: The slack/dual of the state-input inequality constraints. - * @return A dual solution. - */ -DualSolution toDualSolution(const std::vector& time, vector_array_t&& stateIneq, vector_array_t&& stateInputIneq); - -/** - * Convert the optimized slack or dual trajectories as a DualSolution. - * - * @param time: The time discretization. - * @param dualSolution: The dual solution. - * @return The slack/dual of the state inequality constraints (first) and state-input inequality constraints (second). - */ -std::pair fromDualSolution(const std::vector& time, DualSolution&& dualSolution); - -/** - * Interpolates the interior point trajectory using the stored interior point trajectory. The part of the new trajectory that is not - * covered by the stored trajectory is set by vectors of zero size. - * - * @param oldModeSchedule: Mode schedule of the previous optimization. - * @param newModeSchedule: Mode schedule of the new optimization. - * @param time: Time discretization. - * @param oldDualSolution: The old dual solution that stored the previous interior point trajectory. - * @return Interpolated interior point trajectory. - */ -std::pair interpolateInteriorPointTrajectory(const ModeSchedule& oldModeSchedule, - const ModeSchedule& newModeSchedule, - const std::vector& time, - DualSolution&& oldDualSolution); - -} // namespace ipm -} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/src/IpmHelpers.cpp b/ocs2_ipm/src/IpmHelpers.cpp index ea7188006..1d6e3e2b5 100644 --- a/ocs2_ipm/src/IpmHelpers.cpp +++ b/ocs2_ipm/src/IpmHelpers.cpp @@ -31,6 +31,23 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +namespace ocs2 { +namespace { + +MultiplierCollection toMultiplierCollection(vector_t&& stateIneq, vector_t&& stateInputIneq = vector_t()) { + MultiplierCollection multiplierCollection; + multiplierCollection.stateIneq.emplace_back(0.0, std::move(stateIneq)); + multiplierCollection.stateInputIneq.emplace_back(0.0, std::move(stateInputIneq)); + return multiplierCollection; +} + +std::pair fromMultiplierCollection(MultiplierCollection&& multiplierCollection) { + return std::make_pair(std::move(multiplierCollection.stateIneq.front().lagrangian), + std::move(multiplierCollection.stateInputIneq.front().lagrangian)); +} +} // namespace +} // namespace ocs2 + namespace ocs2 { namespace ipm { @@ -113,5 +130,117 @@ 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; } +DualSolution toDualSolution(const std::vector& time, vector_array_t&& stateIneq, vector_array_t&& stateInputIneq) { + // Problem horizon + const int N = static_cast(time.size()) - 1; + + DualSolution dualSolution; + const auto newTimeTrajectory = 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(std::move(stateIneq[i]))); + dualSolution.intermediates.push_back(dualSolution.intermediates.back()); // no event at the initial node + } else { + dualSolution.intermediates.emplace_back(toMultiplierCollection(std::move(stateIneq[i]), std::move(stateInputIneq[i]))); + } + } + dualSolution.final = toMultiplierCollection(std::move(stateIneq[N])); + dualSolution.intermediates.push_back(dualSolution.intermediates.back()); + + return dualSolution; +} + +std::pair fromDualSolution(const std::vector& time, DualSolution&& dualSolution) { + // Problem horizon + const int N = static_cast(time.size()) - 1; + + vector_array_t stateIneq; + vector_array_t stateInputIneq; + stateIneq.reserve(N + 1); + stateInputIneq.reserve(N); + + int preJumpIdx = 0; + for (int i = 0; i < N; ++i) { + if (time[i].event == AnnotatedTime::Event::PreEvent) { + auto result = fromMultiplierCollection(std::move(dualSolution.preJumps[preJumpIdx])); + stateIneq.emplace_back(std::move(result.first)); + stateInputIneq.emplace_back(std::move(result.second)); + ++preJumpIdx; + } else { + auto result = fromMultiplierCollection(std::move(dualSolution.intermediates[i])); + stateIneq.emplace_back(std::move(result.first)); + stateInputIneq.emplace_back(std::move(result.second)); + } + } + + auto result = fromMultiplierCollection(std::move(dualSolution.final)); + stateIneq.emplace_back(std::move(result.first)); + + return std::make_pair(std::move(stateIneq), std::move(stateInputIneq)); +} + +std::pair interpolateInteriorPointTrajectory(const ModeSchedule& oldModeSchedule, + const ModeSchedule& newModeSchedule, + const std::vector& time, + DualSolution&& oldDualSolution) { + const auto oldTimeTrajectory = oldDualSolution.timeTrajectory; + const auto oldPostEventIndices = oldDualSolution.postEventIndices; + const auto newTimeTrajectory = toInterpolationTime(time); + const auto newPostEventIndices = toPostEventIndices(time); + + // 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, newModeSchedule.eventTimes, timePeriod); + const bool interpolateTillFinalTime = numerics::almost_eq(interpolatableTimePeriod.second, timePeriod.second); + + DualSolution newDualSolution; + + // set time and post-event indices + newDualSolution.timeTrajectory = std::move(newTimeTrajectory); + newDualSolution.postEventIndices = std::move(newPostEventIndices); + + // final + if (interpolateTillFinalTime) { + newDualSolution.final = std::move(oldDualSolution.final); + } else { + newDualSolution.final = toMultiplierCollection(vector_t()); + } + + // pre-jumps + newDualSolution.preJumps.resize(newDualSolution.postEventIndices.size()); + if (!newDualSolution.postEventIndices.empty()) { + const auto firstEventTime = newDualSolution.timeTrajectory[newDualSolution.postEventIndices[0] - 1]; + const auto cacheEventIndexBias = getNumberOfPrecedingEvents(oldTimeTrajectory, oldPostEventIndices, firstEventTime); + + for (size_t i = 0; i < newDualSolution.postEventIndices.size(); i++) { + const auto cachedTimeIndex = cacheEventIndexBias + i; + if (cachedTimeIndex < oldDualSolution.preJumps.size()) { + newDualSolution.preJumps[i] = std::move(oldDualSolution.preJumps[cachedTimeIndex]); + } else { + newDualSolution.preJumps[i] = toMultiplierCollection(vector_t()); + } + } + } + + // intermediates + newDualSolution.intermediates.resize(newDualSolution.timeTrajectory.size() - 1); + for (size_t i = 0; i < newDualSolution.timeTrajectory.size() - 1; i++) { + const auto time = newDualSolution.timeTrajectory[i]; + + if (interpolatableTimePeriod.first <= time && time <= interpolatableTimePeriod.second) { + newDualSolution.intermediates[i] = getIntermediateDualSolutionAtTime(oldDualSolution, time); + } else { + newDualSolution.intermediates[i] = toMultiplierCollection(vector_t(), vector_t()); + } + } + + return fromDualSolution(time, std::move(newDualSolution)); +} + } // namespace ipm } // namespace ocs2 diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 9b2651f01..2595e1f5b 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -46,7 +46,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ipm/IpmHelpers.h" #include "ocs2_ipm/IpmInitialization.h" #include "ocs2_ipm/IpmPerformanceIndexComputation.h" -#include "ocs2_ipm/IpmTrajectoryAdjustment.h" namespace ocs2 { diff --git a/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp b/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp deleted file mode 100644 index 646c3c2dc..000000000 --- a/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp +++ /dev/null @@ -1,168 +0,0 @@ -/****************************************************************************** -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/IpmTrajectoryAdjustment.h" - -#include -#include - -namespace ocs2 { -namespace { - -MultiplierCollection toMultiplierCollection(vector_t&& stateIneq, vector_t&& stateInputIneq = vector_t()) { - MultiplierCollection multiplierCollection; - multiplierCollection.stateIneq.emplace_back(0.0, std::move(stateIneq)); - multiplierCollection.stateInputIneq.emplace_back(0.0, std::move(stateInputIneq)); - return multiplierCollection; -} - -std::pair fromMultiplierCollection(MultiplierCollection&& multiplierCollection) { - return std::make_pair(std::move(multiplierCollection.stateIneq.front().lagrangian), - std::move(multiplierCollection.stateInputIneq.front().lagrangian)); -} -} // namespace -} // namespace ocs2 - -namespace ocs2 { -namespace ipm { - -DualSolution toDualSolution(const std::vector& time, vector_array_t&& stateIneq, vector_array_t&& stateInputIneq) { - // Problem horizon - const int N = static_cast(time.size()) - 1; - - DualSolution dualSolution; - const auto newTimeTrajectory = 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(std::move(stateIneq[i]))); - dualSolution.intermediates.push_back(dualSolution.intermediates.back()); // no event at the initial node - } else { - dualSolution.intermediates.emplace_back(toMultiplierCollection(std::move(stateIneq[i]), std::move(stateInputIneq[i]))); - } - } - dualSolution.final = toMultiplierCollection(std::move(stateIneq[N])); - dualSolution.intermediates.push_back(dualSolution.intermediates.back()); - - return dualSolution; -} - -std::pair fromDualSolution(const std::vector& time, DualSolution&& dualSolution) { - // Problem horizon - const int N = static_cast(time.size()) - 1; - - vector_array_t stateIneq; - vector_array_t stateInputIneq; - stateIneq.reserve(N + 1); - stateInputIneq.reserve(N); - - int preJumpIdx = 0; - for (int i = 0; i < N; ++i) { - if (time[i].event == AnnotatedTime::Event::PreEvent) { - auto result = fromMultiplierCollection(std::move(dualSolution.preJumps[preJumpIdx])); - stateIneq.emplace_back(std::move(result.first)); - stateInputIneq.emplace_back(std::move(result.second)); - ++preJumpIdx; - } else { - auto result = fromMultiplierCollection(std::move(dualSolution.intermediates[i])); - stateIneq.emplace_back(std::move(result.first)); - stateInputIneq.emplace_back(std::move(result.second)); - } - } - - auto result = fromMultiplierCollection(std::move(dualSolution.final)); - stateIneq.emplace_back(std::move(result.first)); - - return std::make_pair(std::move(stateIneq), std::move(stateInputIneq)); -} - -std::pair interpolateInteriorPointTrajectory(const ModeSchedule& oldModeSchedule, - const ModeSchedule& newModeSchedule, - const std::vector& time, - DualSolution&& oldDualSolution) { - const auto oldTimeTrajectory = oldDualSolution.timeTrajectory; - const auto oldPostEventIndices = oldDualSolution.postEventIndices; - const auto newTimeTrajectory = toInterpolationTime(time); - const auto newPostEventIndices = toPostEventIndices(time); - - // 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, newModeSchedule.eventTimes, timePeriod); - const bool interpolateTillFinalTime = numerics::almost_eq(interpolatableTimePeriod.second, timePeriod.second); - - DualSolution newDualSolution; - - // set time and post-event indices - newDualSolution.timeTrajectory = std::move(newTimeTrajectory); - newDualSolution.postEventIndices = std::move(newPostEventIndices); - - // final - if (interpolateTillFinalTime) { - newDualSolution.final = std::move(oldDualSolution.final); - } else { - newDualSolution.final = toMultiplierCollection(vector_t()); - } - - // pre-jumps - newDualSolution.preJumps.resize(newDualSolution.postEventIndices.size()); - if (!newDualSolution.postEventIndices.empty()) { - const auto firstEventTime = newDualSolution.timeTrajectory[newDualSolution.postEventIndices[0] - 1]; - const auto cacheEventIndexBias = getNumberOfPrecedingEvents(oldTimeTrajectory, oldPostEventIndices, firstEventTime); - - for (size_t i = 0; i < newDualSolution.postEventIndices.size(); i++) { - const auto cachedTimeIndex = cacheEventIndexBias + i; - if (cachedTimeIndex < oldDualSolution.preJumps.size()) { - newDualSolution.preJumps[i] = std::move(oldDualSolution.preJumps[cachedTimeIndex]); - } else { - newDualSolution.preJumps[i] = toMultiplierCollection(vector_t()); - } - } - } - - // intermediates - newDualSolution.intermediates.resize(newDualSolution.timeTrajectory.size() - 1); - for (size_t i = 0; i < newDualSolution.timeTrajectory.size() - 1; i++) { - const auto time = newDualSolution.timeTrajectory[i]; - - if (interpolatableTimePeriod.first <= time && time <= interpolatableTimePeriod.second) { - newDualSolution.intermediates[i] = getIntermediateDualSolutionAtTime(oldDualSolution, time); - } else { - newDualSolution.intermediates[i] = toMultiplierCollection(vector_t(), vector_t()); - } - } - - return fromDualSolution(time, std::move(newDualSolution)); -} - -} // namespace ipm -} // namespace ocs2 From 02ebd11d5be34bb6e6f3feb125bf3f2ef6d18bed Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Sun, 18 Dec 2022 15:48:15 +0100 Subject: [PATCH 08/21] remove unused argment --- ocs2_ipm/include/ocs2_ipm/IpmHelpers.h | 6 ++---- ocs2_ipm/src/IpmHelpers.cpp | 5 ++--- ocs2_ipm/src/IpmSolver.cpp | 4 ++-- 3 files changed, 6 insertions(+), 9 deletions(-) diff --git a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h index 24d2dad39..041994304 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h @@ -132,14 +132,12 @@ std::pair fromDualSolution(const std::vector interpolateInteriorPointTrajectory(const ModeSchedule& oldModeSchedule, - const ModeSchedule& newModeSchedule, +std::pair interpolateInteriorPointTrajectory(const ModeSchedule& modeSchedule, const std::vector& time, DualSolution&& oldDualSolution); diff --git a/ocs2_ipm/src/IpmHelpers.cpp b/ocs2_ipm/src/IpmHelpers.cpp index 1d6e3e2b5..cc0c8ba8a 100644 --- a/ocs2_ipm/src/IpmHelpers.cpp +++ b/ocs2_ipm/src/IpmHelpers.cpp @@ -184,8 +184,7 @@ std::pair fromDualSolution(const std::vector interpolateInteriorPointTrajectory(const ModeSchedule& oldModeSchedule, - const ModeSchedule& newModeSchedule, +std::pair interpolateInteriorPointTrajectory(const ModeSchedule& modeSchedule, const std::vector& time, DualSolution&& oldDualSolution) { const auto oldTimeTrajectory = oldDualSolution.timeTrajectory; @@ -195,7 +194,7 @@ std::pair interpolateInteriorPointTrajectory(con // 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, newModeSchedule.eventTimes, timePeriod); + const auto interpolatableTimePeriod = findIntersectionToExtendableInterval(oldTimeTrajectory, modeSchedule.eventTimes, timePeriod); const bool interpolateTillFinalTime = numerics::almost_eq(interpolatableTimePeriod.second, timePeriod.second); DualSolution newDualSolution; diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 2595e1f5b..79bb24bc2 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -214,9 +214,9 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, slackIneqTrajectory_); std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, dualIneqTrajectory_); std::tie(slackStateIneq, slackStateInputIneq) = - ipm::interpolateInteriorPointTrajectory(oldModeSchedule, newModeSchedule, timeDiscretization, std::move(slackIneqTrajectory_)); + ipm::interpolateInteriorPointTrajectory(newModeSchedule, timeDiscretization, std::move(slackIneqTrajectory_)); std::tie(dualStateIneq, dualStateInputIneq) = - ipm::interpolateInteriorPointTrajectory(oldModeSchedule, newModeSchedule, timeDiscretization, std::move(dualIneqTrajectory_)); + ipm::interpolateInteriorPointTrajectory(newModeSchedule, timeDiscretization, std::move(dualIneqTrajectory_)); } else { slackStateIneq.resize(timeDiscretization.size()); slackStateInputIneq.resize(timeDiscretization.size() - 1); From 047f5dcddc9271313658edc48101a93b7c4f2f29 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Mon, 19 Dec 2022 16:18:57 +0100 Subject: [PATCH 09/21] modify initialization of the slack and dual trajectory --- ocs2_ipm/CMakeLists.txt | 2 +- ocs2_ipm/include/ocs2_ipm/IpmHelpers.h | 36 ++-- ocs2_ipm/include/ocs2_ipm/IpmSolver.h | 18 +- ocs2_ipm/src/IpmHelpers.cpp | 140 ++++----------- ocs2_ipm/src/IpmSolver.cpp | 198 ++++++++++++++------- ocs2_oc/src/oc_data/TimeDiscretization.cpp | 3 +- 6 files changed, 203 insertions(+), 194 deletions(-) diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt index bd02a70cc..d5e27aae7 100644 --- 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 index 041994304..23e97b716 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h @@ -113,33 +113,35 @@ scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scala * Convert the optimized slack or dual trajectories as a DualSolution. * * @param time: The time discretization. - * @param stateIneq: The slack/dual of the state inequality constraints. - * @param stateInputIneq: The slack/dual of the state-input inequality constraints. + * @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. * @return A dual solution. */ -DualSolution toDualSolution(const std::vector& time, vector_array_t&& stateIneq, vector_array_t&& stateInputIneq); +DualSolution toDualSolution(const std::vector& time, vector_array_t&& slackStateIneq, vector_array_t&& dualStateIneq, + vector_array_t&& slackStateInputIneq, vector_array_t&& dualStateInputIneq); /** - * Convert the optimized slack or dual trajectories as a DualSolution. + * Extracts a slack variable of the state-only and state-input constraints from a MultiplierCollection * - * @param time: The time discretization. - * @param dualSolution: The dual solution. - * @return The slack/dual of the state inequality constraints (first) and state-input inequality constraints (second). + * @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. */ -std::pair fromDualSolution(const std::vector& time, DualSolution&& dualSolution); +void toSlackDual(MultiplierCollection&& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq, + vector_t& slackStateInputIneq, vector_t& dualStateInputIneq); /** - * Interpolates the interior point trajectory using the stored interior point trajectory. The part of the new trajectory that is not - * covered by the stored trajectory is set by vectors of zero size. + * Extracts a slack variable of the state-only constraints from a MultiplierCollection * - * @param modeSchedule: Mode schedule. - * @param time: Time discretization. - * @param oldDualSolution: The old dual solution that stored the previous interior point trajectory. - * @return Interpolated interior point trajectory. + * @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. */ -std::pair interpolateInteriorPointTrajectory(const ModeSchedule& modeSchedule, - const std::vector& time, - DualSolution&& oldDualSolution); +void toSlackDual(MultiplierCollection&& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq); } // 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 410bd45c2..073eae008 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h @@ -123,12 +123,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, - 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 +147,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,8 +201,7 @@ class IpmSolver : public SolverBase { PrimalSolution primalSolution_; vector_array_t costateTrajectory_; vector_array_t projectionMultiplierTrajectory_; - DualSolution slackIneqTrajectory_; - DualSolution dualIneqTrajectory_; + DualSolution slackDualTrajectory_; // Value function in absolute state coordinates (without the constant value) std::vector valueFunction_; diff --git a/ocs2_ipm/src/IpmHelpers.cpp b/ocs2_ipm/src/IpmHelpers.cpp index cc0c8ba8a..d962cf1c9 100644 --- a/ocs2_ipm/src/IpmHelpers.cpp +++ b/ocs2_ipm/src/IpmHelpers.cpp @@ -31,23 +31,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -namespace ocs2 { -namespace { - -MultiplierCollection toMultiplierCollection(vector_t&& stateIneq, vector_t&& stateInputIneq = vector_t()) { - MultiplierCollection multiplierCollection; - multiplierCollection.stateIneq.emplace_back(0.0, std::move(stateIneq)); - multiplierCollection.stateInputIneq.emplace_back(0.0, std::move(stateInputIneq)); - return multiplierCollection; -} - -std::pair fromMultiplierCollection(MultiplierCollection&& multiplierCollection) { - return std::make_pair(std::move(multiplierCollection.stateIneq.front().lagrangian), - std::move(multiplierCollection.stateInputIneq.front().lagrangian)); -} -} // namespace -} // namespace ocs2 - namespace ocs2 { namespace ipm { @@ -130,12 +113,38 @@ 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; } -DualSolution toDualSolution(const std::vector& time, vector_array_t&& stateIneq, vector_array_t&& stateInputIneq) { +namespace { +MultiplierCollection toMultiplierCollection(vector_t&& slackStateIneq, vector_t&& dualStateIneq, + vector_t&& slackStateInputIneq = vector_t(), vector_t&& dualStateInputIneq = vector_t()) { + MultiplierCollection multiplierCollection; + multiplierCollection.stateIneq.emplace_back(0.0, std::move(slackStateIneq)); + multiplierCollection.stateEq.emplace_back(0.0, std::move(dualStateIneq)); + multiplierCollection.stateInputIneq.emplace_back(0.0, std::move(slackStateInputIneq)); + multiplierCollection.stateInputEq.emplace_back(0.0, std::move(dualStateInputIneq)); + return multiplierCollection; +} +} // namespace + +void toSlackDual(MultiplierCollection&& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq, + vector_t& slackStateInputIneq, vector_t& dualStateInputIneq) { + slackStateIneq = std::move(multiplierCollection.stateIneq.front().lagrangian); + dualStateIneq = std::move(multiplierCollection.stateEq.front().lagrangian); + slackStateInputIneq = std::move(multiplierCollection.stateInputIneq.front().lagrangian); + dualStateInputIneq = std::move(multiplierCollection.stateInputEq.front().lagrangian); +} + +void toSlackDual(MultiplierCollection&& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq) { + slackStateIneq = std::move(multiplierCollection.stateIneq.front().lagrangian); + dualStateIneq = std::move(multiplierCollection.stateEq.front().lagrangian); +} + +DualSolution toDualSolution(const std::vector& time, vector_array_t&& slackStateIneq, vector_array_t&& dualStateIneq, + vector_array_t&& slackStateInputIneq, vector_array_t&& dualStateInputIneq) { // Problem horizon const int N = static_cast(time.size()) - 1; DualSolution dualSolution; - const auto newTimeTrajectory = toInterpolationTime(time); + dualSolution.timeTrajectory = toInterpolationTime(time); dualSolution.postEventIndices = toPostEventIndices(time); dualSolution.preJumps.reserve(dualSolution.postEventIndices.size()); @@ -143,103 +152,18 @@ DualSolution toDualSolution(const std::vector& time, vector_array for (int i = 0; i < N; ++i) { if (time[i].event == AnnotatedTime::Event::PreEvent) { - dualSolution.preJumps.emplace_back(toMultiplierCollection(std::move(stateIneq[i]))); + dualSolution.preJumps.emplace_back(toMultiplierCollection(std::move(slackStateIneq[i]), std::move(dualStateIneq[i]))); dualSolution.intermediates.push_back(dualSolution.intermediates.back()); // no event at the initial node } else { - dualSolution.intermediates.emplace_back(toMultiplierCollection(std::move(stateIneq[i]), std::move(stateInputIneq[i]))); + dualSolution.intermediates.emplace_back(toMultiplierCollection(std::move(slackStateIneq[i]), std::move(dualStateIneq[i]), + std::move(slackStateInputIneq[i]), std::move(dualStateInputIneq[i]))); } } - dualSolution.final = toMultiplierCollection(std::move(stateIneq[N])); + dualSolution.final = toMultiplierCollection(std::move(slackStateIneq[N]), std::move(dualStateIneq[N])); dualSolution.intermediates.push_back(dualSolution.intermediates.back()); return dualSolution; } -std::pair fromDualSolution(const std::vector& time, DualSolution&& dualSolution) { - // Problem horizon - const int N = static_cast(time.size()) - 1; - - vector_array_t stateIneq; - vector_array_t stateInputIneq; - stateIneq.reserve(N + 1); - stateInputIneq.reserve(N); - - int preJumpIdx = 0; - for (int i = 0; i < N; ++i) { - if (time[i].event == AnnotatedTime::Event::PreEvent) { - auto result = fromMultiplierCollection(std::move(dualSolution.preJumps[preJumpIdx])); - stateIneq.emplace_back(std::move(result.first)); - stateInputIneq.emplace_back(std::move(result.second)); - ++preJumpIdx; - } else { - auto result = fromMultiplierCollection(std::move(dualSolution.intermediates[i])); - stateIneq.emplace_back(std::move(result.first)); - stateInputIneq.emplace_back(std::move(result.second)); - } - } - - auto result = fromMultiplierCollection(std::move(dualSolution.final)); - stateIneq.emplace_back(std::move(result.first)); - - return std::make_pair(std::move(stateIneq), std::move(stateInputIneq)); -} - -std::pair interpolateInteriorPointTrajectory(const ModeSchedule& modeSchedule, - const std::vector& time, - DualSolution&& oldDualSolution) { - const auto oldTimeTrajectory = oldDualSolution.timeTrajectory; - const auto oldPostEventIndices = oldDualSolution.postEventIndices; - const auto newTimeTrajectory = toInterpolationTime(time); - const auto newPostEventIndices = toPostEventIndices(time); - - // 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, modeSchedule.eventTimes, timePeriod); - const bool interpolateTillFinalTime = numerics::almost_eq(interpolatableTimePeriod.second, timePeriod.second); - - DualSolution newDualSolution; - - // set time and post-event indices - newDualSolution.timeTrajectory = std::move(newTimeTrajectory); - newDualSolution.postEventIndices = std::move(newPostEventIndices); - - // final - if (interpolateTillFinalTime) { - newDualSolution.final = std::move(oldDualSolution.final); - } else { - newDualSolution.final = toMultiplierCollection(vector_t()); - } - - // pre-jumps - newDualSolution.preJumps.resize(newDualSolution.postEventIndices.size()); - if (!newDualSolution.postEventIndices.empty()) { - const auto firstEventTime = newDualSolution.timeTrajectory[newDualSolution.postEventIndices[0] - 1]; - const auto cacheEventIndexBias = getNumberOfPrecedingEvents(oldTimeTrajectory, oldPostEventIndices, firstEventTime); - - for (size_t i = 0; i < newDualSolution.postEventIndices.size(); i++) { - const auto cachedTimeIndex = cacheEventIndexBias + i; - if (cachedTimeIndex < oldDualSolution.preJumps.size()) { - newDualSolution.preJumps[i] = std::move(oldDualSolution.preJumps[cachedTimeIndex]); - } else { - newDualSolution.preJumps[i] = toMultiplierCollection(vector_t()); - } - } - } - - // intermediates - newDualSolution.intermediates.resize(newDualSolution.timeTrajectory.size() - 1); - for (size_t i = 0; i < newDualSolution.timeTrajectory.size() - 1; i++) { - const auto time = newDualSolution.timeTrajectory[i]; - - if (interpolatableTimePeriod.first <= time && time <= interpolatableTimePeriod.second) { - newDualSolution.intermediates[i] = getIntermediateDualSolutionAtTime(oldDualSolution, time); - } else { - newDualSolution.intermediates[i] = toMultiplierCollection(vector_t(), vector_t()); - } - } - - return fromDualSolution(time, std::move(newDualSolution)); -} - } // namespace ipm } // namespace ocs2 diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 79bb24bc2..e0bf26c7f 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -101,8 +101,7 @@ void IpmSolver::reset() { primalSolution_ = PrimalSolution(); costateTrajectory_.clear(); projectionMultiplierTrajectory_.clear(); - slackIneqTrajectory_.clear(); - dualIneqTrajectory_.clear(); + slackDualTrajectory_.clear(); valueFunction_.clear(); performanceIndeces_.clear(); @@ -205,34 +204,26 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f ocpDefinition.targetTrajectoriesPtr = &targetTrajectories; } - // Interior point variables - scalar_t barrierParam = settings_.initialBarrierParameter; - vector_array_t slackStateIneq, slackStateInputIneq, dualStateIneq, dualStateInputIneq; - if (!slackIneqTrajectory_.timeTrajectory.empty()) { - const auto& oldModeSchedule = primalSolution_.modeSchedule_; - const auto& newModeSchedule = this->getReferenceManager().getModeSchedule(); - std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, slackIneqTrajectory_); - std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, dualIneqTrajectory_); - std::tie(slackStateIneq, slackStateInputIneq) = - ipm::interpolateInteriorPointTrajectory(newModeSchedule, timeDiscretization, std::move(slackIneqTrajectory_)); - std::tie(dualStateIneq, dualStateInputIneq) = - ipm::interpolateInteriorPointTrajectory(newModeSchedule, timeDiscretization, std::move(dualIneqTrajectory_)); - } else { - slackStateIneq.resize(timeDiscretization.size()); - slackStateInputIneq.resize(timeDiscretization.size() - 1); - dualStateIneq.resize(timeDiscretization.size()); - dualStateInputIneq.resize(timeDiscretization.size() - 1); - } + // old and new mode schedules for the trajectory spreading + const auto oldModeSchedule = primalSolution_.modeSchedule_; + const auto& newModeSchedule = this->getReferenceManager().getModeSchedule(); - // Trajectory spread of primalSolution_ after trajectory spread of the interior point variables + // Initialize the state and input if (!primalSolution_.timeTrajectory_.empty()) { - std::ignore = trajectorySpread(primalSolution_.modeSchedule_, this->getReferenceManager().getModeSchedule(), primalSolution_); + std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, primalSolution_); } - - // Initialize the state and input after trajectory spread of 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 (!slackDualTrajectory_.timeTrajectory.empty()) { + std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, slackDualTrajectory_); + } + 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) { @@ -261,7 +252,7 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f 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(); @@ -291,8 +282,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); - slackIneqTrajectory_ = ipm::toDualSolution(timeDiscretization, std::move(slackStateIneq), std::move(slackStateInputIneq)); - dualIneqTrajectory_ = ipm::toDualSolution(timeDiscretization, std::move(dualStateIneq), std::move(dualStateInputIneq)); + slackDualTrajectory_ = ipm::toDualSolution(timeDiscretization, std::move(slackStateIneq), std::move(dualStateIneq), + std::move(slackStateInputIneq), std::move(dualStateInputIneq)); problemMetrics_ = multiple_shooting::toProblemMetrics(timeDiscretization, std::move(metrics)); computeControllerTimer_.endTimer(); @@ -380,9 +371,122 @@ 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 = slackDualTrajectory_.timeTrajectory; + const auto& oldPostEventIndices = slackDualTrajectory_.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 < slackDualTrajectory_.preJumps.size()) { + ipm::toSlackDual(std::move(slackDualTrajectory_.preJumps[cachedEventIndex]), slackStateIneq[i], dualStateIneq[i]); + } else { + const auto time = newTimeTrajectory[i]; + const auto& state = x[i]; + constexpr auto request = Request::Constraint; + ocpDefinition.preComputationPtr->requestPreJump(request, time, state); + auto& preComputation = *ocpDefinition.preComputationPtr; + if (!ocpDefinition.preJumpInequalityConstraintPtr->empty()) { + const auto ineqConstraint = toVector(ocpDefinition.preJumpInequalityConstraintPtr->getValue(time, state, preComputation)); + slackStateIneq[i] = + ipm::initializeSlackVariable(ineqConstraint, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); + dualStateIneq[i] = ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, + settings_.initialDualMarginRate); + } else { + slackStateIneq[i].resize(0); + dualStateIneq[i].resize(0); + } + } + slackStateInputIneq[i].resize(0); + dualStateInputIneq[i].resize(0); + ++eventIdx; + } else { + const auto time = newTimeTrajectory[i]; + if (interpolatableTimePeriod.first <= time && time <= interpolatableTimePeriod.second) { + ipm::toSlackDual(getIntermediateDualSolutionAtTime(slackDualTrajectory_, time), slackStateIneq[i], dualStateIneq[i], + slackStateInputIneq[i], dualStateInputIneq[i]); + } else { + constexpr auto request = Request::Constraint; + const auto& state = x[i]; + const auto& input = u[i]; + ocpDefinition.preComputationPtr->request(request, time, state, input); + if (!ocpDefinition.stateInequalityConstraintPtr->empty() && i > 0) { + const auto ineqConstraint = + toVector(ocpDefinition.stateInequalityConstraintPtr->getValue(time, state, *ocpDefinition.preComputationPtr)); + slackStateIneq[i] = + ipm::initializeSlackVariable(ineqConstraint, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); + dualStateIneq[i] = ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, + settings_.initialDualMarginRate); + } else { + slackStateIneq[i].resize(0); + dualStateIneq[i].resize(0); + } + if (!ocpDefinition.inequalityConstraintPtr->empty()) { + const auto ineqConstraint = + toVector(ocpDefinition.inequalityConstraintPtr->getValue(time, state, input, *ocpDefinition.preComputationPtr)); + slackStateInputIneq[i] = + ipm::initializeSlackVariable(ineqConstraint, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); + dualStateInputIneq[i] = ipm::initializeDualVariable(slackStateInputIneq[i], barrierParam, settings_.initialDualLowerBound, + settings_.initialDualMarginRate); + } else { + slackStateInputIneq[i].resize(0); + dualStateInputIneq[i].resize(0); + } + } + } + } + + if (interpolateTillFinalTime) { + ipm::toSlackDual(std::move(slackDualTrajectory_.final), slackStateIneq[N], dualStateIneq[N]); + } else { + const auto time = newTimeTrajectory[N]; + const auto& state = x[N]; + constexpr auto request = Request::Constraint; + ocpDefinition.preComputationPtr->requestFinal(request, time, state); + auto& preComputation = *ocpDefinition.preComputationPtr; + if (!ocpDefinition.finalInequalityConstraintPtr->empty()) { + const auto ineqConstraint = toVector(ocpDefinition.finalInequalityConstraintPtr->getValue(time, state, preComputation)); + slackStateIneq[N] = ipm::initializeSlackVariable(ineqConstraint, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); + dualStateIneq[N] = + ipm::initializeDualVariable(slackStateIneq[N], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); + } else { + slackStateIneq[N].resize(0); + dualStateIneq[N].resize(0); + } + } +} + 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; @@ -409,14 +513,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); @@ -517,9 +621,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, 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; @@ -543,14 +647,6 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector& annotatedTime) { scalar_array_t toInterpolationTime(const std::vector& annotatedTime) { scalar_array_t timeTrajectory; timeTrajectory.reserve(annotatedTime.size()); - for (size_t i = 0; i < annotatedTime.size(); i++) { + for (size_t i = 0; 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; } From 67237dd5c5f0d60524fb809b092c1009bb34cc8d Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Mon, 19 Dec 2022 16:36:14 +0100 Subject: [PATCH 10/21] fix time trajectory for interpolation --- ocs2_ipm/src/IpmSolver.cpp | 4 ++-- ocs2_oc/src/oc_data/TimeDiscretization.cpp | 6 +++++- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index e0bf26c7f..1f541437b 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -408,7 +408,7 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector& if (cachedEventIndex < slackDualTrajectory_.preJumps.size()) { ipm::toSlackDual(std::move(slackDualTrajectory_.preJumps[cachedEventIndex]), slackStateIneq[i], dualStateIneq[i]); } else { - const auto time = newTimeTrajectory[i]; + const auto time = getIntervalStart(timeDiscretization[i]); const auto& state = x[i]; constexpr auto request = Request::Constraint; ocpDefinition.preComputationPtr->requestPreJump(request, time, state); @@ -428,7 +428,7 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector& dualStateInputIneq[i].resize(0); ++eventIdx; } else { - const auto time = newTimeTrajectory[i]; + 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]); diff --git a/ocs2_oc/src/oc_data/TimeDiscretization.cpp b/ocs2_oc/src/oc_data/TimeDiscretization.cpp index 18eb53170..5b9f86ab3 100644 --- a/ocs2_oc/src/oc_data/TimeDiscretization.cpp +++ b/ocs2_oc/src/oc_data/TimeDiscretization.cpp @@ -123,9 +123,13 @@ scalar_array_t toTime(const std::vector& annotatedTime) { } scalar_array_t toInterpolationTime(const std::vector& annotatedTime) { + if (annotatedTime.empty()) { + return scalar_array_t(); + } scalar_array_t timeTrajectory; timeTrajectory.reserve(annotatedTime.size()); - for (size_t i = 0; i < annotatedTime.size() - 1; i++) { + 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 { From 666b81da996e044cf4d194b8f5f4ab0cdf4157a6 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Mon, 19 Dec 2022 16:57:02 +0100 Subject: [PATCH 11/21] fix CMakeLists and add tests --- ocs2_ipm/CMakeLists.txt | 2 +- ocs2_ipm/test/Exp0Test.cpp | 6 ++++++ ocs2_ipm/test/Exp1Test.cpp | 12 ++++++++++++ ocs2_ipm/test/testCircularKinematics.cpp | 13 ++++++++++++- 4 files changed, 31 insertions(+), 2 deletions(-) diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt index d5e27aae7..bd02a70cc 100644 --- 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_FIX + CF_WERROR ) endif(cmake_clang_tools_FOUND) diff --git a/ocs2_ipm/test/Exp0Test.cpp b/ocs2_ipm/test/Exp0Test.cpp index 3521e8d90..ba7a8b25e 100644 --- a/ocs2_ipm/test/Exp0Test.cpp +++ b/ocs2_ipm/test/Exp0Test.cpp @@ -168,4 +168,10 @@ TEST(Exp0Test, 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); + } } \ 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 From 6a3d7ce0263e54dc3e15815b3e713865ed6c4922 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Mon, 19 Dec 2022 17:42:02 +0100 Subject: [PATCH 12/21] modify the dual solution that stores the slack and dual trajectory --- ocs2_ipm/include/ocs2_ipm/IpmHelpers.h | 11 ++-- ocs2_ipm/include/ocs2_ipm/IpmSolver.h | 4 ++ ocs2_ipm/src/IpmHelpers.cpp | 69 ++++++++++++++++++-------- ocs2_ipm/src/IpmSolver.cpp | 9 ++-- 4 files changed, 66 insertions(+), 27 deletions(-) diff --git a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h index 23e97b716..4b4d1f224 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h @@ -30,6 +30,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once #include +#include #include #include #include @@ -113,14 +114,16 @@ scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scala * Convert the optimized slack or dual trajectories as a DualSolution. * * @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. * @return A dual solution. */ -DualSolution toDualSolution(const std::vector& time, vector_array_t&& slackStateIneq, vector_array_t&& dualStateIneq, - vector_array_t&& slackStateInputIneq, vector_array_t&& dualStateInputIneq); +DualSolution toDualSolution(const std::vector& time, const std::vector& constraintsSize, + const vector_array_t& slackStateIneq, const vector_array_t& dualStateIneq, + const vector_array_t& slackStateInputIneq, const vector_array_t& dualStateInputIneq); /** * Extracts a slack variable of the state-only and state-input constraints from a MultiplierCollection @@ -131,7 +134,7 @@ DualSolution toDualSolution(const std::vector& time, vector_array * @param[out] slackStateInputIneq: The slack variable of the state-input inequality constraints. * @param[out] dualStateInputIneq: The dual variable of the state-input inequality constraints. */ -void toSlackDual(MultiplierCollection&& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq, +void toSlackDual(const MultiplierCollection& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq, vector_t& slackStateInputIneq, vector_t& dualStateInputIneq); /** @@ -141,7 +144,7 @@ void toSlackDual(MultiplierCollection&& multiplierCollection, vector_t& slackSta * @param[out] slackStateIneq: The slack variable of the state inequality constraints. * @param[out] dualStateIneq: The dual variable of the state inequality constraints. */ -void toSlackDual(MultiplierCollection&& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq); +void toSlackDual(const MultiplierCollection& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq); } // 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 073eae008..f77a8efea 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 @@ -214,6 +215,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 d962cf1c9..3159d6f74 100644 --- a/ocs2_ipm/src/IpmHelpers.cpp +++ b/ocs2_ipm/src/IpmHelpers.cpp @@ -114,32 +114,61 @@ scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scala } namespace { -MultiplierCollection toMultiplierCollection(vector_t&& slackStateIneq, vector_t&& dualStateIneq, - vector_t&& slackStateInputIneq = vector_t(), vector_t&& dualStateInputIneq = vector_t()) { +MultiplierCollection toMultiplierCollection(const multiple_shooting::ConstraintsSize constraintsSize, const vector_t& slackStateIneq, + const vector_t& dualStateIneq) { MultiplierCollection multiplierCollection; - multiplierCollection.stateIneq.emplace_back(0.0, std::move(slackStateIneq)); - multiplierCollection.stateEq.emplace_back(0.0, std::move(dualStateIneq)); - multiplierCollection.stateInputIneq.emplace_back(0.0, std::move(slackStateInputIneq)); - multiplierCollection.stateInputEq.emplace_back(0.0, std::move(dualStateInputIneq)); + 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)); + 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); + 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)); + } 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 -void toSlackDual(MultiplierCollection&& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq, +void toSlackDual(const MultiplierCollection& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq, vector_t& slackStateInputIneq, vector_t& dualStateInputIneq) { - slackStateIneq = std::move(multiplierCollection.stateIneq.front().lagrangian); - dualStateIneq = std::move(multiplierCollection.stateEq.front().lagrangian); - slackStateInputIneq = std::move(multiplierCollection.stateInputIneq.front().lagrangian); - dualStateInputIneq = std::move(multiplierCollection.stateInputEq.front().lagrangian); + slackStateIneq = extractLagrangian(multiplierCollection.stateIneq); + dualStateIneq = extractLagrangian(multiplierCollection.stateEq); + slackStateInputIneq = extractLagrangian(multiplierCollection.stateInputIneq); + dualStateInputIneq = extractLagrangian(multiplierCollection.stateInputEq); } -void toSlackDual(MultiplierCollection&& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq) { - slackStateIneq = std::move(multiplierCollection.stateIneq.front().lagrangian); - dualStateIneq = std::move(multiplierCollection.stateEq.front().lagrangian); +void toSlackDual(const MultiplierCollection& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq) { + slackStateIneq = extractLagrangian(multiplierCollection.stateIneq); + dualStateIneq = extractLagrangian(multiplierCollection.stateEq); } -DualSolution toDualSolution(const std::vector& time, vector_array_t&& slackStateIneq, vector_array_t&& dualStateIneq, - vector_array_t&& slackStateInputIneq, vector_array_t&& dualStateInputIneq) { +DualSolution toDualSolution(const std::vector& time, const std::vector& constraintsSize, + const vector_array_t& slackStateIneq, const vector_array_t& dualStateIneq, + const vector_array_t& slackStateInputIneq, const vector_array_t& dualStateInputIneq) { // Problem horizon const int N = static_cast(time.size()) - 1; @@ -152,14 +181,14 @@ DualSolution toDualSolution(const std::vector& time, vector_array for (int i = 0; i < N; ++i) { if (time[i].event == AnnotatedTime::Event::PreEvent) { - dualSolution.preJumps.emplace_back(toMultiplierCollection(std::move(slackStateIneq[i]), std::move(dualStateIneq[i]))); + dualSolution.preJumps.emplace_back(toMultiplierCollection(constraintsSize[i], slackStateIneq[i], dualStateIneq[i])); dualSolution.intermediates.push_back(dualSolution.intermediates.back()); // no event at the initial node } else { - dualSolution.intermediates.emplace_back(toMultiplierCollection(std::move(slackStateIneq[i]), std::move(dualStateIneq[i]), - std::move(slackStateInputIneq[i]), std::move(dualStateInputIneq[i]))); + dualSolution.intermediates.emplace_back( + toMultiplierCollection(constraintsSize[i], slackStateIneq[i], dualStateIneq[i], slackStateInputIneq[i], dualStateInputIneq[i])); } } - dualSolution.final = toMultiplierCollection(std::move(slackStateIneq[N]), std::move(dualStateIneq[N])); + dualSolution.final = toMultiplierCollection(constraintsSize[N], slackStateIneq[N], dualStateIneq[N]); dualSolution.intermediates.push_back(dualSolution.intermediates.back()); return dualSolution; diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 1f541437b..94986ee5b 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -39,7 +39,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include #include #include @@ -282,8 +281,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, std::move(slackStateIneq), std::move(dualStateIneq), - std::move(slackStateInputIneq), std::move(dualStateInputIneq)); + slackDualTrajectory_ = + ipm::toDualSolution(timeDiscretization, constraintsSize_, slackStateIneq, dualStateIneq, slackStateInputIneq, dualStateInputIneq); problemMetrics_ = multiple_shooting::toProblemMetrics(timeDiscretization, std::move(metrics)); computeControllerTimer_.endTimer(); @@ -635,6 +634,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector Date: Mon, 19 Dec 2022 17:59:06 +0100 Subject: [PATCH 13/21] refactored slack and dual initialization --- ocs2_ipm/CMakeLists.txt | 2 +- ocs2_ipm/include/ocs2_ipm/IpmHelpers.h | 30 ++++-------------- ocs2_ipm/include/ocs2_ipm/IpmSolver.h | 3 +- ocs2_ipm/src/IpmHelpers.cpp | 44 +++++++++----------------- ocs2_ipm/src/IpmSolver.cpp | 30 +++++++++++------- 5 files changed, 43 insertions(+), 66 deletions(-) diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt index bd02a70cc..d5e27aae7 100644 --- 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 index 4b4d1f224..5eae47eb2 100644 --- 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& time, const std::vector& 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 fromDualSolution(const MultiplierCollection& multiplierCollection); } // 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 f77a8efea..b9d488cf5 100644 --- 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 valueFunction_; diff --git a/ocs2_ipm/src/IpmHelpers.cpp b/ocs2_ipm/src/IpmHelpers.cpp index 3159d6f74..ce349c401 100644 --- 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& 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& time, const std::vector& 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(time.size()) - 1; @@ -181,18 +164,21 @@ DualSolution toDualSolution(const std::vector& 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 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 index 94986ee5b..97656ddea 100644 --- 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& 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& 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& } 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& } 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]; From 2a307e58c7a93ee7c5b14d936a1771d7992d096c Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Mon, 19 Dec 2022 17:59:45 +0100 Subject: [PATCH 14/21] fix CMake --- ocs2_ipm/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt index d5e27aae7..bd02a70cc 100644 --- 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_FIX + CF_WERROR ) endif(cmake_clang_tools_FOUND) From 68e864b18cdec95d68fdb8faec7dd00b0408b297 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Mon, 19 Dec 2022 22:23:28 +0100 Subject: [PATCH 15/21] refactor initialization --- ocs2_ipm/CMakeLists.txt | 1 + ocs2_ipm/include/ocs2_ipm/IpmHelpers.h | 2 +- ocs2_ipm/include/ocs2_ipm/IpmInitialization.h | 54 +++++++++++- ocs2_ipm/include/ocs2_ipm/IpmSolver.h | 4 +- ocs2_ipm/src/IpmHelpers.cpp | 2 +- ocs2_ipm/src/IpmInitialization.cpp | 86 +++++++++++++++++++ ocs2_ipm/src/IpmSolver.cpp | 85 +++++++----------- ocs2_ipm/test/Exp0Test.cpp | 2 +- 8 files changed, 174 insertions(+), 62 deletions(-) create mode 100644 ocs2_ipm/src/IpmInitialization.cpp 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 5eae47eb2..d9f27dac5 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h @@ -128,7 +128,7 @@ DualSolution toDualSolution(const std::vector& time, const std::v * @param multiplierCollection: The MultiplierCollection. * @return slack/dual variables of the state-only (first) and state-input constraints (second). */ -std::pair fromDualSolution(const MultiplierCollection& multiplierCollection); +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..9aade2d5a 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 variable. + */ +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. + */ +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. + */ +vector_t initializePreJumpSlackVariable(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 b9d488cf5..99f316f17 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h @@ -85,9 +85,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; diff --git a/ocs2_ipm/src/IpmHelpers.cpp b/ocs2_ipm/src/IpmHelpers.cpp index ce349c401..f7ebcc812 100644 --- a/ocs2_ipm/src/IpmHelpers.cpp +++ b/ocs2_ipm/src/IpmHelpers.cpp @@ -176,7 +176,7 @@ DualSolution toDualSolution(const std::vector& time, const std::v return dualSolution; } -std::pair fromDualSolution(const MultiplierCollection& multiplierCollection) { +std::pair fromMultiplierCollection(const MultiplierCollection& multiplierCollection) { return std::make_pair(extractLagrangian(multiplierCollection.stateIneq), extractLagrangian(multiplierCollection.stateInputIneq)); } diff --git a/ocs2_ipm/src/IpmInitialization.cpp b/ocs2_ipm/src/IpmInitialization.cpp new file mode 100644 index 000000000..139b40b44 --- /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->requestPreJump(request, time, state); + } + + 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 initializePreJumpSlackVariable(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 97656ddea..98949f28b 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -187,6 +187,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++++++++++++++++++++++++++++++++++++++++++++++++++++++"; @@ -407,24 +415,15 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector& if (timeDiscretization[i].event == AnnotatedTime::Event::PreEvent) { const auto cachedEventIndex = cacheEventIndexBias + eventIdx; 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]); + std::tie(slackStateIneq[i], std::ignore) = ipm::fromMultiplierCollection(slackIneqTrajectory_.preJumps[cachedEventIndex]); + std::tie(dualStateIneq[i], std::ignore) = ipm::fromMultiplierCollection(dualIneqTrajectory_.preJumps[cachedEventIndex]); } else { const auto time = getIntervalStart(timeDiscretization[i]); const auto& state = x[i]; - constexpr auto request = Request::Constraint; - ocpDefinition.preComputationPtr->requestPreJump(request, time, state); - auto& preComputation = *ocpDefinition.preComputationPtr; - if (!ocpDefinition.preJumpInequalityConstraintPtr->empty()) { - const auto ineqConstraint = toVector(ocpDefinition.preJumpInequalityConstraintPtr->getValue(time, state, preComputation)); - slackStateIneq[i] = - ipm::initializeSlackVariable(ineqConstraint, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); - dualStateIneq[i] = ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, - settings_.initialDualMarginRate); - } else { - slackStateIneq[i].resize(0); - dualStateIneq[i].resize(0); - } + slackStateIneq[i] = ipm::initializePreJumpSlackVariable(ocpDefinition, time, state, settings_.initialSlackLowerBound, + settings_.initialSlackMarginRate); + dualStateIneq[i] = + ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); } slackStateInputIneq[i].resize(0); dualStateInputIneq[i].resize(0); @@ -433,58 +432,36 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector& const auto time = getIntervalStart(timeDiscretization[i]); if (interpolatableTimePeriod.first <= time && time <= interpolatableTimePeriod.second) { std::tie(slackStateIneq[i], slackStateInputIneq[i]) = - ipm::fromDualSolution(getIntermediateDualSolutionAtTime(slackIneqTrajectory_, time)); + ipm::fromMultiplierCollection(getIntermediateDualSolutionAtTime(slackIneqTrajectory_, time)); std::tie(dualStateIneq[i], dualStateInputIneq[i]) = - ipm::fromDualSolution(getIntermediateDualSolutionAtTime(slackIneqTrajectory_, time)); + ipm::fromMultiplierCollection(getIntermediateDualSolutionAtTime(slackIneqTrajectory_, time)); } else { - constexpr auto request = Request::Constraint; const auto& state = x[i]; const auto& input = u[i]; - ocpDefinition.preComputationPtr->request(request, time, state, input); - if (!ocpDefinition.stateInequalityConstraintPtr->empty() && i > 0) { - const auto ineqConstraint = - toVector(ocpDefinition.stateInequalityConstraintPtr->getValue(time, state, *ocpDefinition.preComputationPtr)); - slackStateIneq[i] = - ipm::initializeSlackVariable(ineqConstraint, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); - dualStateIneq[i] = ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, - settings_.initialDualMarginRate); - } else { + std::tie(slackStateIneq[i], slackStateInputIneq[i]) = ipm::initializeIntermediateSlackVariable( + ocpDefinition, time, state, input, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); + // Disable the state-only inequality constraints at the initial node + if (i == 0) { slackStateIneq[i].resize(0); - dualStateIneq[i].resize(0); - } - if (!ocpDefinition.inequalityConstraintPtr->empty()) { - const auto ineqConstraint = - toVector(ocpDefinition.inequalityConstraintPtr->getValue(time, state, input, *ocpDefinition.preComputationPtr)); - slackStateInputIneq[i] = - ipm::initializeSlackVariable(ineqConstraint, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); - dualStateInputIneq[i] = ipm::initializeDualVariable(slackStateInputIneq[i], barrierParam, settings_.initialDualLowerBound, - settings_.initialDualMarginRate); - } else { - slackStateInputIneq[i].resize(0); - dualStateInputIneq[i].resize(0); } + dualStateIneq[i] = + ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); + dualStateInputIneq[i] = ipm::initializeDualVariable(slackStateInputIneq[i], barrierParam, settings_.initialDualLowerBound, + settings_.initialDualMarginRate); } } } if (interpolateTillFinalTime) { - std::tie(slackStateIneq[N], std::ignore) = ipm::fromDualSolution(slackIneqTrajectory_.final); - std::tie(dualStateIneq[N], std::ignore) = ipm::fromDualSolution(dualIneqTrajectory_.final); + std::tie(slackStateIneq[N], std::ignore) = ipm::fromMultiplierCollection(slackIneqTrajectory_.final); + std::tie(dualStateIneq[N], std::ignore) = ipm::fromMultiplierCollection(dualIneqTrajectory_.final); } else { - const auto time = newTimeTrajectory[N]; + const auto time = getIntervalStart(timeDiscretization[N]); const auto& state = x[N]; - constexpr auto request = Request::Constraint; - ocpDefinition.preComputationPtr->requestFinal(request, time, state); - auto& preComputation = *ocpDefinition.preComputationPtr; - if (!ocpDefinition.finalInequalityConstraintPtr->empty()) { - const auto ineqConstraint = toVector(ocpDefinition.finalInequalityConstraintPtr->getValue(time, state, preComputation)); - slackStateIneq[N] = ipm::initializeSlackVariable(ineqConstraint, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); - dualStateIneq[N] = - ipm::initializeDualVariable(slackStateIneq[N], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); - } else { - slackStateIneq[N].resize(0); - dualStateIneq[N].resize(0); - } + slackStateIneq[N] = ipm::initializeTerminalSlackVariable(ocpDefinition, time, state, settings_.initialSlackLowerBound, + settings_.initialSlackMarginRate); + dualStateIneq[N] = + ipm::initializeDualVariable(slackStateIneq[N], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); } } diff --git a/ocs2_ipm/test/Exp0Test.cpp b/ocs2_ipm/test/Exp0Test.cpp index ba7a8b25e..2864b59db 100644 --- a/ocs2_ipm/test/Exp0Test.cpp +++ b/ocs2_ipm/test/Exp0Test.cpp @@ -140,7 +140,7 @@ TEST(Exp0Test, Constrained) { const vector_t xmin = (vector_t(2) << -7.5, -7.5).finished(); const vector_t xmax = (vector_t(2) << 7.5, 7.5).finished(); problem.stateInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax)); - problem.finalInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax)); + // problem.finalInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax)); const scalar_t startTime = 0.0; const scalar_t finalTime = 2.0; From 72c0be8a72afc5a19572b8ee65565912e37d71d9 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Mon, 19 Dec 2022 22:51:31 +0100 Subject: [PATCH 16/21] update doc --- ocs2_ipm/include/ocs2_ipm/IpmInitialization.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h b/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h index 9aade2d5a..e18cf2e75 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h @@ -80,7 +80,7 @@ inline vector_t initializeDualVariable(const vector_t& slack, scalar_t barrierPa * @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 variable. + * @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, @@ -94,7 +94,7 @@ std::pair initializeIntermediateSlackVariable(OptimalControl * @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. + * @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); @@ -107,7 +107,7 @@ vector_t initializeTerminalSlackVariable(OptimalControlProblem& ocpDefinition, s * @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. + * @return Initialized slack variable of the pre-jump state-only constraints. */ vector_t initializePreJumpSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state, scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate); From 750f665142a428a46048a610bb577c554debe2e4 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Mon, 19 Dec 2022 22:54:35 +0100 Subject: [PATCH 17/21] add getDualSolution() implementation --- ocs2_ipm/include/ocs2_ipm/IpmSolver.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h index 99f316f17..56e7e282b 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h @@ -67,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_; } From 668ccaec4d2db740ebb2c443adab4f80ecd3ec9a Mon Sep 17 00:00:00 2001 From: Farbod Farshidian Date: Tue, 20 Dec 2022 13:00:44 +0100 Subject: [PATCH 18/21] doc update --- ocs2_doc/docs/intro.rst | 7 ++++--- ocs2_doc/docs/overview.rst | 40 +++++++++++++++++++++++++++++--------- 2 files changed, 35 insertions(+), 12 deletions(-) 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 ~~~~~~~~~~~~~~~~~~~~ From dfabca4f20b86a65073b9c2b0e30ede1d77facbb Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Tue, 20 Dec 2022 15:00:02 +0100 Subject: [PATCH 19/21] fix bugs and add initialization timer --- ocs2_ipm/src/IpmInitialization.cpp | 2 +- ocs2_ipm/src/IpmSolver.cpp | 33 +++++++++++++++--------------- ocs2_ipm/test/Exp0Test.cpp | 2 +- 3 files changed, 19 insertions(+), 18 deletions(-) diff --git a/ocs2_ipm/src/IpmInitialization.cpp b/ocs2_ipm/src/IpmInitialization.cpp index 139b40b44..d2a2a9ccd 100644 --- a/ocs2_ipm/src/IpmInitialization.cpp +++ b/ocs2_ipm/src/IpmInitialization.cpp @@ -39,7 +39,7 @@ std::pair initializeIntermediateSlackVariable(OptimalControl if (!ocpDefinition.stateInequalityConstraintPtr->empty() || !ocpDefinition.inequalityConstraintPtr->empty()) { constexpr auto request = Request::Constraint; - ocpDefinition.preComputationPtr->requestPreJump(request, time, state); + ocpDefinition.preComputationPtr->request(request, time, state, input); } if (!ocpDefinition.stateInequalityConstraintPtr->empty()) { diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 98949f28b..150a6543a 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -107,6 +107,7 @@ void IpmSolver::reset() { // reset timers totalNumIterations_ = 0; + initializationTimer_.reset(); linearQuadraticApproximationTimer_.reset(); solveQpTimer_.reset(); linesearchTimer_.reset(); @@ -114,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) { @@ -127,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(" @@ -216,6 +221,7 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f 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_); @@ -239,6 +245,7 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f initializeCostateTrajectory(timeDiscretization, x, lmd); initializeProjectionMultiplierTrajectory(timeDiscretization, nu); } + initializationTimer_.endTimer(); // Bookkeeping performanceIndeces_.clear(); @@ -418,10 +425,8 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector& std::tie(slackStateIneq[i], std::ignore) = ipm::fromMultiplierCollection(slackIneqTrajectory_.preJumps[cachedEventIndex]); std::tie(dualStateIneq[i], std::ignore) = ipm::fromMultiplierCollection(dualIneqTrajectory_.preJumps[cachedEventIndex]); } else { - const auto time = getIntervalStart(timeDiscretization[i]); - const auto& state = x[i]; - slackStateIneq[i] = ipm::initializePreJumpSlackVariable(ocpDefinition, time, state, settings_.initialSlackLowerBound, - settings_.initialSlackMarginRate); + slackStateIneq[i] = ipm::initializePreJumpSlackVariable(ocpDefinition, getIntervalStart(timeDiscretization[i]), x[i], + settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); dualStateIneq[i] = ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); } @@ -436,14 +441,8 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector& std::tie(dualStateIneq[i], dualStateInputIneq[i]) = ipm::fromMultiplierCollection(getIntermediateDualSolutionAtTime(slackIneqTrajectory_, time)); } else { - const auto& state = x[i]; - const auto& input = u[i]; std::tie(slackStateIneq[i], slackStateInputIneq[i]) = ipm::initializeIntermediateSlackVariable( - ocpDefinition, time, state, input, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); - // Disable the state-only inequality constraints at the initial node - if (i == 0) { - slackStateIneq[i].resize(0); - } + 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, @@ -452,14 +451,16 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector& } } + // 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 { - const auto time = getIntervalStart(timeDiscretization[N]); - const auto& state = x[N]; - slackStateIneq[N] = ipm::initializeTerminalSlackVariable(ocpDefinition, time, state, settings_.initialSlackLowerBound, - settings_.initialSlackMarginRate); + 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); } diff --git a/ocs2_ipm/test/Exp0Test.cpp b/ocs2_ipm/test/Exp0Test.cpp index 2864b59db..ba7a8b25e 100644 --- a/ocs2_ipm/test/Exp0Test.cpp +++ b/ocs2_ipm/test/Exp0Test.cpp @@ -140,7 +140,7 @@ TEST(Exp0Test, Constrained) { const vector_t xmin = (vector_t(2) << -7.5, -7.5).finished(); const vector_t xmax = (vector_t(2) << 7.5, 7.5).finished(); problem.stateInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax)); - // problem.finalInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax)); + problem.finalInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax)); const scalar_t startTime = 0.0; const scalar_t finalTime = 2.0; From f4174ec5cd7c4f30942168b2dca6aa88b985f3e5 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Tue, 20 Dec 2022 15:02:37 +0100 Subject: [PATCH 20/21] improve function name --- ocs2_ipm/include/ocs2_ipm/IpmInitialization.h | 4 ++-- ocs2_ipm/src/IpmInitialization.cpp | 4 ++-- ocs2_ipm/src/IpmSolver.cpp | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h b/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h index e18cf2e75..cdc50a4ec 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h @@ -109,8 +109,8 @@ vector_t initializeTerminalSlackVariable(OptimalControlProblem& ocpDefinition, s * @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 initializePreJumpSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state, - scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate); +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/src/IpmInitialization.cpp b/ocs2_ipm/src/IpmInitialization.cpp index d2a2a9ccd..33f8b7aa1 100644 --- a/ocs2_ipm/src/IpmInitialization.cpp +++ b/ocs2_ipm/src/IpmInitialization.cpp @@ -69,8 +69,8 @@ vector_t initializeTerminalSlackVariable(OptimalControlProblem& ocpDefinition, s return initializeSlackVariable(ineqConstraint, initialSlackLowerBound, initialSlackMarginRate); } -vector_t initializePreJumpSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state, - scalar_t initialSlackLowerBound, scalar_t 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(); } diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 150a6543a..9f156c4cb 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -425,8 +425,8 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector& std::tie(slackStateIneq[i], std::ignore) = ipm::fromMultiplierCollection(slackIneqTrajectory_.preJumps[cachedEventIndex]); std::tie(dualStateIneq[i], std::ignore) = ipm::fromMultiplierCollection(dualIneqTrajectory_.preJumps[cachedEventIndex]); } else { - slackStateIneq[i] = ipm::initializePreJumpSlackVariable(ocpDefinition, getIntervalStart(timeDiscretization[i]), x[i], - settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); + slackStateIneq[i] = ipm::initializeEventSlackVariable(ocpDefinition, getIntervalStart(timeDiscretization[i]), x[i], + settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); dualStateIneq[i] = ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); } From c2ce8398f70cc53febe9ac66dfe76415cc153fa3 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama Date: Tue, 20 Dec 2022 15:30:42 +0100 Subject: [PATCH 21/21] fix time at preJump node of initialization --- ocs2_ipm/src/IpmSolver.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 9f156c4cb..46ea304b7 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -425,7 +425,8 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector& std::tie(slackStateIneq[i], std::ignore) = ipm::fromMultiplierCollection(slackIneqTrajectory_.preJumps[cachedEventIndex]); std::tie(dualStateIneq[i], std::ignore) = ipm::fromMultiplierCollection(dualIneqTrajectory_.preJumps[cachedEventIndex]); } else { - slackStateIneq[i] = ipm::initializeEventSlackVariable(ocpDefinition, getIntervalStart(timeDiscretization[i]), x[i], + 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); @@ -434,7 +435,7 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector& dualStateInputIneq[i].resize(0); ++eventIdx; } else { - const auto time = getIntervalStart(timeDiscretization[i]); + 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));