aligator  0.16.0
A versatile and efficient C++ library for real-time constrained 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"
9#include <boost/core/span.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 KnotType &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 KnotType &knot, Data &d, value v,
98 const value *vn, Scalar mueq) {
99 d.kktMat.setZero();
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);
104
105 d.kktMat(2, 0) = knot.B;
106 d.kktMat(0, 2) = knot.B.transpose();
107 // d.kktMat(2, 2).setZero();
108 d.kktMat(2, 3).setIdentity() *= -1;
109 d.kktMat(3, 2).setIdentity() *= -1;
110 if (vn)
111 d.kktMat(3, 3) = vn->Pxx;
112
113 // 1. factorize
114 d.ldl.compute(d.kktMat.matrix());
115
116 // 2. rhs
117 // feedforward
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];
122 if (vn)
123 yff = -vn->px;
124
125 // feedback
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();
130
131 // parametric
132 RowMatrixRef Kth = d.ft.blockRow(0) = -knot.Gu;
133 RowMatrixRef Zth = d.ft.blockRow(1) = -knot.Gv;
134 d.ft.blockRow(2).setZero();
135 RowMatrixRef Yth = d.ft.blockRow(3);
136 if (vn)
137 Yth = -vn->Pxt;
138
139 d.ldl.solveInPlace(d.ff.matrix());
140 d.ldl.solveInPlace(d.fb.matrix());
141 d.ldl.solveInPlace(d.ft.matrix());
142
143 // 3. update value function
144 Eigen::Transpose At = knot.A.transpose();
145 Eigen::Transpose Ct = knot.C.transpose();
146
147 v.Pxx.noalias() = knot.Q + knot.S * K;
148 v.Pxx.noalias() += Ct * Z;
149 v.Pxx.noalias() += At * L;
150
151 v.Pxt = knot.Gx;
152 v.Pxt.noalias() += K.transpose() * knot.Gu;
153 v.Pxt.noalias() += Z.transpose() * knot.Gv;
154 if (vn)
155 v.Pxt.noalias() += Y.transpose() * vn->Pxt;
156
157 v.Ptt = knot.Gth;
158 v.Ptt.noalias() += Kth.transpose() * knot.Gu;
159 v.Ptt.noalias() += Zth.transpose() * knot.Gv;
160 if (vn)
161 v.Ptt.noalias() += Yth.transpose() * vn->Pxt;
162
163 v.px.noalias() = knot.q + knot.S * kff;
164 v.px.noalias() += Ct * zff;
165 v.px.noalias() += At * lff;
166
167 v.pt.noalias() = knot.gamma + knot.Gu.transpose() * kff;
168 v.pt.noalias() += knot.Gv.transpose() * zff;
169 if (vn)
170 v.pt.noalias() += vn->Pxt.transpose() * yff;
171 }
172
173 static bool forwardStep(size_t i, bool isTerminal, const KnotType &knot,
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];
182
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);
187
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);
192
193 if (knot.nu > 0)
194 us[i].noalias() = kff + K * xs[i];
195 vs[i].noalias() = zff + Z * xs[i];
196 if (theta_.has_value()) {
197 if (knot.nu > 0)
198 us[i].noalias() += Kth * theta_.value();
199 vs[i].noalias() += Zth * theta_.value();
200 }
201
202 if (isTerminal)
203 return true;
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();
209 }
210 return true;
211 }
212};
213
214#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
215extern template struct DenseKernel<context::Scalar>;
216#endif
217} // namespace aligator::gar
Block matrix class, with a fixed or dynamic-size number of row and column blocks.
auto blockRow(size_t i)
MatrixType & matrix()
unsigned int uint
Definition logger.hpp:9
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