9#include <boost/core/span.hpp>
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} {
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;
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;
71 d.ldl.compute(d.kktMat.
matrix());
72 d.ldl.solveInPlace(d.ff.
matrix());
73 d.ldl.solveInPlace(d.fb.
matrix());
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());
79 Eigen::Transpose Ct = knot.
C.transpose();
81 v.Pxx.noalias() = knot.
Q + knot.
S * K;
82 v.Pxx.noalias() += Ct * Z;
84 v.Pxt.noalias() = knot.
Gx + K.transpose() * knot.
Gu;
85 v.Pxt.noalias() += Z.transpose() * knot.
Gv;
87 v.Ptt.noalias() = knot.
Gth + knot.
Gu.transpose() * Kth;
88 v.Ptt.noalias() += knot.
Gv.transpose() * Zth;
90 v.px.noalias() = knot.
q + knot.
S * kff;
91 v.px.noalias() += Ct * zff;
93 v.pt.noalias() = knot.
gamma + knot.
Gu.transpose() * kff;
94 v.pt.noalias() += knot.
Gv.transpose() * zff;
98 const value *vn,
Scalar mueq) {
100 d.kktMat(0, 0) = knot.
R;
101 d.kktMat(1, 0) = knot.
D;
102 d.kktMat(0, 1) = knot.
D.transpose();
103 d.kktMat(1, 1).diagonal().setConstant(-mueq);
105 d.kktMat(2, 0) = knot.
B;
106 d.kktMat(0, 2) = knot.
B.transpose();
108 d.kktMat(2, 3).setIdentity() *= -1;
109 d.kktMat(3, 2).setIdentity() *= -1;
111 d.kktMat(3, 3) = vn->Pxx;
114 d.ldl.compute(d.kktMat.
matrix());
118 VectorRef kff = d.ff[0] = -knot.
r;
119 VectorRef zff = d.ff[1] = -knot.
d;
120 VectorRef lff = d.ff[2] = -knot.
f;
121 VectorRef yff = d.ff[3];
126 RowMatrixRef K = d.fb.
blockRow(0) = -knot.
S.transpose();
127 RowMatrixRef Z = d.fb.
blockRow(1) = -knot.
C;
128 RowMatrixRef L = d.fb.
blockRow(2) = -knot.
A;
129 RowMatrixRef Y = d.fb.
blockRow(3).setZero();
132 RowMatrixRef Kth = d.ft.
blockRow(0) = -knot.
Gu;
133 RowMatrixRef Zth = d.ft.
blockRow(1) = -knot.
Gv;
135 RowMatrixRef Yth = d.ft.
blockRow(3);
139 d.ldl.solveInPlace(d.ff.
matrix());
140 d.ldl.solveInPlace(d.fb.
matrix());
141 d.ldl.solveInPlace(d.ft.
matrix());
144 Eigen::Transpose At = knot.
A.transpose();
145 Eigen::Transpose Ct = knot.
C.transpose();
147 v.Pxx.noalias() = knot.
Q + knot.
S * K;
148 v.Pxx.noalias() += Ct * Z;
149 v.Pxx.noalias() += At * L;
152 v.Pxt.noalias() += K.transpose() * knot.
Gu;
153 v.Pxt.noalias() += Z.transpose() * knot.
Gv;
155 v.Pxt.noalias() += Y.transpose() * vn->Pxt;
158 v.Ptt.noalias() += Kth.transpose() * knot.
Gu;
159 v.Ptt.noalias() += Zth.transpose() * knot.
Gv;
161 v.Ptt.noalias() += Yth.transpose() * vn->Pxt;
163 v.px.noalias() = knot.
q + knot.
S * kff;
164 v.px.noalias() += Ct * zff;
165 v.px.noalias() += At * lff;
167 v.pt.noalias() = knot.
gamma + knot.
Gu.transpose() * kff;
168 v.pt.noalias() += knot.
Gv.transpose() * zff;
170 v.pt.noalias() += vn->Pxt.transpose() * yff;
174 const Data &d, boost::span<VectorXs> xs,
175 boost::span<VectorXs> us, boost::span<VectorXs> vs,
176 boost::span<VectorXs> lbdas,
177 const std::optional<ConstVectorRef> &theta_) {
178 ConstVectorRef kff = d.ff[0];
179 ConstVectorRef zff = d.ff[1];
180 ConstVectorRef lff = d.ff[2];
181 ConstVectorRef yff = d.ff[3];
183 ConstRowMatrixRef K = d.fb.
blockRow(0);
184 ConstRowMatrixRef Z = d.fb.
blockRow(1);
185 ConstRowMatrixRef L = d.fb.
blockRow(2);
186 ConstRowMatrixRef Y = d.fb.
blockRow(3);
188 ConstRowMatrixRef Kth = d.ft.
blockRow(0);
189 ConstRowMatrixRef Zth = d.ft.
blockRow(1);
190 ConstRowMatrixRef Lth = d.ft.
blockRow(2);
191 ConstRowMatrixRef Yth = d.ft.
blockRow(3);
194 us[i].noalias() = kff + K * xs[i];
195 vs[i].noalias() = zff + Z * xs[i];
196 if (theta_.has_value()) {
198 us[i].noalias() += Kth * theta_.value();
199 vs[i].noalias() += Zth * theta_.value();
204 lbdas[i + 1].noalias() = lff + L * xs[i];
205 xs[i + 1].noalias() = yff + Y * xs[i];
206 if (theta_.has_value()) {
207 lbdas[i + 1].noalias() += Lth * theta_.value();
208 xs[i + 1].noalias() += Yth * theta_.value();
214#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
215extern template struct DenseKernel<context::Scalar>;
Block matrix class, with a fixed or dynamic-size number of row and column blocks.
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)
BunchKaufman< MatrixXs > ldl
A dense Bunch-Kaufman based kernel.
static bool forwardStep(size_t i, bool isTerminal, const KnotType &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_)
LqrKnotTpl< Scalar > KnotType
static void terminalSolve(const KnotType &knot, Data &d, value v, Scalar mueq)
ALIGATOR_DYNAMIC_TYPEDEFS_WITH_ROW_TYPES(Scalar)
static void stageKernelSolve(const KnotType &knot, Data &d, value v, const value *vn, Scalar mueq)
Struct describing a stage of a constrained LQ problem.
ArenaMatrix< MatrixXs > Gv
ArenaMatrix< MatrixXs > A
ArenaMatrix< MatrixXs > D
ArenaMatrix< MatrixXs > Q
ArenaMatrix< VectorXs > q
ArenaMatrix< MatrixXs > Gx
ArenaMatrix< MatrixXs > Gth
ArenaMatrix< MatrixXs > B
ArenaMatrix< MatrixXs > R
ArenaMatrix< MatrixXs > S
ArenaMatrix< VectorXs > d
ArenaMatrix< VectorXs > gamma
ArenaMatrix< MatrixXs > Gu
ArenaMatrix< MatrixXs > C
ArenaMatrix< VectorXs > r
ArenaMatrix< VectorXs > f