Skip to content

Commit 68e864b

Browse files
mayatakamayataka
committed
refactor initialization
1 parent 2a307e5 commit 68e864b

File tree

8 files changed

+174
-62
lines changed

8 files changed

+174
-62
lines changed

‎ocs2_ipm/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -50,6 +50,7 @@ include_directories(
5050
# Multiple shooting solver library
5151
add_library(${PROJECT_NAME}
5252
src/IpmHelpers.cpp
53+
src/IpmInitialization.cpp
5354
src/IpmPerformanceIndexComputation.cpp
5455
src/IpmSettings.cpp
5556
src/IpmSolver.cpp

‎ocs2_ipm/include/ocs2_ipm/IpmHelpers.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -128,7 +128,7 @@ DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, const std::v
128128
* @param multiplierCollection: The MultiplierCollection.
129129
* @return slack/dual variables of the state-only (first) and state-input constraints (second).
130130
*/
131-
std::pair<vector_t, vector_t> fromDualSolution(const MultiplierCollection& multiplierCollection);
131+
std::pair<vector_t, vector_t> fromMultiplierCollection(const MultiplierCollection& multiplierCollection);
132132

133133
} // namespace ipm
134134
} // namespace ocs2

‎ocs2_ipm/include/ocs2_ipm/IpmInitialization.h‎

Lines changed: 52 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
3030
#pragma once
3131

3232
#include <ocs2_core/Types.h>
33+
#include <ocs2_oc/oc_problem/OptimalControlProblem.h>
3334

3435
namespace ocs2 {
3536
namespace ipm {
@@ -44,7 +45,11 @@ namespace ipm {
4445
* @return Initialized slack variable.
4546
*/
4647
inline vector_t initializeSlackVariable(const vector_t& ineqConstraint, scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate) {
47-
return (1.0 + initialSlackMarginRate) * ineqConstraint.cwiseMax(initialSlackLowerBound);
48+
if (ineqConstraint.size() > 0) {
49+
return (1.0 + initialSlackMarginRate) * ineqConstraint.cwiseMax(initialSlackLowerBound);
50+
} else {
51+
return vector_t();
52+
}
4853
}
4954

5055
/**
@@ -59,8 +64,53 @@ inline vector_t initializeSlackVariable(const vector_t& ineqConstraint, scalar_t
5964
*/
6065
inline vector_t initializeDualVariable(const vector_t& slack, scalar_t barrierParam, scalar_t initialDualLowerBound,
6166
scalar_t initialDualMarginRate) {
62-
return (1.0 + initialDualMarginRate) * (barrierParam * slack.cwiseInverse()).cwiseMax(initialDualLowerBound);
67+
if (slack.size() > 0) {
68+
return (1.0 + initialDualMarginRate) * (barrierParam * slack.cwiseInverse()).cwiseMax(initialDualLowerBound);
69+
} else {
70+
return vector_t();
71+
}
6372
}
6473

74+
/**
75+
* Initializes the slack variable at a single intermediate node.
76+
*
77+
* @param ocpDefinition: Definition of the optimal control problem.
78+
* @param time : Time of this node
79+
* @param state : State
80+
* @param input : Input
81+
* @param initialSlackLowerBound : Lower bound of the initial slack variables. Corresponds to `slack_bound_push` option of IPOPT.
82+
* @param initialSlackMarginRate : Additional margin rate of the initial slack variables. Corresponds to `slack_bound_frac` option of IPOPT.
83+
* @return Initialized slack variable.
84+
*/
85+
std::pair<vector_t, vector_t> initializeIntermediateSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time,
86+
const vector_t& state, const vector_t& input,
87+
scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate);
88+
89+
/**
90+
* Initializes the slack variable at the terminal node.
91+
*
92+
* @param ocpDefinition: Definition of the optimal control problem.
93+
* @param time : Time at the terminal node
94+
* @param state : Terminal state
95+
* @param initialSlackLowerBound : Lower bound of the initial slack variables. Corresponds to `slack_bound_push` option of IPOPT.
96+
* @param initialSlackMarginRate : Additional margin rate of the initial slack variables. Corresponds to `slack_bound_frac` option of IPOPT.
97+
* @return Initialized slack variable.
98+
*/
99+
vector_t initializeTerminalSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state,
100+
scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate);
101+
102+
/**
103+
* Initializes the slack variable at an event node.
104+
*
105+
* @param ocpDefinition: Definition of the optimal control problem.
106+
* @param time : Time at the event node
107+
* @param state : Pre-event state
108+
* @param initialSlackLowerBound : Lower bound of the initial slack variables. Corresponds to `slack_bound_push` option of IPOPT.
109+
* @param initialSlackMarginRate : Additional margin rate of the initial slack variables. Corresponds to `slack_bound_frac` option of IPOPT.
110+
* @return Initialized slack variable.
111+
*/
112+
vector_t initializePreJumpSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state,
113+
scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate);
114+
65115
} // namespace ipm
66116
} // namespace ocs2

