aligator  0.15.0
A primal-dual augmented Lagrangian-type solver for nonlinear trajectory optimization.
Loading...
Searching...
No Matches
dense-kernel.hpp
Go to the documentation of this file.
1
3#pragma once
4
6
7#include "blk-matrix.hpp"
8#include "lqr-problem.hpp"
10
11namespace aligator::gar {
12
14template <typename _Scalar> struct DenseKernel {
15 using Scalar = _Scalar;
18
19 struct Data {
23
29
30 Data(uint nx, uint nu, uint nc, uint nx2, uint nth)
31 : kktMat({nu, nc, nx2, nx2}, {nu, nc, nx2, nx2})
32 , fb({nu, nc, nx2, nx2}, {nx})
33 , ft({nu, nc, nx2, nx2}, {nth})
34 , ff({nu, nc, nx2, nx2})
35 , ldl{nu + nc + 2 * nx2} {
36 setZero();
37 }
38
39 void setZero() {
40 kktMat.setZero();
41 fb.setZero();
42 ft.setZero();
43 ff.setZero();
44 }
45 };
46
47 // simple struct, can be copied
48 struct value {
49 MatrixRef Pxx;
50 MatrixRef Pxt;
51 MatrixRef Ptt;
52 VectorRef px;
53 VectorRef pt;
54 };
55
56 inline static void terminalSolve(const_view_t knot, Data &d, value v,
57 Scalar mueq) {
58 d.kktMat.setZero();
59
60 // assemble last-stage kkt matrix - includes input 'u'
61 d.kktMat(0, 0) = knot.R;
62 d.kktMat(0, 1) = knot.D.transpose();
63 d.kktMat(1, 0) = knot.D;
64 d.kktMat(1, 1).diagonal().array() = -mueq;
65
66 VectorRef kff = d.ff[0] = -knot.r;
67 VectorRef zff = d.ff[1] = -knot.d;
68 RowMatrixRef K = d.fb.blockRow(0) = -knot.S.transpose();
69 RowMatrixRef Z = d.fb.blockRow(1) = -knot.C;
70
71 d.ldl.compute(d.kktMat.matrix());
72 d.ldl.solveInPlace(d.ff.matrix());
73 d.ldl.solveInPlace(d.fb.matrix());
74
75 RowMatrixRef Kth = d.ft.blockRow(0) = -knot.Gu;
76 RowMatrixRef Zth = d.ft.blockRow(1) = -knot.Gv;
77 d.ldl.solveInPlace(d.ft.matrix());
78
79 Eigen::Transpose Ct = knot.C.transpose();
80
81 v.Pxx.noalias() = knot.Q + knot.S * K;
82 v.Pxx.noalias() += Ct * Z;
83
84 v.Pxt.noalias() = knot.Gx + K.transpose() * knot.Gu;
85 v.Pxt.noalias() += Z.transpose() * knot.Gv;
86
87 v.Ptt.noalias() = knot.Gth + knot.Gu.transpose() * Kth;
88 v.Ptt.noalias() += knot.Gv.transpose() * Zth;
89
90 v.px.noalias() = knot.q + knot.S * kff;
91 v.px.noalias() += Ct * zff;
92
93 v.pt.noalias() = knot.gamma + knot.Gu.transpose() * kff;
94 v.pt.noalias() += knot.Gv.transpose() * zff;
95 }
96
97 inline static void stageKernelSolve(const_view_t knot, Data &d, value v,
98 const value *vn, Scalar mudyn,
99 Scalar mueq) {
100 d.kktMat.setZero();
101 d.kktMat(0, 0) = knot.R;
102 d.kktMat(1, 0) = knot.D;
103 d.kktMat(0, 1) = knot.D.transpose();
104 d.kktMat(1, 1).diagonal().setConstant(-mueq);
105
106 d.kktMat(2, 0) = knot.B;
107 d.kktMat(0, 2) = knot.B.transpose();
108 d.kktMat(2, 2).diagonal().setConstant(-mudyn);
109 d.kktMat(2, 3) = knot.E;
110 d.kktMat(3, 2) = knot.E.transpose();
111 if (vn)
112 d.kktMat(3, 3) = vn->Pxx;
113
114 // 1. factorize
115 d.ldl.compute(d.kktMat.matrix());
116
117 // 2. rhs
118 // feedforward
119 VectorRef kff = d.ff[0] = -knot.r;
120 VectorRef zff = d.ff[1] = -knot.d;
121 VectorRef lff = d.ff[2] = -knot.f;
122 VectorRef yff = d.ff[3];
123 if (vn)
124 yff = -vn->px;
125
126 // feedback
127 RowMatrixRef K = d.fb.blockRow(0) = -knot.S.transpose();
128 RowMatrixRef Z = d.fb.blockRow(1) = -knot.C;
129 RowMatrixRef L = d.fb.blockRow(2) = -knot.A;
130 RowMatrixRef Y = d.fb.blockRow(3).setZero();
131
132 // parametric
133 RowMatrixRef Kth = d.ft.blockRow(0) = -knot.Gu;
134 RowMatrixRef Zth = d.ft.blockRow(1) = -knot.Gv;
135 d.ft.blockRow(2).setZero();
136 RowMatrixRef Yth = d.ft.blockRow(3);
137 if (vn)
138 Yth = -vn->Pxt;
139
140 d.ldl.solveInPlace(d.ff.matrix());
141 d.ldl.solveInPlace(d.fb.matrix());
142 d.ldl.solveInPlace(d.ft.matrix());
143
144 // 3. update value function
145 Eigen::Transpose At = knot.A.transpose();
146 Eigen::Transpose Ct = knot.C.transpose();
147
148 v.Pxx.noalias() = knot.Q + knot.S * K;
149 v.Pxx.noalias() += Ct * Z;
150 v.Pxx.noalias() += At * L;
151
152 v.Pxt = knot.Gx;
153 v.Pxt.noalias() += K.transpose() * knot.Gu;
154 v.Pxt.noalias() += Z.transpose() * knot.Gv;
155 if (vn)
156 v.Pxt.noalias() += Y.transpose() * vn->Pxt;
157
158 v.Ptt = knot.Gth;
159 v.Ptt.noalias() += Kth.transpose() * knot.Gu;
160 v.Ptt.noalias() += Zth.transpose() * knot.Gv;
161 if (vn)
162 v.Ptt.noalias() += Yth.transpose() * vn->Pxt;
163
164 v.px.noalias() = knot.q + knot.S * kff;
165 v.px.noalias() += Ct * zff;
166 v.px.noalias() += At * lff;
167
168 v.pt.noalias() = knot.gamma + knot.Gu.transpose() * kff;
169 v.pt.noalias() += knot.Gv.transpose() * zff;
170 if (vn)
171 v.pt.noalias() += vn->Pxt.transpose() * yff;
172 }
173
174 static bool forwardStep(size_t i, bool isTerminal, const_view_t knot,
175 const Data &d, boost::span<VectorXs> xs,
176 boost::span<VectorXs> us, boost::span<VectorXs> vs,
177 boost::span<VectorXs> lbdas,
178 const std::optional<ConstVectorRef> &theta_) {
179 ConstVectorRef kff = d.ff[0];
180 ConstVectorRef zff = d.ff[1];
181 ConstVectorRef lff = d.ff[2];
182 ConstVectorRef yff = d.ff[3];
183
184 ConstRowMatrixRef K = d.fb.blockRow(0);
185 ConstRowMatrixRef Z = d.fb.blockRow(1);
186 ConstRowMatrixRef L = d.fb.blockRow(2);
187 ConstRowMatrixRef Y = d.fb.blockRow(3);
188
189 ConstRowMatrixRef Kth = d.ft.blockRow(0);
190 ConstRowMatrixRef Zth = d.ft.blockRow(1);
191 ConstRowMatrixRef Lth = d.ft.blockRow(2);
192 ConstRowMatrixRef Yth = d.ft.blockRow(3);
193
194 if (knot.nu > 0)
195 us[i].noalias() = kff + K * xs[i];
196 vs[i].noalias() = zff + Z * xs[i];
197 if (theta_.has_value()) {
198 if (knot.nu > 0)
199 us[i].noalias() += Kth * theta_.value();
200 vs[i].noalias() += Zth * theta_.value();
201 }
202
203 if (isTerminal)
204 return true;
205 lbdas[i + 1].noalias() = lff + L * xs[i];
206 xs[i + 1].noalias() = yff + Y * xs[i];
207 if (theta_.has_value()) {
208 lbdas[i + 1].noalias() += Lth * theta_.value();
209 xs[i + 1].noalias() += Yth * theta_.value();
210 }
211 return true;
212 }
213};
214
215#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
216extern template struct DenseKernel<context::Scalar>;
217#endif
218} // namespace aligator::gar
Block matrix class, with a fixed-size number of row and column blocks.
auto blockRow(size_t i)
MatrixType & matrix()
unsigned int uint
Definition work.hpp:8
BunchKaufman< MatrixType_, UpLo_ > & compute(const EigenBase< InputType > &a)
bool solveInPlace(Eigen::MatrixBase< RhsType > &bAndX) const
BlkMatrix< VectorXs, 4, 1 > BlkVec4
BlkMatrix< MatrixXs, 4, 4 > BlkMat44
BlkMatrix< RowMatrixXs, 4, 1 > BlkRowMat41
Data(uint nx, uint nu, uint nc, uint nx2, uint nth)
Eigen::BunchKaufman< MatrixXs > ldl
A dense Bunch-Kaufman based kernel.
typename LqrKnotTpl< Scalar >::const_view_t const_view_t
static bool forwardStep(size_t i, bool isTerminal, const_view_t knot, const Data &d, boost::span< VectorXs > xs, boost::span< VectorXs > us, boost::span< VectorXs > vs, boost::span< VectorXs > lbdas, const std::optional< ConstVectorRef > &theta_)
static void terminalSolve(const_view_t knot, Data &d, value v, Scalar mueq)
ALIGATOR_DYNAMIC_TYPEDEFS_WITH_ROW_TYPES(Scalar)
static void stageKernelSolve(const_view_t knot, Data &d, value v, const value *vn, Scalar mudyn, Scalar mueq)
__view_base< true > const_view_t