aligator  0.16.0
A versatile and efficient C++ library for real-time constrained trajectory optimization.
Loading...
Searching...
No Matches
lqr-problem.hpp
Go to the documentation of this file.
1
2#pragma once
3
7
8#include <fmt/format.h>
9#include <optional>
10
11namespace aligator {
12namespace gar {
13
34template <typename Scalar> struct LqrKnotTpl {
36 static constexpr int Alignment = Eigen::AlignedMax;
38
44
51
52 ArenaMatrix<MatrixXs> Gth; //< \f$\theta^\top G_\theta\theta\f$ term
53 ArenaMatrix<MatrixXs> Gx; //< \f$x^\top G_x \theta\f$ term in Lagrangian
54 ArenaMatrix<MatrixXs> Gu; //< \f$u^\top G_x \theta\f$ term in Lagrangian
55 ArenaMatrix<MatrixXs> Gv; //< \f$\nu^\top G_x \theta\f$ term in Lagrangian
56 ArenaMatrix<VectorXs> gamma; //< \f$\gamma^\top \theta\f$ term in Lagrangian
57
58 LqrKnotTpl() = default;
59 explicit LqrKnotTpl(const allocator_type &alloc);
60
62 allocator_type alloc = {});
63
66 : LqrKnotTpl(nx, nu, nc, nx2, 0, alloc) {}
67
70 : LqrKnotTpl(nx, nu, nc, nx, 0, alloc) {}
71
73 LqrKnotTpl(const LqrKnotTpl &other, allocator_type alloc = {});
76 LqrKnotTpl(LqrKnotTpl &&other) noexcept;
78 LqrKnotTpl(LqrKnotTpl &&other, const allocator_type &alloc);
83
84 ~LqrKnotTpl() = default;
85
87 void assign(const LqrKnotTpl<Scalar> &other);
88
89 // reallocates entire buffer for contigousness
91
92 bool isApprox(const LqrKnotTpl &other,
93 Scalar prec = std::numeric_limits<Scalar>::epsilon()) const;
94
95 friend bool operator==(const LqrKnotTpl &lhs, const LqrKnotTpl &rhs) {
96 return lhs.isApprox(rhs);
97 }
98
99 allocator_type get_allocator() const { return m_allocator; }
100
101private:
102 allocator_type m_allocator;
103};
104
105template <typename Scalar> struct LqrProblemTpl {
107 static constexpr int Alignment = Eigen::AlignedMax;
109 using KnotVector = std::pmr::vector<KnotType>;
114
115 inline int horizon() const noexcept { return (int)stages.size() - 1; }
117 inline uint nc0() const noexcept { return (uint)g0.rows(); }
118
119 explicit LqrProblemTpl(allocator_type alloc = {})
120 : G0(alloc)
121 , g0(alloc)
122 , stages(alloc) {
123 assert(check_allocators());
124 }
125
127 LqrProblemTpl(const KnotVector &knots, long nc0, allocator_type alloc = {});
131
134 : LqrProblemTpl(other.stages, other.nc0(), alloc) {
135 this->G0 = other.G0;
136 this->g0 = other.g0;
137 }
138
141 : LqrProblemTpl(std::move(other.stages), other.nc0()) {
142 this->G0 = other.G0;
143 this->g0 = other.g0;
144 }
145
147 this->G0 = std::move(other.G0);
148 this->g0 = std::move(other.g0);
149 this->stages = std::move(other.stages);
150 return *this;
151 }
152
153 ~LqrProblemTpl() = default;
154
156 if (stages.empty())
157 return;
158 for (uint i = 0; i <= (uint)horizon(); i++) {
159 stages[i].addParameterization(nth);
160 }
161 }
162
163 inline bool isParameterized() const {
164 return !stages.empty() && (stages[0].nth > 0);
165 }
166
167 inline bool isInitialized() const { return !stages.empty(); }
168
169 inline uint ntheta() const { return stages[0].nth; }
170
171 [[nodiscard]] bool isApprox(const LqrProblemTpl &other) {
172 if (horizon() != other.horizon() || !G0.isApprox(other.G0) ||
173 !g0.isApprox(other.g0))
174 return false;
175 for (uint i = 0; i < uint(horizon()); i++) {
176 if (!stages[i].isApprox(other.stages[i]))
177 return false;
178 }
179 return true;
180 }
181
183 [[nodiscard]] Scalar
184 evaluate(const VectorOfVectors &xs, const VectorOfVectors &us,
185 const std::optional<ConstVectorRef> &theta) const;
186
187 allocator_type get_allocator() const { return G0.get_allocator(); }
188
189private:
191 [[nodiscard]] bool check_allocators() const {
192 return get_allocator() == g0.get_allocator() &&
193 get_allocator() == stages.get_allocator();
194 }
195};
196
197template <typename Scalar>
198[[nodiscard]] bool lqrKnotsSameDim(const LqrKnotTpl<Scalar> &lhs,
199 const LqrKnotTpl<Scalar> &rhs) {
200 return (lhs.nx == rhs.nx) && (lhs.nu == rhs.nu) && (lhs.nc == rhs.nc) &&
201 (lhs.nx2 == rhs.nx2) && (lhs.nth == rhs.nth);
202}
203
204template <typename Scalar>
205std::ostream &operator<<(std::ostream &oss, const LqrKnotTpl<Scalar> &self) {
206 oss << "LqrKnot {";
207 oss << fmt::format("\n nx: {:d}", self.nx) //
208 << fmt::format("\n nu: {:d}", self.nu) //
209 << fmt::format("\n nc: {:d}", self.nc);
210 if (self.nth > 0) {
211 oss << fmt::format("\n nth: {:d}", self.nth);
212 }
213#ifndef NDEBUG
214 oss << eigenPrintWithPreamble(self.Q, "\n Q: ") //
215 << eigenPrintWithPreamble(self.S, "\n S: ") //
216 << eigenPrintWithPreamble(self.R, "\n R: ") //
217 << eigenPrintWithPreamble(self.q, "\n q: ") //
218 << eigenPrintWithPreamble(self.r, "\n r: ");
219
220 oss << eigenPrintWithPreamble(self.A, "\n A: ") //
221 << eigenPrintWithPreamble(self.B, "\n B: ") //
222 << eigenPrintWithPreamble(self.f, "\n f: ");
223
224 oss << eigenPrintWithPreamble(self.C, "\n C: ") //
225 << eigenPrintWithPreamble(self.D, "\n D: ") //
226 << eigenPrintWithPreamble(self.d, "\n d: ");
227 if (self.nth > 0) {
228 oss << eigenPrintWithPreamble(self.Gth, "\n Gth: ") //
229 << eigenPrintWithPreamble(self.Gx, "\n Gx: ") //
230 << eigenPrintWithPreamble(self.Gu, "\n Gu: ") //
231 << eigenPrintWithPreamble(self.gamma, "\n gamma: ");
232 }
233#endif
234 oss << "\n}";
235 return oss;
236}
237
238#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
239extern template struct LqrKnotTpl<context::Scalar>;
240extern template struct LqrProblemTpl<context::Scalar>;
241#endif
242
243} // namespace gar
244} // namespace aligator
allocator_type get_allocator() const noexcept
A convenience subclass of std::pmr::polymorphic_allocator for bytes.
Definition allocator.hpp:16
std::ostream & operator<<(std::ostream &oss, const LqrKnotTpl< Scalar > &self)
bool lqrKnotsSameDim(const LqrKnotTpl< Scalar > &lhs, const LqrKnotTpl< Scalar > &rhs)
Main package namespace.
auto eigenPrintWithPreamble(const Eigen::EigenBase< D > &mat, std::string_view text, Eigen::IOFormat ft=EIGEN_DEFAULT_IO_FORMAT)
Definition math.hpp:131
unsigned int uint
Definition logger.hpp:11
Struct describing a stage of a constrained LQ problem.
LqrKnotTpl(LqrKnotTpl &&other) noexcept
Move constructor. Allocator will be moved from other. Other will be have m_empty_after_move set to tr...
allocator_type get_allocator() const
LqrKnotTpl(LqrKnotTpl &&other, const allocator_type &alloc)
Extended move constructor.
void assign(const LqrKnotTpl< Scalar > &other)
Assign matrices (and dimensions) from another LqrKnotTpl.
LqrKnotTpl(uint nx, uint nu, uint nc, uint nx2, uint nth, allocator_type alloc={})
friend bool operator==(const LqrKnotTpl &lhs, const LqrKnotTpl &rhs)
LqrKnotTpl(uint nx, uint nu, uint nc, allocator_type alloc={})
Delegating constructor, assumes nx2 = nx, and nth = 0.
LqrKnotTpl(const allocator_type &alloc)
bool isApprox(const LqrKnotTpl &other, Scalar prec=std::numeric_limits< Scalar >::epsilon()) const
LqrKnotTpl & operator=(LqrKnotTpl &&)
Move assignment. Other allocator will be stolen.
LqrKnotTpl & addParameterization(uint nth)
LqrKnotTpl(const LqrKnotTpl &other, allocator_type alloc={})
Copy constructor. Allocator must be given.
polymorphic_allocator allocator_type
LqrKnotTpl & operator=(const LqrKnotTpl &other)
Copy assignment. Current allocator will be reused if required.
LqrKnotTpl(uint nx, uint nu, uint nc, uint nx2, allocator_type alloc={})
Delegating constructor, assumes nth = 0.
LqrProblemTpl(const LqrProblemTpl &other, allocator_type alloc={})
Copy constructor. Will copy the allocator from other.
std::pmr::vector< KnotType > KnotVector
allocator_type get_allocator() const
LqrProblemTpl(allocator_type alloc={})
LqrProblemTpl & operator=(LqrProblemTpl &&other)
LqrKnotTpl< Scalar > KnotType
Scalar evaluate(const VectorOfVectors &xs, const VectorOfVectors &us, const std::optional< ConstVectorRef > &theta) const
Evaluate the quadratic objective.
polymorphic_allocator allocator_type
LqrProblemTpl(const KnotVector &knots, long nc0, allocator_type alloc={})
This constructor will take the knots as-is.
LqrProblemTpl(LqrProblemTpl &&other)
Move constructor - we steal the allocator from the source object.
int horizon() const noexcept
uint nc0() const noexcept
Dimension of the initial condition constraint.
bool isApprox(const LqrProblemTpl &other)
LqrProblemTpl(KnotVector &&knots, long nc0)
This constructor will take the knots as-is, copying their specified allocator.