‎ocs2_ipm/include/ocs2_ipm/IpmSolver.h‎

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -85,9 +85,7 @@ class IpmSolver : public SolverBase {
8585

8686
vector_t getStateInputEqualityConstraintLagrangian(scalar_t time, const vector_t& state) const override;
8787

88-
MultiplierCollection getIntermediateDualSolution(scalar_t time) const override {
89-
throw std::runtime_error("[IpmSolver] getIntermediateDualSolution() not available yet.");
90-
}
88+
MultiplierCollection getIntermediateDualSolution(scalar_t time) const override;
9189

9290
private:
9391
void runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) override;

‎ocs2_ipm/src/IpmHelpers.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -176,7 +176,7 @@ DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, const std::v
176176
return dualSolution;
177177
}
178178

179-
std::pair<vector_t, vector_t> fromDualSolution(const MultiplierCollection& multiplierCollection) {
179+
std::pair<vector_t, vector_t> fromMultiplierCollection(const MultiplierCollection& multiplierCollection) {
180180
return std::make_pair(extractLagrangian(multiplierCollection.stateIneq), extractLagrangian(multiplierCollection.stateInputIneq));
181181
}
182182

‎ocs2_ipm/src/IpmInitialization.cpp‎

Lines changed: 86 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,86 @@
1+
/******************************************************************************
2+
Copyright (c) 2020, Farbod Farshidian. All rights reserved.
3+
4+
Redistribution and use in source and binary forms, with or without
5+
modification, are permitted provided that the following conditions are met:
6+
7+
* Redistributions of source code must retain the above copyright notice, this
8+
list of conditions and the following disclaimer.
9+
10+
* Redistributions in binary form must reproduce the above copyright notice,
11+
this list of conditions and the following disclaimer in the documentation
12+
and/or other materials provided with the distribution.
13+
14+
* Neither the name of the copyright holder nor the names of its
15+
contributors may be used to endorse or promote products derived from
16+
this software without specific prior written permission.
17+
18+
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19+
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20+
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21+
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
22+
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23+
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24+
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25+
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26+
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27+
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28+
******************************************************************************/
29+
30+
#include "ocs2_ipm/IpmInitialization.h"
31+
32+
namespace ocs2 {
33+
namespace ipm {
34+
35+
std::pair<vector_t, vector_t> initializeIntermediateSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time,
36+
const vector_t& state, const vector_t& input,
37+
scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate) {
38+
vector_t slackStateIneq, slackStateInputIneq;
39+
40+
if (!ocpDefinition.stateInequalityConstraintPtr->empty() || !ocpDefinition.inequalityConstraintPtr->empty()) {
41+
constexpr auto request = Request::Constraint;
42+
ocpDefinition.preComputationPtr->requestPreJump(request, time, state);
43+
}
44+
45+
if (!ocpDefinition.stateInequalityConstraintPtr->empty()) {
46+
const auto ineqConstraint =
47+
toVector(ocpDefinition.stateInequalityConstraintPtr->getValue(time, state, *ocpDefinition.preComputationPtr));
48+
slackStateIneq = initializeSlackVariable(ineqConstraint, initialSlackLowerBound, initialSlackMarginRate);
49+
}
50+
51+
if (!ocpDefinition.inequalityConstraintPtr->empty()) {
52+
const auto ineqConstraint =
53+
toVector(ocpDefinition.inequalityConstraintPtr->getValue(time, state, input, *ocpDefinition.preComputationPtr));
54+
slackStateInputIneq = initializeSlackVariable(ineqConstraint, initialSlackLowerBound, initialSlackMarginRate);
55+
}
56+
57+
return std::make_pair(std::move(slackStateIneq), std::move(slackStateInputIneq));
58+
}
59+
60+
vector_t initializeTerminalSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state,
61+
scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate) {
62+
if (ocpDefinition.finalInequalityConstraintPtr->empty()) {
63+
return vector_t();
64+
}
65+
66+
constexpr auto request = Request::Constraint;
67+
ocpDefinition.preComputationPtr->requestFinal(request, time, state);
68+
const auto ineqConstraint = toVector(ocpDefinition.finalInequalityConstraintPtr->getValue(time, state, *ocpDefinition.preComputationPtr));
69+
return initializeSlackVariable(ineqConstraint, initialSlackLowerBound, initialSlackMarginRate);
70+
}
71+
72+
vector_t initializePreJumpSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state,
73+
scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate) {
74+
if (ocpDefinition.preJumpInequalityConstraintPtr->empty()) {
75+
return vector_t();
76+
}
77+
78+
constexpr auto request = Request::Constraint;
79+
ocpDefinition.preComputationPtr->requestPreJump(request, time, state);
80+
const auto ineqConstraint =
81+
toVector(ocpDefinition.preJumpInequalityConstraintPtr->getValue(time, state, *ocpDefinition.preComputationPtr));
82+
return initializeSlackVariable(ineqConstraint, initialSlackLowerBound, initialSlackMarginRate);
83+
}
84+
85+
} // namespace ipm
86+
} // namespace ocs2

