smooth_feedback
Control and estimation on Lie groups
Loading...
Searching...
No Matches
ocp_flatten.hpp
Go to the documentation of this file.
1// Copyright (C) 2022 Petter Nilsson. MIT License.
2
3#pragma once
4
14#include <Eigen/Core>
15#include <Eigen/Sparse>
16#include <smooth/bundle.hpp>
17#include <smooth/diff.hpp>
18
19#include "ocp.hpp"
20#include "smooth/lie_sparse.hpp"
21#include "utils/sparse.hpp"
22
23namespace smooth::feedback {
24
25// \cond
26namespace detail {
27
29static constexpr std::array<double, 23> kBn{
30 1, // 0
31 -1. / 2, // 1
32 1. / 6, // 2
33 0., // 3
34 -1. / 30, // 4
35 0, // 5
36 1. / 42, // 6
37 0, // 7
38 -1. / 30, // 8
39 0, // 9
40 5. / 66, // 10
41 0, // 11
42 -691. / 2730, // 12
43 0, // 13
44 7. / 6, // 14
45 0, // 15
46 -3617. / 510, // 16
47 0, // 17
48 43867. / 798, // 18
49 0, // 19
50 -174611. / 330, // 20
51 0, // 21
52 854513. / 138, // 22
53};
54
58template<LieGroup G>
59inline auto generators_sparse_reordered = []() -> std::array<Eigen::SparseMatrix<double, Eigen::RowMajor>, Dof<G>> {
60 std::array<Eigen::SparseMatrix<double, Eigen::RowMajor>, Dof<G>> ret;
61 for (auto k = 0u; k < Dof<G>; ++k) {
62 ret[k].resize(Dof<G>, Dof<G>);
63 for (auto i = 0u; i < Dof<G>; ++i) { ret[k].row(i) = ::smooth::generators_sparse<G>[i].row(k); }
64 ret[k].makeCompressed();
65 }
66 return ret;
67}();
68
72template<LieGroup G>
73inline Eigen::SparseMatrix<double> d_ad = []() -> Eigen::SparseMatrix<double> {
74 Eigen::SparseMatrix<double> ret;
75 ret.resize(Dof<G>, Dof<G> * Dof<G>);
76 for (auto i = 0u; i < Dof<G>; ++i) {
77 for (auto j = 0u; j < Dof<G>; ++j) { ret.col(j * Dof<G> + i) = smooth::generators_sparse<G>[i].row(j).transpose(); }
78 }
79 ret.makeCompressed();
80 return ret;
81}();
82
89template<LieGroup X, Manifold U, typename F, typename Xl, typename Ul>
90class FlatDyn
91{
92private:
93 using BundleT = smooth::Bundle<Eigen::Vector<double, 1>, X, U>;
94
95 static constexpr auto Nx = Dof<X>;
96 static constexpr auto Nu = Dof<U>;
97 static constexpr auto Nouts = Nx;
98 static constexpr auto Nvars = 1 + Nx + Nu;
99
100 static constexpr auto t_B = 0;
101 static constexpr auto x_B = t_B + 1;
102 static constexpr auto u_B = x_B + Nx;
103
104 using E = Tangent<X>;
105 using V = Tangent<U>;
106
107 F f;
108 Xl xl;
109 Ul ul;
110
111 Eigen::SparseMatrix<double> ad_e_ = smooth::ad_sparse_pattern<X>;
112 Eigen::SparseMatrix<double> ad_vi = smooth::ad_sparse_pattern<X>;
113
114 Eigen::SparseMatrix<double> Joplus_ = smooth::d_exp_sparse_pattern<BundleT>;
115 Eigen::SparseMatrix<double> Hoplus_ = smooth::d2_exp_sparse_pattern<BundleT>;
116
117 Eigen::SparseMatrix<double> dexpinv_e_ = smooth::d_exp_sparse_pattern<X>;
118 Eigen::SparseMatrix<double> d2expinv_e_ = smooth::d2_exp_sparse_pattern<X>;
119
120 Eigen::SparseMatrix<double> J_{Nouts, Nvars};
121 Eigen::SparseMatrix<double> H_{Nvars, Nouts * Nvars};
122
123 // Would ideally like to remove these temporaries...
124 Eigen::SparseMatrix<double> ji_{Nouts, Nvars}, ji_tmp_{Nouts, Nvars}, hi_{Nvars, Nouts *Nvars},
125 hi_tmp_{Nvars, Nouts *Nvars};
126
128 void update_joplus(const E & e, const V & v, const E & dxl, const V & dul)
129 {
130 dr_exp_sparse<BundleT>(Joplus_, (Tangent<BundleT>() << 1, e, v).finished());
131
132 block_write(Joplus_, x_B, t_B, Ad<X>(smooth::exp<X>(-e)) * dxl);
133 block_write(Joplus_, u_B, t_B, Ad<U>(smooth::exp<U>(-v)) * dul);
134
135 Joplus_.makeCompressed();
136 }
137
139 void update_hoplus(const E & e, const V & v, const E & dxl, const V & dul)
140 {
141 d2r_exp_sparse<BundleT>(Hoplus_, (Tangent<BundleT>() << 1, e, v).finished());
142
143 // d (Ad_X b) = -ad_(Ad_X b) * Ad_X
144 const TangentMap<X> Adexp_X = Ad<X>(smooth::exp<X>(-e));
145 const TangentMap<X> dAdexp_X = ad<X>(Adexp_X * dxl) * Adexp_X * dr_exp<X>(-e);
146 const TangentMap<U> Adexp_U = Ad<U>(smooth::exp<U>(-v));
147 const TangentMap<U> dAdexp_U = ad<U>(Adexp_U * dul) * Adexp_U * dr_exp<U>(-v);
148
149 for (auto nx = 0u; nx < Nx; ++nx) {
150 const auto b0 = Nvars * (x_B + nx);
151 block_write(Hoplus_, t_B, b0 + x_B, dAdexp_X.middleRows(nx, 1));
152 }
153 for (auto nu = 0u; nu < Nu; ++nu) {
154 const auto b0 = Nvars * (u_B + nu);
155 block_write(Hoplus_, t_B, b0 + u_B, dAdexp_U.middleRows(nu, 1));
156 }
157
158 Hoplus_.makeCompressed();
159 }
160
161public:
162 template<typename A1, typename A2, typename A3>
163 FlatDyn(A1 && a1, A2 && a2, A3 && a3) : f(std::forward<A1>(a1)), xl(std::forward<A2>(a2)), ul(std::forward<A3>(a3))
164 {}
165
166 template<typename T>
167 CastT<T, E> operator()(const T & t, const CastT<T, E> & e, const CastT<T, V> & v) const
168 {
169 using XT = CastT<T, X>;
170
171 // can not double-differentiate, so we hide derivative of xl w.r.t. t
172 const double tdbl = static_cast<double>(t);
173 const auto [unused, dxlval] = diff::dr<1>(xl, wrt(tdbl));
174
175 return dr_expinv<XT>(e) * (f(t, rplus(xl(t), e), rplus(ul(t), v)) - dxlval.template cast<T>())
176 + ad<XT>(e) * dxlval.template cast<T>();
177 }
178
179 // First derivative
180 std::reference_wrapper<const Eigen::SparseMatrix<double>>
181 jacobian(double t, const E & e, const V & v) requires(diff::detail::diffable_order1<F, std::tuple<double, X, U>>)
182 {
183 const double tdbl = static_cast<double>(t);
184 const auto [xlval, dxlval] = diff::dr<1>(xl, wrt(tdbl));
185 const auto [ulval, dulval] = diff::dr<1>(ul, wrt(tdbl));
186 const auto x = rplus(xlval, e);
187 const auto u = rplus(ulval, v);
188
189 dr_expinv_sparse<X>(dexpinv_e_, e);
190 d2r_expinv_sparse<X>(d2expinv_e_, e);
191 update_joplus(e, v, dxlval, dulval);
192
193 // value and derivative of f
194 const auto fval = f(t, x, u);
195 const auto & Jf = f.jacobian(t, x, u);
196
197 // Want to differentiate drexpinv * (f o plus - dxl) + ad dxl
198
199 // Start with drexpinv * d (f \circ (+))
200 J_ = dexpinv_e_ * Jf * Joplus_;
201 // Add d ( drexpinv ) * (f \circ (+) - dxl)
202 for (auto i = 0u; i < d2expinv_e_.outerSize(); ++i) {
203 for (Eigen::InnerIterator it(d2expinv_e_, i); it; ++it) {
204 J_.coeffRef(it.col() / Nx, 1 + (it.col() % Nx)) += (fval(it.row()) - dxlval(it.row())) * it.value();
205 }
206 }
207 // Add d ( ad ) * dxl
208 for (auto i = 0u; i < d_ad<X>.outerSize(); ++i) {
209 for (Eigen::InnerIterator it(d_ad<X>, i); it; ++it) {
210 J_.coeffRef(it.col() / Nx, 1 + (it.col() % Nx)) += dxlval(it.row()) * it.value();
211 }
212 }
213
214 J_.makeCompressed();
215 return J_;
216 }
217
218 // Second derivative
219 // \sum Bn (-1)^n / n! d2r (ad_a^n f)_aa - \sum Bn / n! d2r(ad_a^n dxl)_aa
220 std::reference_wrapper<const Eigen::SparseMatrix<double>>
221 hessian(double t, const E & e, const V & v) requires(diff::detail::diffable_order1<F, std::tuple<double, X, U>> &&
222 diff::detail::diffable_order2<F, std::tuple<double, X, U>>)
223 {
224 const double tdbl = static_cast<double>(t);
225 const auto [xlval, dxlval] = diff::dr<1>(xl, wrt(tdbl));
226 const auto [ulval, dulval] = diff::dr<1>(ul, wrt(tdbl));
227
228 const auto x = rplus(xlval, e);
229 const auto u = rplus(ul(t), v);
230 const auto & Jf = f.jacobian(t, x, u); // nx x (1 + nx + nu)
231 const auto & Hf = f.hessian(t, x, u); // (1 + nx + nu) x (nx * (1 + nx + nu))
232
233 ad_sparse<X>(ad_e_, e);
234 update_joplus(e, v, dxlval, dulval);
235 update_hoplus(e, v, dxlval, dulval);
236
237 double coef = 1; // hold (-1)^i / i!
238 Tangent<X> vi = f(t, x, u) - dxlval; // (ad_a)^i * (f - dxl)
239 ji_ = Jf * Joplus_; // dr (vi)_{t, e, v}
240 set_zero(hi_);
241 d2r_fog(hi_, Jf, Hf, Joplus_, Hoplus_); // d2r (vi)_{t, e, v}
242
243 set_zero(H_);
244 for (auto iter = 0u; iter < std::tuple_size_v<decltype(kBn)>; ++iter) {
245 if (kBn[iter] != 0) { block_add(H_, 0, 0, hi_, kBn[iter] * coef); }
246
247 // update hi_
248 hi_tmp_.setZero();
249 for (auto i = 0u; i < ad_e_.outerSize(); ++i) {
250 for (Eigen::InnerIterator it(ad_e_, i); it; ++it) {
251 const auto b0 = it.row() * Nvars;
252 block_add(hi_tmp_, 0, b0, hi_.middleCols(it.col() * Nvars, Nvars), it.value());
253 }
254 }
255 for (auto k = 0u; k < Nx; ++k) {
256 const auto b0 = k * Nvars;
257 block_add(hi_tmp_, 1, b0, generators_sparse_reordered<X>[k] * ji_);
258 block_add(hi_tmp_, 0, b0 + 1, ji_.transpose() * generators_sparse_reordered<X>[k], -1);
259 }
260 std::swap(hi_, hi_tmp_);
261
262 // update ji
263 ji_tmp_.setZero();
264 ji_tmp_ = ad_e_ * ji_;
265 ad_sparse<X>(ad_vi, vi);
266 block_add(ji_tmp_, 0, 1, ad_vi, -1);
267 std::swap(ji_, ji_tmp_);
268
269 // update vi
270 vi.applyOnTheLeft(ad_e_);
271
272 coef *= (-1.) / (iter + 1);
273 }
274
275 H_.makeCompressed();
276 return H_;
277 }
278};
279
285template<LieGroup X, Manifold U, std::size_t Nouts, typename F, typename Xl, typename Ul>
286class FlatInnerFun
287{
288private:
289 using BundleT = smooth::Bundle<Eigen::Vector<double, 1>, X, U>;
290
291 F f;
292 Xl xl;
293 Ul ul;
294
295 static constexpr auto Nx = Dof<X>;
296 static constexpr auto Nu = Dof<U>;
297 static constexpr auto Nvars = 1 + Nx + Nu;
298
299 using E = Tangent<X>;
300 using V = Tangent<U>;
301
302 static constexpr auto t_B = 0;
303 static constexpr auto x_B = t_B + 1;
304 static constexpr auto u_B = x_B + Nx;
305
306 Eigen::SparseMatrix<double> Joplus_ = smooth::d_exp_sparse_pattern<BundleT>;
307 Eigen::SparseMatrix<double> Hoplus_ = smooth::d2_exp_sparse_pattern<BundleT>;
308
309 Eigen::SparseMatrix<double> J_{Nouts, Nvars};
310 Eigen::SparseMatrix<double> H_{Nvars, Nouts * Nvars};
311
313 void update_joplus(const E & e, const V & v, const E & dxl, const V & dul)
314 {
315 dr_exp_sparse<BundleT>(Joplus_, (Tangent<BundleT>() << 1, e, v).finished());
316
317 block_write(Joplus_, x_B, t_B, Ad<X>(smooth::exp<X>(-e)) * dxl);
318 block_write(Joplus_, u_B, t_B, Ad<U>(smooth::exp<U>(-v)) * dul);
319
320 Joplus_.makeCompressed();
321 }
322
324 void update_hoplus(const E & e, const V & v, const E & dxl, const V & dul)
325 {
326 d2r_exp_sparse<BundleT>(Hoplus_, (Tangent<BundleT>() << 1, e, v).finished());
327
328 // d (Ad_X b) = -ad_(Ad_X b) * Ad_X
329 const TangentMap<X> Adexp_X = Ad<X>(smooth::exp<X>(-e));
330 const TangentMap<X> dAdexp_X = ad<X>(Adexp_X * dxl) * Adexp_X * dr_exp<X>(-e);
331 const TangentMap<U> Adexp_U = Ad<U>(smooth::exp<U>(-v));
332 const TangentMap<U> dAdexp_U = ad<U>(Adexp_U * dul) * Adexp_U * dr_exp<U>(-v);
333
334 for (auto nx = 0u; nx < Nx; ++nx) {
335 const auto b0 = Nvars * (x_B + nx);
336 block_write(Hoplus_, t_B, b0 + x_B, dAdexp_X.middleRows(nx, 1));
337 }
338 for (auto nu = 0u; nu < Nu; ++nu) {
339 const auto b0 = Nvars * (u_B + nu);
340 block_write(Hoplus_, t_B, b0 + u_B, dAdexp_U.middleRows(nu, 1));
341 }
342
343 Hoplus_.makeCompressed();
344 }
345
346public:
347 template<typename A1, typename A2, typename A3>
348 FlatInnerFun(A1 && a1, A2 && a2, A3 && a3)
349 : f(std::forward<A1>(a1)), xl(std::forward<A2>(a2)), ul(std::forward<A3>(a3))
350 {}
351
352 template<typename T>
353 Eigen::Vector<T, Nouts> operator()(const T & t, const CastT<T, E> & e, const CastT<T, V> & v) const
354 {
355 return f.template operator()<T>(t, rplus(xl(t), e), rplus(ul(t), v));
356 }
357
358 std::reference_wrapper<const Eigen::SparseMatrix<double>>
359 jacobian(double t, const E & e, const V & v) requires(diff::detail::diffable_order1<F, std::tuple<double, X, U>>)
360 {
361 const auto & [xlval, dxlval] = diff::dr<1>(xl, wrt(t));
362 const auto & [ulval, dulval] = diff::dr<1>(ul, wrt(t));
363 const auto & Jf = f.jacobian(t, rplus(xlval, e), rplus(ulval, v));
364
365 update_joplus(e, v, dxlval, dulval);
366
367 J_ = Jf * Joplus_;
368 J_.makeCompressed();
369 return J_;
370 }
371
372 std::reference_wrapper<const Eigen::SparseMatrix<double>>
373 hessian(double t, const E & e, const V & v) requires(diff::detail::diffable_order1<F, std::tuple<double, X, U>> &&
374 diff::detail::diffable_order2<F, std::tuple<double, X, U>>)
375 {
376 const auto & [xlval, dxlval] = diff::dr<1>(xl, wrt(t));
377 const auto & [ulval, dulval] = diff::dr<1>(ul, wrt(t));
378 const auto x = rplus(xlval, e);
379 const auto u = rplus(ulval, v);
380 const auto & Jf = f.jacobian(t, x, u);
381 const auto & Hf = f.hessian(t, x, u);
382
383 update_joplus(e, v, dxlval, dulval);
384 update_hoplus(e, v, dxlval, dulval);
385
386 set_zero(H_);
387 d2r_fog(H_, Jf, Hf, Joplus_, Hoplus_);
388 H_.makeCompressed();
389 return H_;
390 }
391};
392
398template<LieGroup X, Manifold U, std::size_t Nq, std::size_t Nouts, typename F, typename Xl>
399class FlatEndptFun
400{
401private:
402 using E = Tangent<X>;
403 using Q = Eigen::Vector<Scalar<X>, Nq>;
404 using BundleT = smooth::Bundle<Eigen::Vector<double, 1>, X, X, Q>;
405
406 F f;
407 Xl xl;
408
409 static constexpr auto Nx = Dof<X>;
410 static constexpr auto Nvars = 1 + 2 * Nx + Nq;
411
412 static constexpr auto tf_B = 0;
413 static constexpr auto x0_B = tf_B + 1;
414 static constexpr auto xf_B = x0_B + Nx;
415 static constexpr auto q_B = xf_B + Nx;
416
417 Eigen::SparseMatrix<double> Joplus_ = d_exp_sparse_pattern<BundleT>;
418 Eigen::SparseMatrix<double> Hoplus_ = d2_exp_sparse_pattern<BundleT>;
419
420 Eigen::SparseMatrix<double> J_{Nouts, Nvars};
421 Eigen::SparseMatrix<double> H_{Nvars, Nouts * Nvars};
422
424 void update_joplus(const E & e0, const E & ef, const E & dxlf)
425 {
426 dr_exp_sparse<BundleT>(Joplus_, (Tangent<BundleT>() << 1, e0, ef, Q::Ones()).finished());
427
428 block_write(Joplus_, xf_B, tf_B, Ad<X>(smooth::exp<X>(-ef)) * dxlf);
429
430 Joplus_.makeCompressed();
431 }
432
434 void update_hoplus(const E & e0, const E & ef, [[maybe_unused]] const E & dxlf)
435 {
436 d2r_exp_sparse<BundleT>(Hoplus_, (Tangent<BundleT>() << 1, e0, ef, Q::Ones()).finished());
437
438 // dr (Ad_X b)_X = -ad_{Ad_X b} Ad_X
439 const TangentMap<X> Adexp_f = Ad<X>(smooth::exp<X>(-ef));
440 const TangentMap<X> dAdexp_f = ad<X>(Adexp_f * dxlf) * Adexp_f * dr_exp<X>(-ef);
441
442 for (auto nx = 0u; nx < Nx; ++nx) {
443 const auto b0 = Nvars * (xf_B + nx);
444 block_write(Hoplus_, tf_B, b0 + xf_B, dAdexp_f.middleRows(nx, 1));
445 }
446
447 Hoplus_.makeCompressed();
448 }
449
450public:
451 template<typename A1, typename A2>
452 FlatEndptFun(A1 && a1, A2 && a2) : f(std::forward<A1>(a1)), xl(std::forward<A2>(a2))
453 {}
454
455 template<typename T>
456 auto operator()(const T & tf, const CastT<T, E> & e0, const CastT<T, E> & ef, const CastT<T, Q> & q) const
457 {
458 return f.template operator()<T>(tf, rplus(xl(T(0.)), e0), rplus(xl(tf), ef), q);
459 }
460
461 std::reference_wrapper<const Eigen::SparseMatrix<double>>
462 jacobian(double tf, const E & e0, const E & ef, const Q & q) requires(
463 diff::detail::diffable_order1<F, std::tuple<double, X, X, Q>>)
464 {
465 const auto & [xlfval, dxlfval] = diff::dr<1>(xl, wrt(tf));
466 const auto & Jf = f.jacobian(tf, rplus(xl(0.), e0), rplus(xlfval, ef), q);
467
468 update_joplus(e0, ef, dxlfval);
469
470 J_ = Jf * Joplus_;
471 J_.makeCompressed();
472 return J_;
473 }
474
475 std::reference_wrapper<const Eigen::SparseMatrix<double>>
476 hessian(double tf, const E & e0, const E & ef, const Q & q) requires(
477 diff::detail::diffable_order1<F, std::tuple<double, X, X, Q>> &&
478 diff::detail::diffable_order2<F, std::tuple<double, X, X, Q>>)
479 {
480 const auto & [xlfval, dxlfval] = diff::dr<1>(xl, wrt(tf));
481 const auto x0 = rplus(xl(0.), e0);
482 const auto xf = rplus(xlfval, ef);
483 const auto & Jf = f.jacobian(tf, x0, xf, q); // Nouts x Nx
484 const auto & Hf = f.hessian(tf, x0, xf, q); // Nx x (Nouts * Nx)
485
486 update_joplus(e0, ef, dxlfval);
487 update_hoplus(e0, ef, dxlfval);
488
489 set_zero(H_);
490 d2r_fog(H_, Jf, Hf, Joplus_, Hoplus_);
491 H_.makeCompressed();
492 return H_;
493 }
494};
495
496} // namespace detail
497// \endcond
498
513auto flatten_ocp(const OCPType auto & ocp, auto && xl, auto && ul)
514{
515 using ocp_t = std::decay_t<decltype(ocp)>;
516 using X = typename ocp_t::X;
517 using U = typename ocp_t::U;
518 using Xl = decltype(xl);
519 using Ul = decltype(ul);
520
521 static constexpr auto Nq = ocp_t::Nq;
522
523 return OCP<
524 Tangent<X>,
525 Tangent<U>,
526 detail::FlatEndptFun<X, U, Nq, 1, decltype(ocp.theta), Xl>,
527 detail::FlatDyn<X, U, decltype(ocp.f), Xl, Ul>,
528 detail::FlatInnerFun<X, U, Nq, decltype(ocp.g), Xl, Ul>,
529 detail::FlatInnerFun<X, U, ocp_t::Ncr, decltype(ocp.cr), Xl, Ul>,
530 detail::FlatEndptFun<X, U, Nq, ocp_t::Nce, decltype(ocp.ce), Xl>>{
531 .theta = detail::FlatEndptFun<X, U, Nq, 1, decltype(ocp.theta), Xl>{ocp.theta, xl},
532 .f = detail::FlatDyn<X, U, decltype(ocp.f), Xl, Ul>{ocp.f, xl, ul},
533 .g = detail::FlatInnerFun<X, U, Nq, decltype(ocp.g), Xl, Ul>{ocp.g, xl, ul},
534 .cr = detail::FlatInnerFun<X, U, ocp_t::Ncr, decltype(ocp.cr), Xl, Ul>{ocp.cr, xl, ul},
535 .crl = ocp.crl,
536 .cru = ocp.cru,
537 .ce = detail::FlatEndptFun<X, U, Nq, ocp_t::Nce, decltype(ocp.ce), Xl>{ocp.ce, xl},
538 .cel = ocp.cel,
539 .ceu = ocp.ceu,
540 };
541}
542
549template<LieGroup X, Manifold U>
550auto unflatten_ocpsol(const auto & flatsol, auto && xl_fun, auto && ul_fun)
551{
552 using ocpsol_t = std::decay_t<decltype(flatsol)>;
553
554 auto u_unflat = [ul_fun = std::forward<decltype(ul_fun)>(ul_fun), usol = flatsol.u](double t) -> U {
555 return rplus(ul_fun(t), usol(t));
556 };
557
558 auto x_unflat = [xl_fun = std::forward<decltype(xl_fun)>(xl_fun), xsol = flatsol.x](double t) -> X {
559 return rplus(xl_fun(t), xsol(t));
560 };
561
562 return OCPSolution<X, U, ocpsol_t::Nq, ocpsol_t::Ncr, ocpsol_t::Nce>{
563 .t0 = flatsol.t0,
564 .tf = flatsol.tf,
565 .Q = flatsol.Q,
566 .u = std::move(u_unflat),
567 .x = std::move(x_unflat),
568 .lambda_q = flatsol.lambda_q,
569 .lambda_ce = flatsol.lambda_ce,
570 .lambda_dyn = flatsol.lambda_dyn,
571 .lambda_cr = flatsol.lambda_cr,
572 };
573}
574
575} // namespace smooth::feedback
smooth::feedback::OCPType
Concept that is true for OCP specializations.
Definition: ocp.hpp:103
smooth::feedback::set_zero
void set_zero(MeshValue< Deriv > &mv)
Reset MeshValue to zeros.
Definition: mesh_function.hpp:80
ocp.hpp
Optimal control problem definition.
smooth::feedback::unflatten_ocpsol
auto unflatten_ocpsol(const auto &flatsol, auto &&xl_fun, auto &&ul_fun)
Unflatten a FlatOCPSolution.
Definition: ocp_flatten.hpp:550
smooth::feedback::flatten_ocp
auto flatten_ocp(const OCPType auto &ocp, auto &&xl, auto &&ul)
Flatten a LieGroup OCP by defining it in the tangent space around a trajectory.
Definition: ocp_flatten.hpp:513
sparse.hpp
Sparse matrix utilities.
smooth::feedback::d2r_fog
void d2r_fog(Eigen::SparseMatrix< double > &out, const S1 &Jf, const S2 &Hf, const S3 &Jg, const S4 &Hg, Eigen::Index r0=0, Eigen::Index c0=0)
(Right) Hessian of composed function .
Definition: sparse.hpp:213
smooth::feedback::block_write
void block_write(Eigen::SparseMatrix< double, Options > &dest, Eigen::Index row0, Eigen::Index col0, const Source &source, double scale=1, bool upper_only=false)
Write block into a sparse matrix.
Definition: sparse.hpp:70
smooth::feedback::block_add
void block_add(Eigen::SparseMatrix< double, Options > &dest, Eigen::Index row0, Eigen::Index col0, const Source &source, double scale=1, bool upper_only=false)
Add block into a sparse matrix.
Definition: sparse.hpp:35
smooth::feedback::OCPSolution
Solution to OCP.
Definition: ocp.hpp:115
smooth::feedback::OCPSolution::t0
double t0
Initial and final time.
Definition: ocp.hpp:130
smooth::feedback::OCP
Optimal control problem definition.
Definition: ocp.hpp:52
smooth::feedback::OCP::theta
Theta theta
Objective function .
Definition: ocp.hpp:79
Generated by doxygen 1.9.5