aligator  0.15.0
A versatile and efficient C++ library for real-time constrained trajectory optimization.
Loading...
Searching...
No Matches
riccati-kernel.hpp
Go to the documentation of this file.
1
3#pragma once
4
6#include "lqr-problem.hpp"
7#include "blk-matrix.hpp"
9
10#include <boost/core/make_span.hpp>
11
12#include <optional>
13
14namespace aligator {
15namespace gar {
16
18template <class T, class A>
19inline boost::span<T> make_span_from_indices(std::vector<T, A> &vec, size_t i0,
20 size_t i1) {
21 return boost::make_span(vec.data() + i0, i1 - i0);
22}
23
25template <class T, class A>
26inline boost::span<const T> make_span_from_indices(const std::vector<T, A> &vec,
27 size_t i0, size_t i1) {
28 return boost::make_span(vec.data() + i0, i1 - i0);
29}
30
32template <typename _Scalar> struct StageFactor {
33 using Scalar = _Scalar;
36
37 struct CostToGo {
39 ArenaMatrix<MatrixXs> Vxx; //< "cost-to-go" matrix
40 ArenaMatrix<VectorXs> vx; //< "cost-to-go" gradient
41 ArenaMatrix<MatrixXs> Vxt; //< cross-Hessian
42 ArenaMatrix<MatrixXs> Vtt; //< parametric Hessian
43 ArenaMatrix<VectorXs> vt; //< parametric vector
44
45 CostToGo(uint nx, uint nth, const allocator_type &alloc = {})
46 : Vxx(nx, nx, alloc)
47 , vx(nx, alloc)
48 , Vxt(nx, nth, alloc)
49 , Vtt(nth, nth, alloc)
50 , vt(nth, alloc) {
51 Vxt.setZero();
52 Vtt.setZero();
53 vt.setZero();
54 }
55
56 allocator_type get_allocator() const { return Vxx.get_allocator(); }
57
58 CostToGo(const CostToGo &other, const allocator_type &alloc = {})
59 : Vxx(other.Vxx, alloc)
60 , vx(other.vx, alloc)
61 , Vxt(other.Vxt, alloc)
62 , Vtt(other.Vtt, alloc)
63 , vt(other.vt, alloc) {}
64 CostToGo(CostToGo &&other) noexcept = default;
65 CostToGo(CostToGo &&other, const allocator_type &alloc)
66 : Vxx(std::move(other.Vxx), alloc)
67 , vx(std::move(other.vx), alloc)
68 , Vxt(std::move(other.Vxt), alloc)
69 , Vtt(std::move(other.Vtt), alloc)
70 , vt(std::move(other.vt), alloc) {}
71 CostToGo &operator=(const CostToGo &) = default;
72 CostToGo &operator=(CostToGo &&) = default;
73 };
74
76 const allocator_type &alloc = {});
77
78 allocator_type get_allocator() const { return Qhat.get_allocator(); }
79
80 StageFactor(const StageFactor &other, const allocator_type &alloc = {});
81 StageFactor(StageFactor &&) noexcept = default;
82 StageFactor(StageFactor &&other, const allocator_type &alloc);
83
84 StageFactor &operator=(const StageFactor &) = default;
85 StageFactor &operator=(StageFactor &&) = default;
86 ~StageFactor() = default;
87
89 ArenaMatrix<MatrixXs> Qhat;
90 ArenaMatrix<MatrixXs> Rhat;
91 ArenaMatrix<MatrixXs> Shat;
92 ArenaMatrix<VectorXs> qhat;
93 ArenaMatrix<VectorXs> rhat;
94 ArenaMatrix<RowMatrixXs> AtV;
95 ArenaMatrix<RowMatrixXs> BtV;
96 ArenaMatrix<MatrixXs> Gxhat;
97 ArenaMatrix<MatrixXs> Guhat;
98 BlkMatrix<VectorXs, 3, 1> ff; //< feedforward gains
99 BlkMatrix<RowMatrixXs, 3, 1> fb; //< feedback gains
100 BlkMatrix<RowMatrixXs, 3, 1> fth; //< parameter feedback gains
101 BlkMatrix<MatrixXs, 2, 2> kktMat; //< reduced KKT matrix buffer
102 BunchKaufman<MatrixXs> kktChol; //< reduced KKT LDLT solver
103 CostToGo vm; //< cost-to-go parameters
104};
105
108template <typename Scalar> struct ProximalRiccatiKernel {
112 using CostToGo = typename StageFactorType::CostToGo;
113
114 struct kkt0_t {
119 kkt0_t(uint nx, uint nc, uint nth)
120 : mat({nx, nc}, {nx, nc})
121 , ff(mat.rowDims())
122 , fth(mat.rowDims(), {nth}) {}
123 };
124
125 static void terminalSolve(const KnotType &model, const Scalar mueq,
126 StageFactorType &d);
127
128 static bool backwardImpl(boost::span<const KnotType> stages,
129 const Scalar mueq,
130 boost::span<StageFactorType> datas);
131
133 static void computeInitial(VectorRef x0, VectorRef lbd0, const kkt0_t &kkt0,
134 const std::optional<ConstVectorRef> &theta_);
135
136 static void stageKernelSolve(const KnotType &model, StageFactorType &d,
137 CostToGo &vn, const Scalar mueq);
138
140 static bool
141 forwardImpl(boost::span<const KnotType> stages,
142 boost::span<const StageFactorType> datas,
143 boost::span<VectorXs> xs, boost::span<VectorXs> us,
144 boost::span<VectorXs> vs, boost::span<VectorXs> lbdas,
145 const std::optional<ConstVectorRef> &theta_ = std::nullopt);
146};
147
148#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
149extern template struct StageFactor<context::Scalar>;
150extern template struct ProximalRiccatiKernel<context::Scalar>;
151#endif
152
153} // namespace gar
154} // namespace aligator
ArenaMatrix & setZero(Index newSize)
Block matrix class, with a fixed or dynamic-size number of row and column blocks.
const RowDimsType & rowDims() const
A convenience subclass of std::pmr::polymorphic_allocator for bytes.
Definition allocator.hpp:16
boost::span< T > make_span_from_indices(std::vector< T, A > &vec, size_t i0, size_t i1)
Create a boost::span object from a vector and two indices.
Main package namespace.
unsigned int uint
Definition logger.hpp:10
Struct describing a stage of a constrained LQ problem.
Kernel for use in Riccati-like algorithms for the proximal LQ subproblem.
static void terminalSolve(const KnotType &model, const Scalar mueq, StageFactorType &d)
static bool forwardImpl(boost::span< const KnotType > stages, boost::span< const StageFactorType > datas, boost::span< VectorXs > xs, boost::span< VectorXs > us, boost::span< VectorXs > vs, boost::span< VectorXs > lbdas, const std::optional< ConstVectorRef > &theta_=std::nullopt)
Forward sweep.
static bool backwardImpl(boost::span< const KnotType > stages, const Scalar mueq, boost::span< StageFactorType > datas)
static void stageKernelSolve(const KnotType &model, StageFactorType &d, CostToGo &vn, const Scalar mueq)
static void computeInitial(VectorRef x0, VectorRef lbd0, const kkt0_t &kkt0, const std::optional< ConstVectorRef > &theta_)
Solve initial stage.
typename StageFactorType::CostToGo CostToGo
CostToGo & operator=(CostToGo &&)=default
CostToGo(CostToGo &&other) noexcept=default
CostToGo(CostToGo &&other, const allocator_type &alloc)
::aligator::polymorphic_allocator allocator_type
CostToGo & operator=(const CostToGo &)=default
CostToGo(const CostToGo &other, const allocator_type &alloc={})
CostToGo(uint nx, uint nth, const allocator_type &alloc={})
Per-node struct for all computations in the factorization.
StageFactor(const StageFactor &other, const allocator_type &alloc={})
BlkMatrix< RowMatrixXs, 3, 1 > fb
BlkMatrix< RowMatrixXs, 3, 1 > fth
BlkMatrix< VectorXs, 3, 1 > ff
allocator_type get_allocator() const
ALIGATOR_DYNAMIC_TYPEDEFS_WITH_ROW_TYPES(Scalar)
StageFactor(StageFactor &&) noexcept=default
BlkMatrix< MatrixXs, 2, 2 > kktMat
::aligator::polymorphic_allocator allocator_type
StageFactor(uint nx, uint nu, uint nc, uint nx2, uint nth, const allocator_type &alloc={})