‎ocs2_ipm/src/IpmSolver.cpp‎

Lines changed: 31 additions & 54 deletions
Original file line numberDiff line numberDiff line change
@@ -187,6 +187,14 @@ vector_t IpmSolver::getStateInputEqualityConstraintLagrangian(scalar_t time, con
187187
}
188188
}
189189

190+
MultiplierCollection IpmSolver::getIntermediateDualSolution(scalar_t time) const {
191+
if (!dualIneqTrajectory_.timeTrajectory.empty()) {
192+
return getIntermediateDualSolutionAtTime(dualIneqTrajectory_, time);
193+
} else {
194+
throw std::runtime_error("[IpmSolver] getIntermediateDualSolution() not available yet.");
195+
}
196+
}
197+
190198
void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) {
191199
if (settings_.printSolverStatus || settings_.printLinesearch) {
192200
std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++";
@@ -407,24 +415,15 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector<AnnotatedTime>&
407415
if (timeDiscretization[i].event == AnnotatedTime::Event::PreEvent) {
408416
const auto cachedEventIndex = cacheEventIndexBias + eventIdx;
409417
if (cachedEventIndex < slackIneqTrajectory_.preJumps.size()) {
410-
std::tie(slackStateIneq[i], std::ignore) = ipm::fromDualSolution(slackIneqTrajectory_.preJumps[cachedEventIndex]);
411-
std::tie(dualStateIneq[i], std::ignore) = ipm::fromDualSolution(dualIneqTrajectory_.preJumps[cachedEventIndex]);
418+
std::tie(slackStateIneq[i], std::ignore) = ipm::fromMultiplierCollection(slackIneqTrajectory_.preJumps[cachedEventIndex]);
419+
std::tie(dualStateIneq[i], std::ignore) = ipm::fromMultiplierCollection(dualIneqTrajectory_.preJumps[cachedEventIndex]);
412420
} else {
413421
const auto time = getIntervalStart(timeDiscretization[i]);
414422
const auto& state = x[i];
415-
constexpr auto request = Request::Constraint;
416-
ocpDefinition.preComputationPtr->requestPreJump(request, time, state);
417-
auto& preComputation = *ocpDefinition.preComputationPtr;
418-
if (!ocpDefinition.preJumpInequalityConstraintPtr->empty()) {
419-
const auto ineqConstraint = toVector(ocpDefinition.preJumpInequalityConstraintPtr->getValue(time, state, preComputation));
420-
slackStateIneq[i] =
421-
ipm::initializeSlackVariable(ineqConstraint, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate);
422-
dualStateIneq[i] = ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound,
423-
settings_.initialDualMarginRate);
424-
} else {
425-
slackStateIneq[i].resize(0);
426-
dualStateIneq[i].resize(0);
427-
}
423+
slackStateIneq[i] = ipm::initializePreJumpSlackVariable(ocpDefinition, time, state, settings_.initialSlackLowerBound,
424+
settings_.initialSlackMarginRate);
425+
dualStateIneq[i] =
426+
ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate);
428427
}
429428
slackStateInputIneq[i].resize(0);
430429
dualStateInputIneq[i].resize(0);
@@ -433,58 +432,36 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector<AnnotatedTime>&
433432
const auto time = getIntervalStart(timeDiscretization[i]);
434433
if (interpolatableTimePeriod.first <= time && time <= interpolatableTimePeriod.second) {
435434
std::tie(slackStateIneq[i], slackStateInputIneq[i]) =
436-
ipm::fromDualSolution(getIntermediateDualSolutionAtTime(slackIneqTrajectory_, time));
435+
ipm::fromMultiplierCollection(getIntermediateDualSolutionAtTime(slackIneqTrajectory_, time));
437436
std::tie(dualStateIneq[i], dualStateInputIneq[i]) =
438-
ipm::fromDualSolution(getIntermediateDualSolutionAtTime(slackIneqTrajectory_, time));
437+
ipm::fromMultiplierCollection(getIntermediateDualSolutionAtTime(slackIneqTrajectory_, time));
439438
} else {
440-
constexpr auto request = Request::Constraint;
441439
const auto& state = x[i];
442440
const auto& input = u[i];
443-
ocpDefinition.preComputationPtr->request(request, time, state, input);
444-
if (!ocpDefinition.stateInequalityConstraintPtr->empty() && i > 0) {
445-
const auto ineqConstraint =
446-
toVector(ocpDefinition.stateInequalityConstraintPtr->getValue(time, state, *ocpDefinition.preComputationPtr));
447-
slackStateIneq[i] =
448-
ipm::initializeSlackVariable(ineqConstraint, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate);
449-
dualStateIneq[i] = ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound,
450-
settings_.initialDualMarginRate);
451-
} else {
441+
std::tie(slackStateIneq[i], slackStateInputIneq[i]) = ipm::initializeIntermediateSlackVariable(
442+
ocpDefinition, time, state, input, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate);
443+
// Disable the state-only inequality constraints at the initial node
444+
if (i == 0) {
452445
slackStateIneq[i].resize(0);
453-
dualStateIneq[i].resize(0);
454-
}
455-
if (!ocpDefinition.inequalityConstraintPtr->empty()) {
456-
const auto ineqConstraint =
457-
toVector(ocpDefinition.inequalityConstraintPtr->getValue(time, state, input, *ocpDefinition.preComputationPtr));
458-
slackStateInputIneq[i] =
459-
ipm::initializeSlackVariable(ineqConstraint, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate);
460-
dualStateInputIneq[i] = ipm::initializeDualVariable(slackStateInputIneq[i], barrierParam, settings_.initialDualLowerBound,
461-
settings_.initialDualMarginRate);
462-
} else {
463-
slackStateInputIneq[i].resize(0);
464-
dualStateInputIneq[i].resize(0);
465446
}
447+
dualStateIneq[i] =
448+
ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate);
449+
dualStateInputIneq[i] = ipm::initializeDualVariable(slackStateInputIneq[i], barrierParam, settings_.initialDualLowerBound,
450+
settings_.initialDualMarginRate);
466451
}
467452
}
468453
}
469454

