Skip to content

Commit 34be3bf

Browse files
mayatakafarbod-farshidian
mayataka
authored and
farbod-farshidian
committed
Merged in dev/interior_point/lagrangian (pull request #681)
add Lagrangian evaluations and modify projection Approved-by: Farbod Farshidian
2 parents 1227b86 + f9aed2b commit 34be3bf

File tree

19 files changed

+337
-86
lines changed

19 files changed

+337
-86
lines changed

‎ocs2_oc/CMakeLists.txt

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -51,7 +51,9 @@ add_library(${PROJECT_NAME}
5151
src/approximate_model/LinearQuadraticApproximator.cpp
5252
src/multiple_shooting/Helpers.cpp
5353
src/multiple_shooting/Initialization.cpp
54+
src/multiple_shooting/LagrangianEvaluation.cpp
5455
src/multiple_shooting/PerformanceIndexComputation.cpp
56+
src/multiple_shooting/ProjectionMultiplierCoefficients.cpp
5557
src/multiple_shooting/Transcription.cpp
5658
src/oc_data/LoopshapingPrimalSolution.cpp
5759
src/oc_data/TimeDiscretization.cpp
@@ -128,7 +130,7 @@ install(DIRECTORY test/include/${PROJECT_NAME}/
128130
## $ catkin_test_results ../../../build/ocs2_oc
129131

130132
catkin_add_gtest(test_${PROJECT_NAME}_multiple_shooting
131-
test/multiple_shooting/testHelpers.cpp
133+
test/multiple_shooting/testProjectionMultiplierCoefficients.cpp
132134
test/multiple_shooting/testTranscriptionPerformanceIndex.cpp
133135
)
134136
add_dependencies(test_${PROJECT_NAME}_multiple_shooting

‎ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h

Lines changed: 0 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -101,27 +101,5 @@ PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSche
101101
PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSchedule&& modeSchedule, vector_array_t&& x, vector_array_t&& u,
102102
matrix_array_t&& KMatrices);
103103

104-
/**
105-
* Coefficients to compute the Newton step of the Lagrange multiplier associated with the state-input equality constraint such that
106-
* dfdx*dx + dfdu*du + dfdcostate*dcostate + f
107-
*/
108-
struct ProjectionMultiplierCoefficients {
109-
matrix_t dfdx;
110-
matrix_t dfdu;
111-
matrix_t dfdcostate;
112-
vector_t f;
113-
114-
/**
115-
* Extracts the coefficients of the Lagrange multiplier associated with the state-input equality constraint.
116-
*
117-
* @param dynamics : Dynamics
118-
* @param cost : Cost
119-
* @param constraintProjection : Constraint projection.
120-
* @param pseudoInverse : Left pseudo-inverse of D^T of the state-input equality constraint.
121-
*/
122-
void compute(const VectorFunctionLinearApproximation& dynamics, const ScalarFunctionQuadraticApproximation& cost,
123-
const VectorFunctionLinearApproximation& constraintProjection, const matrix_t& pseudoInverse);
124-
};
125-
126104
} // namespace multiple_shooting
127105
} // namespace ocs2

‎ocs2_oc/include/ocs2_oc/multiple_shooting/LagrangianEvaluation.h

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+
#pragma once
31+
32+
#include <ocs2_core/Types.h>
33+
34+
namespace ocs2 {
35+
namespace multiple_shooting {
36+
37+
/**
38+
* Computes the SSE of the residual in the dual feasibilities.
39+
*
40+
* @param lagrangian : Quadratic approximation of the Lagrangian for a single node.
41+
* @return SSE of the residual in the dual feasibilities
42+
*/
43+
inline scalar_t evaluateDualFeasibilities(const ScalarFunctionQuadraticApproximation& lagrangian) {
44+
return lagrangian.dfdx.squaredNorm() + lagrangian.dfdu.squaredNorm();
45+
}
46+
47+
/**
48+
* Evaluates the quadratic approximation of the Lagrangian for a single intermediate node.
49+
*
50+
* @param lmd : Costate at start of the interval
51+
* @param lmd_next : Costate at the end of the interval
52+
* @param nu : Lagrange multiplier of the projection constraint.
53+
* @param cost : Quadratic approximation of the cost.
54+
* @param dynamics : Linear approximation of the dynamics
55+
* @param stateInputEqConstraints : Linear approximation of the state-input equality constraints
56+
* @return Quadratic approximation of the Lagrangian.
57+
*/
58+
ScalarFunctionQuadraticApproximation evaluateLagrangianIntermediateNode(const vector_t& lmd, const vector_t& lmd_next, const vector_t& nu,
59+
ScalarFunctionQuadraticApproximation&& cost,
60+
const VectorFunctionLinearApproximation& dynamics,
61+
const VectorFunctionLinearApproximation& stateInputEqConstraints);
62+
63+
/**
64+
* Evaluates the quadratic approximation of the Lagrangian for the terminal node.
65+
*
66+
* @param lmd : Costate at start of the interval
67+
* @param cost : Quadratic approximation of the cost.
68+
* @return Quadratic approximation of the Lagrangian.
69+
*/
70+
ScalarFunctionQuadraticApproximation evaluateLagrangianTerminalNode(const vector_t& lmd, ScalarFunctionQuadraticApproximation&& cost);
71+
72+
/**
73+
* Evaluates the quadratic approximation of the Lagrangian for the event node.
74+
*
75+
* @param lmd : Costate at start of the interval
76+
* @param lmd_next : Costate at the end of the the interval
77+
* @param cost : Quadratic approximation of the cost.
78+
* @param dynamics : Dynamics
79+
* @return Quadratic approximation of the Lagrangian.
80+
*/
81+
ScalarFunctionQuadraticApproximation evaluateLagrangianEventNode(const vector_t& lmd, const vector_t& lmd_next,
82+
ScalarFunctionQuadraticApproximation&& cost,
83+
const VectorFunctionLinearApproximation& dynamics);
84+
85+
} // namespace multiple_shooting
86+
} // namespace ocs2

‎ocs2_oc/include/ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h

Lines changed: 60 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
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+
#pragma once
31+
32+
#include <ocs2_core/Types.h>
33+
34+
namespace ocs2 {
35+
namespace multiple_shooting {
36+
37+
/**
38+
* Coefficients to compute the Newton step of the Lagrange multiplier associated with the state-input equality constraint such that
39+
* dfdx * dx + dfdu * du + dfdcostate * dcostate + f
40+
*/
41+
struct ProjectionMultiplierCoefficients {
42+
matrix_t dfdx;
43+
matrix_t dfdu;
44+
matrix_t dfdcostate;
45+
vector_t f;
46+
47+
/**
48+
* Computes the coefficients of the Lagrange multiplier associated with the state-input equality constraint.
49+
*
50+
* @param cost : The cost quadratic approximation.
51+
* @param dynamics : The dynamics linear approximation.
52+
* @param constraintProjection : The constraint projection.
53+
* @param pseudoInverse : Left pseudo-inverse of D^T of the state-input linearized equality constraint (C dx + D du + e = 0).
54+
*/
55+
void compute(const ScalarFunctionQuadraticApproximation& cost, const VectorFunctionLinearApproximation& dynamics,
56+
const VectorFunctionLinearApproximation& constraintProjection, const matrix_t& pseudoInverse);
57+
};
58+
59+
} // namespace multiple_shooting
60+
} // namespace ocs2

‎ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
3232
#include <ocs2_core/Types.h>
3333
#include <ocs2_core/integration/SensitivityIntegrator.h>
3434

35+
#include "ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h"
3536
#include "ocs2_oc/oc_problem/OptimalControlProblem.h"
3637

3738
namespace ocs2 {
@@ -41,13 +42,13 @@ namespace multiple_shooting {
4142
* Results of the transcription at an intermediate node
4243
*/
4344
struct Transcription {
44-
VectorFunctionLinearApproximation dynamics;
4545
ScalarFunctionQuadraticApproximation cost;
46-
matrix_t constraintPseudoInverse;
47-
VectorFunctionLinearApproximation constraintsProjection;
46+
VectorFunctionLinearApproximation dynamics;
4847
VectorFunctionLinearApproximation stateInputEqConstraints;
4948
VectorFunctionLinearApproximation stateIneqConstraints;
5049
VectorFunctionLinearApproximation stateInputIneqConstraints;
50+
VectorFunctionLinearApproximation constraintsProjection;
51+
ProjectionMultiplierCoefficients projectionMultiplierCoefficients;
5152
};
5253

5354
/**
@@ -70,9 +71,9 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP
7071
* Apply the state-input equality constraint projection for a single intermediate node transcription.
7172
*
7273
* @param transcription : Transcription for a single intermediate node
73-
* @param extractEqualityConstraintsPseudoInverse
74+
* @param extractProjectionMultiplier : Whether to extract the projection multiplier.
7475
*/
75-
void projectTranscription(Transcription& transcription, bool extractEqualityConstraintsPseudoInverse = false);
76+
void projectTranscription(Transcription& transcription, bool extractProjectionMultiplier = false);
7677

7778
/**
7879
* Results of the transcription at a terminal node
@@ -97,8 +98,8 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont
9798
* Results of the transcription at an event
9899
*/
99100
struct EventTranscription {
100-
VectorFunctionLinearApproximation dynamics;
101101
ScalarFunctionQuadraticApproximation cost;
102+
VectorFunctionLinearApproximation dynamics;
102103
VectorFunctionLinearApproximation eqConstraints;
103104
VectorFunctionLinearApproximation ineqConstraints;
104105
};

‎ocs2_oc/src/lintTarget.cpp

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
3434
// multiple_shooting
3535
#include <ocs2_oc/multiple_shooting/Helpers.h>
3636
#include <ocs2_oc/multiple_shooting/Initialization.h>
37+
#include <ocs2_oc/multiple_shooting/LagrangianEvaluation.h>
3738
#include <ocs2_oc/multiple_shooting/PerformanceIndexComputation.h>
39+
#include <ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h>
3840
#include <ocs2_oc/multiple_shooting/Transcription.h>
3941

4042
// oc_data
@@ -77,6 +79,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
7779

7880
// trajectory_adjustment
7981
#include <ocs2_oc/trajectory_adjustment/TrajectorySpreading.h>
82+
#include <ocs2_oc/trajectory_adjustment/TrajectorySpreadingHelperFunctions.h>
8083

8184
// dummy target for clang toolchain
8285
int main() {

‎ocs2_oc/src/multiple_shooting/Helpers.cpp

Lines changed: 0 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -119,23 +119,5 @@ PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSche
119119
return primalSolution;
120120
}
121121

122-
void ProjectionMultiplierCoefficients::compute(const VectorFunctionLinearApproximation& dynamics,
123-
const ScalarFunctionQuadraticApproximation& cost,
124-
const VectorFunctionLinearApproximation& constraintProjection,
125-
const matrix_t& pseudoInverse) {
126-
vector_t semiprojectedCost_dfdu = cost.dfdu;
127-
semiprojectedCost_dfdu.noalias() += cost.dfduu * constraintProjection.f;
128-
129-
matrix_t semiprojectedCost_dfdux = cost.dfdux;
130-
semiprojectedCost_dfdux.noalias() += cost.dfduu * constraintProjection.dfdx;
131-
132-
const matrix_t semiprojectedCost_dfduu = cost.dfduu * constraintProjection.dfdu;
133-
134-
this->dfdx.noalias() = -pseudoInverse * semiprojectedCost_dfdux;
135-
this->dfdu.noalias() = -pseudoInverse * semiprojectedCost_dfduu;
136-
this->dfdcostate.noalias() = -pseudoInverse * dynamics.dfdu.transpose();
137-
this->f.noalias() = -pseudoInverse * semiprojectedCost_dfdu;
138-
}
139-
140122
} // namespace multiple_shooting
141123
} // namespace ocs2

‎ocs2_oc/src/multiple_shooting/LagrangianEvaluation.cpp

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
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_oc/multiple_shooting/LagrangianEvaluation.h"
31+
32+
namespace ocs2 {
33+
namespace multiple_shooting {
34+
35+
ScalarFunctionQuadraticApproximation evaluateLagrangianIntermediateNode(const vector_t& lmd, const vector_t& lmd_next, const vector_t& nu,
36+
ScalarFunctionQuadraticApproximation&& cost,
37+
const VectorFunctionLinearApproximation& dynamics,
38+
const VectorFunctionLinearApproximation& stateInputEqConstraints) {
39+
ScalarFunctionQuadraticApproximation lagrangian = std::move(cost);
40+
lagrangian.dfdx.noalias() += dynamics.dfdx.transpose() * lmd_next;
41+
lagrangian.dfdx.noalias() -= lmd;
42+
lagrangian.dfdu.noalias() += dynamics.dfdu.transpose() * lmd_next;
43+
if (stateInputEqConstraints.f.size() > 0) {
44+
lagrangian.dfdx.noalias() += stateInputEqConstraints.dfdx.transpose() * nu;
45+
lagrangian.dfdu.noalias() += stateInputEqConstraints.dfdu.transpose() * nu;
46+
}
47+
return lagrangian;
48+
}
49+
50+
ScalarFunctionQuadraticApproximation evaluateLagrangianTerminalNode(const vector_t& lmd, ScalarFunctionQuadraticApproximation&& cost) {
51+
ScalarFunctionQuadraticApproximation lagrangian = std::move(cost);
52+
lagrangian.dfdx.noalias() -= lmd;
53+
return lagrangian;
54+
}
55+
56+
ScalarFunctionQuadraticApproximation evaluateLagrangianEventNode(const vector_t& lmd, const vector_t& lmd_next,
57+
ScalarFunctionQuadraticApproximation&& cost,
58+
const VectorFunctionLinearApproximation& dynamics) {
59+
ScalarFunctionQuadraticApproximation lagrangian = std::move(cost);
60+
lagrangian.dfdx.noalias() += dynamics.dfdx.transpose() * lmd_next;
61+
lagrangian.dfdx.noalias() -= lmd;
62+
return lagrangian;
63+
}
64+
65+
} // namespace multiple_shooting
66+
} // namespace ocs2

0 commit comments

Comments
 (0)