cyqlone develop
Fast, parallel and vectorized solver for linear systems with optimal control structure.
Loading...
Searching...
No Matches
data.tpp
Go to the documentation of this file.
1#include <cyqlone/cyqlone.hpp>
2
3#include <batmat/assume.hpp>
4#include <batmat/linalg/simdify.hpp>
5#include <guanaqo/blas/hl-blas-interface.hpp>
6#include <limits>
7#if !BATMAT_WITH_OPENMP
8#include <batmat/thread-pool.hpp>
9#endif
10
11namespace CYQLONE_NS(cyqlone) {
12
13using batmat::linalg::simdify;
14
15namespace detail {
16/// Simple (inefficient) matrix copy that supports slices with non-unit strides.
17template <class T1, class I1, class S1, guanaqo::StorageOrder O1, class T2, class I2, class S2,
18 guanaqo::StorageOrder O2>
19void copy(guanaqo::MatrixView<T1, I1, S1, O1> src, guanaqo::MatrixView<T2, I2, S2, O2> dst) {
20 assert(src.rows == dst.rows);
21 assert(src.cols == dst.cols);
22 for (index_t r = 0; r < src.rows; ++r) // TODO: optimize
23 for (index_t c = 0; c < src.cols; ++c)
24 dst(r, c) = src(r, c);
25}
26template <class T0, class T1, class I1, class S1, guanaqo::StorageOrder O1, class T2, class I2,
27 class S2, guanaqo::StorageOrder O2>
28/// Simple (inefficient) scaled matrix copy that supports slices with non-unit strides.
29void scale(T0 scalar, guanaqo::MatrixView<T1, I1, S1, O1> src,
30 guanaqo::MatrixView<T2, I2, S2, O2> dst) {
31 assert(src.rows == dst.rows);
32 assert(src.cols == dst.cols);
33 for (index_t r = 0; r < src.rows; ++r) // TODO: optimize
34 for (index_t c = 0; c < src.cols; ++c)
35 dst(r, c) = scalar * src(r, c);
36}
37} // namespace detail
38
39template <index_t VL, class T, StorageOrder DefaultOrder, class Ctx>
40CyqloneSolver<VL, T, DefaultOrder, Ctx>
41CyqloneSolver<VL, T, DefaultOrder, Ctx>::build(const CyqloneStorage<value_type> &ocp, index_t p) {
42 BATMAT_ASSERT(p > 0);
43 BATMAT_ASSERT(v == 1 || is_pow_2(p));
44 CyqloneSolver<VL, T, DefaultOrder> res{
45 .N_horiz = ocp.N_horiz,
46 .nx = ocp.nx,
47 .nu = ocp.nu,
48 .ny = ocp.ny,
49 .ny_0 = ocp.ny_0,
50 .ny_N = ocp.ny_N,
51 .p = p,
52 };
53 res.update_data(ocp);
54 return res;
55}
56
57// For lgp = 5, lgv = 2, N = 3 << lgp
58//
59// | Stage j | Thread c | Index i | Data di | λ(A) | λ(I) | bλ(A) | bλ(I) |
60// |:-------:|:--------:|:-------:|:-------:|-----:|-----:|------:|------:|
61// | 0/96 | 0 | 0 | 0 | 0 | 93 | 0 | 7* |
62// | 95 | 0 | 1 | 1 | | | | |
63// | 94 | 0 | 2 | 2 | | | | |
64// | | | | | | | | |
65// | 3 | 1 | 0 | 3 | 3 | 0 | 1 | 0 |
66// | 2 | 1 | 1 | 4 | | | | |
67// | 1 | 1 | 2 | 5 | | | | |
68// | | | | | | | | |
69// | 6 | 2 | 0 | 6 | 6 | 3 | 2 | 1 |
70// | 5 | 2 | 1 | 7 | | | | |
71// | 4 | 2 | 2 | 8 | | | | |
72// | | | | | | | | |
73// | 9 | 3 | 0 | 9 | 9 | 6 | 3 | 2 |
74// | 8 | 3 | 1 | 10 | | | | |
75// | 7 | 3 | 2 | 11 | | | | |
76// | | | | | | | | |
77// | 12 | 4 | 0 | 12 | 12 | 9 | 4 | 3 |
78// | 11 | 4 | 1 | 13 | | | | |
79// | 10 | 4 | 2 | 14 | | | | |
80// | | | | | | | | |
81// | 15 | 5 | 0 | 15 | 15 | 12 | 5 | 4 |
82// | 14 | 5 | 1 | 16 | | | | |
83// | 13 | 5 | 2 | 17 | | | | |
84// | | | | | | | | |
85// | 18 | 6 | 0 | 18 | 18 | 15 | 6 | 5 |
86// | 17 | 6 | 1 | 19 | | | | |
87// | 16 | 6 | 2 | 20 | | | | |
88// | | | | | | | | |
89// | 21 | 7 | 0 | 21 | 21 | 18 | 7 | 6 |
90// | 20 | 7 | 1 | 22 | | | | |
91// | 19 | 7 | 2 | 23 | | | | |
92
93template <index_t VL, class T, StorageOrder DefaultOrder, class Ctx>
94void CyqloneSolver<VL, T, DefaultOrder, Ctx>::update_data(const CyqloneStorage<value_type> &ocp) {
95 using cyqlone::detail::copy;
96 using cyqlone::detail::scale;
97 BATMAT_ASSERT(ocp.N_horiz == N_horiz);
98 BATMAT_ASSERT(ocp.nx == nx);
99 BATMAT_ASSERT(ocp.nu == nu);
100 BATMAT_ASSERT(ocp.ny == ny);
101 BATMAT_ASSERT(ocp.ny_0 == ny_0);
102 BATMAT_ASSERT(ocp.ny_N == ny_N);
103 const auto scale_QN = 1 / static_cast<value_type>(ceil_N() - N_horiz + 1);
104 for (index_t c = 0; c < p; ++c) {
105 const index_t k0 = c * n;
106 const index_t di0 = c * n;
107 for (index_t i = 0; i < n; ++i) {
108 index_t di = di0 + i;
109 for (index_t vi = 0; vi < v; ++vi) {
110 auto k = sub_wrap_ceil_N(k0 + vi * p * n, i);
111 if (k == 0) {
112 if (ceil_N() == N_horiz) {
113 copy(ocp.data_F(0), data_F.batch(di)(vi)); // A, B
114 copy(ocp.data_H(0), data_H.batch(di)(vi)); // R, S, Q
115 } else {
116 copy(ocp.data_F(0).left_cols(nu),
117 data_F.batch(di)(vi).left_cols(nu)); // B
118 data_F.batch(di)(vi).right_cols(nx).set_constant(0); // A = 0
119 copy(ocp.data_H(0).top_left(nu, nu), //
120 data_H.batch(di)(vi).top_left(nu, nu)); // R
121 data_H.batch(di)(vi).bottom_left(nx, nu).set_constant(0); // S = 0
122 scale(scale_QN, ocp.data_H(0).bottom_right(nx, nx), //
123 data_H.batch(di)(vi).bottom_right(nx, nx)); // Q = α Q(N)
124 }
125 copy(ocp.data_G0N(0).transposed(),
126 data_Gᵀ.batch(di)(vi).left_cols(ny_0 + ny_N)); // D, C
127 } else if (k < N_horiz) {
128 copy(ocp.data_F(k), data_F.batch(di)(vi)); // A, B
129 copy(ocp.data_H(k), data_H.batch(di)(vi)); // R, S, Q
130 copy(ocp.data_G(k - 1).transposed(), //
131 data_Gᵀ.batch(di)(vi).left_cols(ny)); // D, C
132 } else {
133 data_F.batch(di)(vi).set_constant(0); // B = 0
134 data_F.batch(di)(vi).right_cols(nx).set_diagonal(1); // A = I
135 data_H.batch(di)(vi).left_cols(nu).set_constant(0); // S = 0
136 data_H.batch(di)(vi).top_left(nu, nu).set_diagonal(1); // R = I
137 scale(scale_QN, ocp.data_H(0).bottom_right(nx, nx), //
138 data_H.batch(di)(vi).bottom_right(nx, nx)); // Q = α Q(N)
139 data_Gᵀ.batch(di)(vi).left_cols(ny).set_constant(0); // D, C
140 }
141 }
142 }
143 }
144}
145
146template <index_t VL, class T, StorageOrder DefaultOrder, class Ctx>
147void CyqloneSolver<VL, T, DefaultOrder, Ctx>::initialize_rhs(const CyqloneStorage<value_type> &ocp,
148 mut_view<> rhs) const {
149 BATMAT_ASSERT(rhs.depth() == ceil_N());
150 BATMAT_ASSERT(rhs.rows() == nx);
151 BATMAT_ASSERT(rhs.cols() == 1);
152 for (index_t ti = 0; ti < p; ++ti) {
153 const index_t k0 = ti * n;
154 const index_t di0 = ti * n;
155 for (index_t i = 0; i < n; ++i) {
156 index_t di = di0 + i;
157 for (index_t vi = 0; vi < v; ++vi) {
158 auto k = sub_wrap_ceil_N(k0 + vi * p * n, i);
159 if (k < N_horiz) {
160 rhs.batch(di)(vi) = ocp.data_c(k);
161 } else {
162 rhs.batch(di)(vi).set_constant(0);
163 }
164 }
165 }
166 }
167}
168
169template <index_t VL, class T, StorageOrder DefaultOrder, class Ctx>
170void CyqloneSolver<VL, T, DefaultOrder, Ctx>::initialize_gradient(
171 const CyqloneStorage<value_type> &ocp, mut_view<> grad) const {
172 BATMAT_ASSERT(grad.depth() == ceil_N());
173 BATMAT_ASSERT(grad.rows() == nu + nx);
174 BATMAT_ASSERT(grad.cols() == 1);
175 const auto scale_qN = 1 / static_cast<value_type>(ceil_N() - N_horiz + 1);
176 for (index_t ti = 0; ti < p; ++ti) {
177 const index_t k0 = ti * n;
178 const index_t di0 = ti * n;
179 for (index_t i = 0; i < n; ++i) {
180 index_t di = di0 + i;
181 for (index_t vi = 0; vi < v; ++vi) {
182 auto k = sub_wrap_ceil_N(k0 + vi * p * n, i);
183 if (k == 0) {
184 if (ceil_N() == N_horiz) {
185 grad.batch(di)(vi) = ocp.data_rq(0);
186 } else {
187 grad.batch(di)(vi).top_rows(nu) = ocp.data_rq(0).top_rows(nu);
188 cyqlone::detail::scale(scale_qN, ocp.data_rq(0).bottom_rows(nx),
189 grad.batch(di)(vi).bottom_rows(nx));
190 }
191 } else if (k < N_horiz) {
192 grad.batch(di)(vi) = ocp.data_rq(k);
193 } else {
194 grad.batch(di)(vi).top_rows(nu).set_constant(0);
195 cyqlone::detail::scale(scale_qN, ocp.data_rq(0).bottom_rows(nx),
196 grad.batch(di)(vi).bottom_rows(nx));
197 }
198 }
199 }
200 }
201}
202
203template <index_t VL, class T, StorageOrder DefaultOrder, class Ctx>
204void CyqloneSolver<VL, T, DefaultOrder, Ctx>::initialize_bounds(
205 const CyqloneStorage<value_type> &ocp, mut_view<> b_min, mut_view<> b_max) const {
206 const index_t nyM = std::max(ny, ny_0 + ny_N);
207 BATMAT_ASSERT(b_min.depth() == ceil_N());
208 BATMAT_ASSERT(b_min.rows() == nyM);
209 BATMAT_ASSERT(b_min.cols() == 1);
210 BATMAT_ASSERT(b_max.depth() == ceil_N());
211 BATMAT_ASSERT(b_max.rows() == nyM);
212 BATMAT_ASSERT(b_max.cols() == 1);
213 const auto inf = std::numeric_limits<value_type>::infinity();
214 for (index_t ti = 0; ti < p; ++ti) {
215 const index_t k0 = ti * n;
216 const index_t di0 = ti * n;
217 for (index_t i = 0; i < n; ++i) {
218 index_t di = di0 + i;
219 for (index_t vi = 0; vi < v; ++vi) {
220 auto k = sub_wrap_ceil_N(k0 + vi * p * n, i);
221 auto b_min_i = b_min.batch(di)(vi), b_max_i = b_max.batch(di)(vi);
222 if (k == 0) {
223 b_min_i.top_rows(ny_0 + ny_N) = ocp.data_lb0N(0);
224 b_max_i.top_rows(ny_0 + ny_N) = ocp.data_ub0N(0);
225 b_min_i.bottom_rows(nyM - ny_0 - ny_N).set_constant(-inf);
226 b_max_i.bottom_rows(nyM - ny_0 - ny_N).set_constant(+inf);
227 } else if (k < N_horiz) {
228 b_min_i.top_rows(ny) = ocp.data_lb(k - 1);
229 b_max_i.top_rows(ny) = ocp.data_ub(k - 1);
230 b_min_i.bottom_rows(nyM - ny).set_constant(-inf);
231 b_max_i.bottom_rows(nyM - ny).set_constant(+inf);
232 } else {
233 b_min_i.set_constant(-inf);
234 b_max_i.set_constant(+inf);
235 }
236 }
237 }
238 }
239}
240
241template <index_t VL, class T, StorageOrder DefaultOrder, class Ctx>
242void CyqloneSolver<VL, T, DefaultOrder, Ctx>::pack_variables(std::span<const value_type> ux_lin,
243 mut_view<> ux) const {
244 const index_t nux = nu + nx;
245 BATMAT_ASSERT(static_cast<index_t>(ux_lin.size()) == nux * N_horiz);
246 BATMAT_ASSERT(ux.depth() == ceil_N());
247 BATMAT_ASSERT(ux.rows() == nux);
248 BATMAT_ASSERT(ux.cols() == 1);
249 for (index_t ti = 0; ti < p; ++ti) {
250 const index_t k0 = ti * n;
251 const index_t di0 = ti * n;
252 for (index_t i = 0; i < n; ++i) {
253 index_t di = di0 + i;
254 for (index_t vi = 0; vi < v; ++vi) {
255 auto k = sub_wrap_ceil_N(k0 + vi * p * n, i);
256 using crview = guanaqo::MatrixView<const value_type, index_t>;
257 if (k == 0) {
258 ux.batch(di)(vi).top_rows(nu) = crview::as_column(ux_lin.first(nu));
259 if (ceil_N() == N_horiz)
260 ux.batch(di)(vi).bottom_rows(nx) =
261 crview::as_column(ux_lin.subspan(nu + (N_horiz - 1) * nux, nx));
262 else
263 ux.batch(di)(vi).bottom_rows(nx).set_constant(0);
264 } else if (k < N_horiz) {
265 ux.batch(di)(vi).top_rows(nu) = crview::as_column(ux_lin.subspan(k * nux, nu));
266 ux.batch(di)(vi).bottom_rows(nx) =
267 crview::as_column(ux_lin.subspan(nu + (k - 1) * nux, nx));
268 } else if (k == N_horiz) {
269 // only pack the last state if we have padding stages
270 ux.batch(di)(vi).bottom_rows(nx) =
271 crview::as_column(ux_lin.subspan(nu + (N_horiz - 1) * nux, nx));
272 } else {
273 ux.batch(di)(vi).set_constant(0);
274 }
275 }
276 }
277 }
278}
279
280template <index_t VL, class T, StorageOrder DefaultOrder, class Ctx>
281void CyqloneSolver<VL, T, DefaultOrder, Ctx>::unpack_variables(view<> ux,
282 std::span<value_type> ux_lin) const {
283 const index_t nux = nu + nx;
284 BATMAT_ASSERT(static_cast<index_t>(ux_lin.size()) == nux * N_horiz);
285 BATMAT_ASSERT(ux.depth() == ceil_N());
286 BATMAT_ASSERT(ux.rows() == nux);
287 BATMAT_ASSERT(ux.cols() == 1);
288 for (index_t ti = 0; ti < p; ++ti) {
289 const index_t k0 = ti * n;
290 const index_t di0 = ti * n;
291 for (index_t i = 0; i < n; ++i) {
292 index_t di = di0 + i;
293 for (index_t vi = 0; vi < v; ++vi) {
294 auto k = sub_wrap_ceil_N(k0 + vi * p * n, i);
295 using rview = guanaqo::MatrixView<value_type, index_t>;
296 if (k == 0) {
297 rview::as_column(ux_lin.first(nu)) = ux.batch(di)(vi).top_rows(nu);
298 if (ceil_N() == N_horiz)
299 rview::as_column(ux_lin.subspan(nu + (N_horiz - 1) * nux, nx)) =
300 ux.batch(di)(vi).bottom_rows(nx);
301 } else if (k < N_horiz) {
302 rview::as_column(ux_lin.subspan(k * nux, nu)) = ux.batch(di)(vi).top_rows(nu);
303 rview::as_column(ux_lin.subspan(nu + (k - 1) * nux, nx)) =
304 ux.batch(di)(vi).bottom_rows(nx);
305 } else if (k == N_horiz) {
306 // only unpack the last state if we have padding stages
307 rview::as_column(ux_lin.subspan(nu + (N_horiz - 1) * nux, nx)) =
308 ux.batch(di)(vi).bottom_rows(nx);
309 }
310 }
311 }
312 }
313}
314
315template <index_t VL, class T, StorageOrder DefaultOrder, class Ctx>
316void CyqloneSolver<VL, T, DefaultOrder, Ctx>::pack_dynamics(std::span<const value_type> λ_lin,
317 mut_view<> λ) const {
318 const index_t nλ = nx;
319 BATMAT_ASSERT(static_cast<index_t>(λ_lin.size()) == nλ * N_horiz);
320 BATMAT_ASSERT(λ.depth() == ceil_N());
321 BATMAT_ASSERT(λ.rows() == nλ);
322 BATMAT_ASSERT(λ.cols() == 1);
323 for (index_t ti = 0; ti < p; ++ti) {
324 const index_t k0 = ti * n;
325 const index_t di0 = ti * n;
326 for (index_t i = 0; i < n; ++i) {
327 index_t di = di0 + i;
328 for (index_t vi = 0; vi < v; ++vi) {
329 auto k = sub_wrap_ceil_N(k0 + vi * p * n, i);
330 using crview = guanaqo::MatrixView<const value_type, index_t>;
331 if (k < N_horiz) {
332 λ.batch(di)(vi) = crview::as_column(λ_lin.subspan(k * nλ, nλ));
333 } else {
334 λ.batch(di)(vi).set_constant(0);
335 }
336 }
337 }
338 }
339}
340
341template <index_t VL, class T, StorageOrder DefaultOrder, class Ctx>
342void CyqloneSolver<VL, T, DefaultOrder, Ctx>::unpack_dynamics(view<> λ,
343 std::span<value_type> λ_lin) const {
344 const index_t nλ = nx;
345 BATMAT_ASSERT(static_cast<index_t>(λ_lin.size()) == nλ * N_horiz);
346 BATMAT_ASSERT(λ.depth() == ceil_N());
347 BATMAT_ASSERT(λ.rows() == nλ);
348 BATMAT_ASSERT(λ.cols() == 1);
349 for (index_t ti = 0; ti < p; ++ti) {
350 const index_t k0 = ti * n;
351 const index_t di0 = ti * n;
352 for (index_t i = 0; i < n; ++i) {
353 index_t di = di0 + i;
354 for (index_t vi = 0; vi < v; ++vi) {
355 auto k = sub_wrap_ceil_N(k0 + vi * p * n, i);
356 using rview = guanaqo::MatrixView<value_type, index_t>;
357 if (k < N_horiz) {
358 rview::as_column(λ_lin.subspan(k * nλ, nλ)) = λ.batch(di)(vi);
359 }
360 }
361 }
362 }
363}
364
365template <index_t VL, class T, StorageOrder DefaultOrder, class Ctx>
366void CyqloneSolver<VL, T, DefaultOrder, Ctx>::pack_constraints(std::span<const value_type> y_lin,
367 mut_view<> y,
368 value_type fill) const {
369 BATMAT_ASSERT(static_cast<index_t>(y_lin.size()) == ny * (N_horiz - 1) + ny_0 + ny_N);
370 BATMAT_ASSERT(y.depth() == ceil_N());
371 BATMAT_ASSERT(y.rows() == std::max(ny, ny_0 + ny_N));
372 BATMAT_ASSERT(y.cols() == 1);
373 for (index_t ti = 0; ti < p; ++ti) {
374 const index_t k0 = ti * n;
375 const index_t di0 = ti * n;
376 for (index_t i = 0; i < n; ++i) {
377 index_t di = di0 + i;
378 for (index_t vi = 0; vi < v; ++vi) {
379 auto k = sub_wrap_ceil_N(k0 + vi * p * n, i);
380 using crview = guanaqo::MatrixView<const value_type, index_t>;
381 if (k == 0) {
382 index_t ny_pad = std::max(ny, ny_0 + ny_N) - (ny_0 + ny_N);
383 y.batch(di)(vi).top_rows(ny_0) = crview::as_column(y_lin.first(ny_0));
384 y.batch(di)(vi).bottom_rows(ny_N) =
385 crview::as_column(y_lin.subspan(ny_0 + (N_horiz - 1) * ny, ny_N));
386 y.batch(di)(vi).bottom_rows(ny_pad).set_constant(fill);
387 } else if (k < N_horiz) {
388 index_t ny_pad = std::max(ny, ny_0 + ny_N) - ny;
389 y.batch(di)(vi).top_rows(ny) =
390 crview::as_column(y_lin.subspan(ny_0 + (k - 1) * ny, ny));
391 y.batch(di)(vi).bottom_rows(ny_pad).set_constant(fill);
392 } else {
393 y.batch(di)(vi).set_constant(0);
394 }
395 }
396 }
397 }
398}
399
400template <index_t VL, class T, StorageOrder DefaultOrder, class Ctx>
401void CyqloneSolver<VL, T, DefaultOrder, Ctx>::unpack_constraints(
402 view<> y, std::span<value_type> y_lin) const {
403 BATMAT_ASSERT(static_cast<index_t>(y_lin.size()) == ny * (N_horiz - 1) + ny_0 + ny_N);
404 BATMAT_ASSERT(y.depth() == ceil_N());
405 BATMAT_ASSERT(y.rows() == std::max(ny, ny_0 + ny_N));
406 BATMAT_ASSERT(y.cols() == 1);
407 for (index_t ti = 0; ti < p; ++ti) {
408 const index_t k0 = ti * n;
409 const index_t di0 = ti * n;
410 for (index_t i = 0; i < n; ++i) {
411 index_t di = di0 + i;
412 for (index_t vi = 0; vi < v; ++vi) {
413 auto k = sub_wrap_ceil_N(k0 + vi * p * n, i);
414 using rview = guanaqo::MatrixView<value_type, index_t>;
415 if (k == 0) {
416 rview::as_column(y_lin.first(ny_0)) = y.batch(di)(vi).top_rows(ny_0);
417 rview::as_column(y_lin.subspan(ny_0 + (N_horiz - 1) * ny, ny_N)) =
418 y.batch(di)(vi).bottom_rows(ny_N);
419 } else if (k < N_horiz) {
420 rview::as_column(y_lin.subspan(ny_0 + (k - 1) * ny, ny)) =
421 y.batch(di)(vi).top_rows(ny);
422 }
423 }
424 }
425 }
426}
427
428template <index_t VL, class T, StorageOrder DefaultOrder, class Ctx>
435
436template <index_t VL, class T, StorageOrder DefaultOrder, class Ctx>
443
444template <index_t VL, class T, StorageOrder DefaultOrder, class Ctx>
445auto CyqloneSolver<VL, T, DefaultOrder, Ctx>::initialize_bounds(
446 const CyqloneStorage<value_type> &ocp) const -> std::pair<matrix<>, matrix<>> {
447 std::pair b{initialize_general_constraints(), initialize_general_constraints()};
448 initialize_bounds(ocp, b.first, b.second);
449 return b;
450}
451
452template <index_t VL, class T, StorageOrder DefaultOrder, class Ctx>
453auto CyqloneSolver<VL, T, DefaultOrder, Ctx>::pack_variables(
454 std::span<const value_type> ux_lin) const -> matrix<> {
455 matrix<> ux = initialize_variables();
456 pack_variables(ux_lin, ux);
457 return ux;
458}
459
460template <index_t VL, class T, StorageOrder DefaultOrder, class Ctx>
461auto CyqloneSolver<VL, T, DefaultOrder, Ctx>::unpack_variables(view<> ux) const
462 -> std::vector<value_type> {
463 std::vector<value_type> ux_lin(num_variables());
464 unpack_variables(ux, ux_lin);
465 return ux_lin;
466}
467
468template <index_t VL, class T, StorageOrder DefaultOrder, class Ctx>
469auto CyqloneSolver<VL, T, DefaultOrder, Ctx>::pack_dynamics(std::span<const value_type> λ_lin) const
470 -> matrix<> {
471 matrix<> λ = initialize_dynamics_constraints();
472 pack_dynamics(λ_lin, λ);
473 return λ;
474}
475
476template <index_t VL, class T, StorageOrder DefaultOrder, class Ctx>
477auto CyqloneSolver<VL, T, DefaultOrder, Ctx>::unpack_dynamics(view<> λ) const
478 -> std::vector<value_type> {
479 std::vector<value_type> λ_lin(num_dynamics_constraints());
480 unpack_dynamics(λ, λ_lin);
481 return λ_lin;
482}
483
484template <index_t VL, class T, StorageOrder DefaultOrder, class Ctx>
485auto CyqloneSolver<VL, T, DefaultOrder, Ctx>::pack_constraints(std::span<const value_type> y_lin,
486 value_type fill) const -> matrix<> {
487 matrix<> y = initialize_general_constraints();
488 pack_constraints(y_lin, y, fill);
489 return y;
490}
491
492template <index_t VL, class T, StorageOrder DefaultOrder, class Ctx>
493auto CyqloneSolver<VL, T, DefaultOrder, Ctx>::unpack_constraints(view<> y) const
494 -> std::vector<value_type> {
495 std::vector<value_type> y_lin(num_general_constraints());
496 unpack_constraints(y, y_lin);
497 return y_lin;
498}
499
500template <index_t VL, class T, StorageOrder DefaultOrder, class Ctx>
501auto CyqloneSolver<VL, T, DefaultOrder, Ctx>::initialize_variables() const -> matrix<> {
502 return matrix<>{{.depth = ceil_N(), .rows = nu + nx, .cols = 1}};
503}
504
505template <index_t VL, class T, StorageOrder DefaultOrder, class Ctx>
506auto CyqloneSolver<VL, T, DefaultOrder, Ctx>::initialize_dynamics_constraints() const -> matrix<> {
507 return matrix<>{{.depth = ceil_N(), .rows = nx, .cols = 1}};
508}
509
510template <index_t VL, class T, StorageOrder DefaultOrder, class Ctx>
511auto CyqloneSolver<VL, T, DefaultOrder, Ctx>::initialize_general_constraints() const -> matrix<> {
512 return matrix<>{{.depth = ceil_N(), .rows = std::max(ny, ny_0 + ny_N), .cols = 1}};
513}
514
515} // namespace CYQLONE_NS(cyqlone)
BATMAT_ASSERT
#define BATMAT_ASSERT(x)
cyqlone.hpp
The main header for the Cyqlone and Tricyqle linear solvers.
guanaqo::StorageOrder
StorageOrder
batmat::linalg::copy
void copy(VA &&A, VB &&B, Opts... opts)
cyqlone::linalg::scale
void scale(T alpha, Vx &&x, Vz &&z)
Multiply a vector by a scalar z = αx.
Definition linalg.hpp:294
batmat::linalg::fill
void fill(simdified_value_t< VB > a, VB &&B)
batmat::linalg::simdify
constexpr auto simdify(simdifiable auto &&a) -> simdified_view_t< decltype(a)>
cyqlone::detail::copy
void copy(guanaqo::MatrixView< T1, I1, S1, O1 > src, guanaqo::MatrixView< T2, I2, S2, O2 > dst)
Simple (inefficient) matrix copy that supports slices with non-unit strides.
Definition data.tpp:19
cyqlone::detail::scale
void scale(T0 scalar, guanaqo::MatrixView< T1, I1, S1, O1 > src, guanaqo::MatrixView< T2, I2, S2, O2 > dst)
Simple (inefficient) scaled matrix copy that supports slices with non-unit strides.
Definition data.tpp:29
cyqlone::is_pow_2
constexpr bool is_pow_2(index_t n)
Definition cyqlone.hpp:32
batmat::matrix::Matrix::bottom_rows
auto bottom_rows(index_type n)
batmat::matrix::Matrix::left_cols
auto left_cols(index_type n)
batmat::matrix::Matrix::top_rows
auto top_rows(index_type n)
batmat::matrix::Matrix::top_left
auto top_left(index_type nr, index_type nc)
batmat::matrix::Matrix::bottom_right
auto bottom_right(index_type nr, index_type nc)
cyqlone::CyqloneSolver
Linear solver for systems with optimal control structure.
Definition cyqlone.hpp:561
cyqlone::CyqloneSolver::n
const index_t n
Number of stages per thread per vector lane (rounded up).
Definition cyqlone.hpp:605
cyqlone::CyqloneSolver::num_variables
index_t num_variables() const
Get the total number of primal variables in the OCP.
Definition cyqlone.hpp:577
cyqlone::CyqloneSolver::data_H
matrix< default_order > data_H
Stage-wise Hessian blocks H(j) = [ R(j) S(j); S(j)ᵀ Q(j) ] of the OCP cost function.
Definition cyqlone.hpp:762
cyqlone::CyqloneSolver::initialize_variables
matrix initialize_variables() const
Get a zero-initialized matrix for the primal variables u and x.
Definition data.tpp:501
cyqlone::CyqloneSolver::ceil_N
index_t ceil_N() const
Horizon length, rounded up to a multiple of the number of parallel execution units.
Definition cyqlone.hpp:653
cyqlone::CyqloneSolver::view
typename tricyqle_t::template view< O > view
Non-owning immutable view type for matrix.
Definition cyqlone.hpp:693
cyqlone::CyqloneSolver::update_data
void update_data(const CyqloneStorage< value_type > &ocp)
Update the internal data structures to reflect changes in the OCP data (without changing the problem ...
Definition data.tpp:94
cyqlone::CyqloneSolver::data_F
matrix< default_order > data_F
Stage-wise dynamics matrices F(j) = [ B(j) A(j) ] of the OCP.
Definition cyqlone.hpp:766
cyqlone::CyqloneSolver::data_Gᵀ
matrix< default_order > data_Gᵀ
Stage-wise constraint Jacobians G(j)ᵀ = [ D(j) C(j) ]ᵀ of the OCP.
Definition cyqlone.hpp:770
cyqlone::CyqloneSolver::pack_variables
void pack_variables(std::span< const value_type > ux_lin, mut_view<> ux) const
Definition data.tpp:242
cyqlone::CyqloneSolver::pack_constraints
void pack_constraints(std::span< const value_type > y_lin, mut_view<> y, value_type fill=0) const
Definition data.tpp:366
cyqlone::CyqloneSolver::initialize_gradient
void initialize_gradient(const CyqloneStorage< value_type > &ocp, mut_view<> grad) const
Initialize the gradient vector for the OCP cost function, using the custom Cyqlone storage format.
Definition data.tpp:170
cyqlone::CyqloneSolver::unpack_variables
void unpack_variables(view<> ux, std::span< value_type > ux_lin) const
Definition data.tpp:281
cyqlone::CyqloneSolver::sub_wrap_ceil_N
index_t sub_wrap_ceil_N(index_t a, index_t b) const
Subtract b from a modulo N_horiz.
Definition indexing.tpp:53
cyqlone::CyqloneSolver::unpack_constraints
void unpack_constraints(view<> y, std::span< value_type > y_lin) const
Definition data.tpp:401
cyqlone::CyqloneSolver::matrix
typename tricyqle_t::template matrix< O > matrix
Owning type for a batch of matrices (with batch size v).
Definition cyqlone.hpp:690
cyqlone::CyqloneSolver::N_horiz
const index_t N_horiz
Horizon length of the optimal control problem.
Definition cyqlone.hpp:567
cyqlone::CyqloneSolver::num_dynamics_constraints
index_t num_dynamics_constraints() const
Get the total number of dynamics constraints in the OCP.
Definition cyqlone.hpp:581
cyqlone::CyqloneSolver::build
static CyqloneSolver build(const CyqloneStorage< value_type > &ocp, index_t p)
Initialize a Cyqlone solver for the given OCP.
Definition data.tpp:41
cyqlone::CyqloneSolver::ny
const index_t ny
Number of general constraints of the OCP per stage.
Definition cyqlone.hpp:570
cyqlone::CyqloneSolver::initialize_dynamics_constraints
matrix initialize_dynamics_constraints() const
Get a zero-initialized matrix for the dynamics constraints (or their multipliers).
Definition data.tpp:506
cyqlone::CyqloneSolver::initialize_rhs
void initialize_rhs(const CyqloneStorage< value_type > &ocp, mut_view<> rhs) const
Initialize the right-hand side vector for the dynamics constraints of the OCP, using the custom Cyqlo...
Definition data.tpp:147
cyqlone::CyqloneSolver::initialize_general_constraints
matrix initialize_general_constraints() const
Get a zero-initialized matrix for the general constraints (or their multipliers).
Definition data.tpp:511
cyqlone::CyqloneSolver::initialize_bounds
void initialize_bounds(const CyqloneStorage< value_type > &ocp, mut_view<> b_min, mut_view<> b_max) const
Initialize the lower and upper bounds for the general constraints of the OCP, using the custom Cyqlon...
Definition data.tpp:204
cyqlone::CyqloneSolver::mut_view
typename tricyqle_t::template mut_view< O > mut_view
Non-owning mutable view type for matrix.
Definition cyqlone.hpp:696
cyqlone::CyqloneSolver::pack_dynamics
void pack_dynamics(std::span< const value_type > λ_lin, mut_view<> λ) const
Definition data.tpp:316
cyqlone::CyqloneSolver::ny_0
const index_t ny_0
Number of general constraints at stage 0, D(0) u(0).
Definition cyqlone.hpp:571
cyqlone::CyqloneSolver::nu
const index_t nu
Number of controls of the OCP.
Definition cyqlone.hpp:569
cyqlone::CyqloneSolver::p
const index_t p
Number of processors/threads.
Definition cyqlone.hpp:601
cyqlone::CyqloneSolver::num_general_constraints
index_t num_general_constraints() const
Get the total number of general constraints in the OCP.
Definition cyqlone.hpp:585
cyqlone::CyqloneSolver::ny_N
const index_t ny_N
Number of general constraints at the final stage, C(N) x(N).
Definition cyqlone.hpp:572
cyqlone::CyqloneSolver::unpack_dynamics
void unpack_dynamics(view<> λ, std::span< value_type > λ_lin) const
Definition data.tpp:342
cyqlone::CyqloneSolver::v
static constexpr index_t v
Vector length.
Definition cyqlone.hpp:603
cyqlone::CyqloneSolver::nx
const index_t nx
Number of states of the OCP.
Definition cyqlone.hpp:568
cyqlone::CyqloneStorage
Storage for a linear-quadratic OCP with the initial states x₀ eliminated.
Definition cyqlone-storage.hpp:39