470455
if (interpolateTillFinalTime) {
471-
std::tie(slackStateIneq[N], std::ignore) = ipm::fromDualSolution(slackIneqTrajectory_.final);
472-
std::tie(dualStateIneq[N], std::ignore) = ipm::fromDualSolution(dualIneqTrajectory_.final);
456+
std::tie(slackStateIneq[N], std::ignore) = ipm::fromMultiplierCollection(slackIneqTrajectory_.final);
457+
std::tie(dualStateIneq[N], std::ignore) = ipm::fromMultiplierCollection(dualIneqTrajectory_.final);
473458
} else {
474-
const auto time = newTimeTrajectory[N];
459+
const auto time = getIntervalStart(timeDiscretization[N]);
475460
const auto& state = x[N];
476-
constexpr auto request = Request::Constraint;
477-
ocpDefinition.preComputationPtr->requestFinal(request, time, state);
478-
auto& preComputation = *ocpDefinition.preComputationPtr;
479-
if (!ocpDefinition.finalInequalityConstraintPtr->empty()) {
480-
const auto ineqConstraint = toVector(ocpDefinition.finalInequalityConstraintPtr->getValue(time, state, preComputation));
481-
slackStateIneq[N] = ipm::initializeSlackVariable(ineqConstraint, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate);
482-
dualStateIneq[N] =
483-
ipm::initializeDualVariable(slackStateIneq[N], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate);
484-
} else {
485-
slackStateIneq[N].resize(0);
486-
dualStateIneq[N].resize(0);
487-
}
461+
slackStateIneq[N] = ipm::initializeTerminalSlackVariable(ocpDefinition, time, state, settings_.initialSlackLowerBound,
462+
settings_.initialSlackMarginRate);
463+
dualStateIneq[N] =
464+
ipm::initializeDualVariable(slackStateIneq[N], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate);
488465
}
489466
}
490467

‎ocs2_ipm/test/Exp0Test.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -140,7 +140,7 @@ TEST(Exp0Test, Constrained) {
140140
const vector_t xmin = (vector_t(2) << -7.5, -7.5).finished();
141141
const vector_t xmax = (vector_t(2) << 7.5, 7.5).finished();
142142
problem.stateInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax));
143-
problem.finalInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax));
143+
// problem.finalInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax));
144144

145145
const scalar_t startTime = 0.0;
146146
const scalar_t finalTime = 2.0;

0 commit comments

Comments
 (0)