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