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
0 commit comments