aligator  0.15.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
5#include "aligator/math.hpp"
8#include <fmt/format.h>
9
10#include <optional>
11
12namespace aligator {
13namespace gar {
14
35template <typename Scalar> struct LqrKnotTpl {
37 static constexpr int Alignment = Eigen::AlignedMax;
39
45
52
53 ArenaMatrix<MatrixXs> Gth; //< \f$\theta^\top G_\theta\theta\f$ term
54 ArenaMatrix<MatrixXs> Gx; //< \f$x^\top G_x \theta\f$ term in Lagrangian
55 ArenaMatrix<MatrixXs> Gu; //< \f$u^\top G_x \theta\f$ term in Lagrangian
56 ArenaMatrix<MatrixXs> Gv; //< \f$\nu^\top G_x \theta\f$ term in Lagrangian
57 ArenaMatrix<VectorXs> gamma; //< \f$\gamma^\top \theta\f$ term in Lagrangian
58
59 LqrKnotTpl() = default;
60 explicit LqrKnotTpl(const allocator_type &alloc);
61
63 allocator_type alloc = {});
64
67 : LqrKnotTpl(nx, nu, nc, nx2, 0, alloc) {}
68
71 : LqrKnotTpl(nx, nu, nc, nx, 0, alloc) {}
72
74 LqrKnotTpl(const LqrKnotTpl &other, allocator_type alloc = {});
77 LqrKnotTpl(LqrKnotTpl &&other) noexcept;
79 LqrKnotTpl(LqrKnotTpl &&other, const allocator_type &alloc);
84
85 ~LqrKnotTpl() = default;
86
88 void assign(const LqrKnotTpl<Scalar> &other);
89
90 // reallocates entire buffer for contigousness
92
93 bool isApprox(const LqrKnotTpl &other,
94 Scalar prec = std::numeric_limits<Scalar>::epsilon()) const;
95
96 friend bool operator==(const LqrKnotTpl &lhs, const LqrKnotTpl &rhs) {
97 return lhs.isApprox(rhs);
98 }
99
100 allocator_type get_allocator() const { return m_allocator; }
101
102private:
103 allocator_type m_allocator;
104};
105
106template <typename Scalar> struct LqrProblemTpl {
108 static constexpr int Alignment = Eigen::AlignedMax;
110 using KnotVector = std::pmr::vector<KnotType>;
115
116 inline int horizon() const noexcept { return (int)stages.size() - 1; }
118 inline uint nc0() const noexcept { return (uint)g0.rows(); }
119
120 explicit LqrProblemTpl(allocator_type alloc = {})
121 : G0(alloc)
122 , g0(alloc)
123 , stages(alloc) {
124 assert(check_allocators());
125 }
126
128 LqrProblemTpl(const KnotVector &knots, long nc0, allocator_type alloc = {});
132
135 : LqrProblemTpl(other.stages, other.nc0(), alloc) {
136 this->G0 = other.G0;
137 this->g0 = other.g0;
138 }
139
142 : LqrProblemTpl(std::move(other.stages), other.nc0()) {
143 this->G0 = other.G0;
144 this->g0 = other.g0;
145 }
146
148 this->G0 = std::move(other.G0);
149 this->g0 = std::move(other.g0);
150 this->stages = std::move(other.stages);
151 return *this;
152 }
153
154 ~LqrProblemTpl() = default;
155
157 if (stages.empty())
158 return;
159 for (uint i = 0; i <= (uint)horizon(); i++) {
160 stages[i].addParameterization(nth);
161 }
162 }
163
164 inline bool isParameterized() const {
165 return !stages.empty() && (stages[0].nth > 0);
166 }
167
168 inline bool isInitialized() const { return !stages.empty(); }
169
170 inline uint ntheta() const { return stages[0].nth; }
171
172 [[nodiscard]] bool isApprox(const LqrProblemTpl &other) {
173 if (horizon() != other.horizon() || !G0.isApprox(other.G0) ||
174 !g0.isApprox(other.g0))
175 return false;
176 for (uint i = 0; i < uint(horizon()); i++) {
177 if (!stages[i].isApprox(other.stages[i]))
178 return false;
179 }
180 return true;
181 }
182
184 [[nodiscard]] Scalar
185 evaluate(const VectorOfVectors &xs, const VectorOfVectors &us,
186 const std::optional<ConstVectorRef> &theta) const;
187
188 allocator_type get_allocator() const { return G0.get_allocator(); }
189
190private:
192 [[nodiscard]] bool check_allocators() const {
193 return get_allocator() == g0.get_allocator() &&
194 get_allocator() == stages.get_allocator();
195 }
196};
197
198template <typename Scalar>
199[[nodiscard]] bool lqrKnotsSameDim(const LqrKnotTpl<Scalar> &lhs,
200 const LqrKnotTpl<Scalar> &rhs) {
201 return (lhs.nx == rhs.nx) && (lhs.nu == rhs.nu) && (lhs.nc == rhs.nc) &&
202 (lhs.nx2 == rhs.nx2) && (lhs.nth == rhs.nth);
203}
204
205template <typename Scalar>
206std::ostream &operator<<(std::ostream &oss, const LqrKnotTpl<Scalar> &self) {
207 oss << "LqrKnot {";
208 oss << fmt::format("\n nx: {:d}", self.nx) //
209 << fmt::format("\n nu: {:d}", self.nu) //
210 << fmt::format("\n nc: {:d}", self.nc);
211 if (self.nth > 0) {
212 oss << fmt::format("\n nth: {:d}", self.nth);
213 }
214#ifndef NDEBUG
215 oss << eigenPrintWithPreamble(self.Q, "\n Q: ") //
216 << eigenPrintWithPreamble(self.S, "\n S: ") //
217 << eigenPrintWithPreamble(self.R, "\n R: ") //
218 << eigenPrintWithPreamble(self.q, "\n q: ") //
219 << eigenPrintWithPreamble(self.r, "\n r: ");
220
221 oss << eigenPrintWithPreamble(self.A, "\n A: ") //
222 << eigenPrintWithPreamble(self.B, "\n B: ") //
223 << eigenPrintWithPreamble(self.f, "\n f: ");
224
225 oss << eigenPrintWithPreamble(self.C, "\n C: ") //
226 << eigenPrintWithPreamble(self.D, "\n D: ") //
227 << eigenPrintWithPreamble(self.d, "\n d: ");
228 if (self.nth > 0) {
229 oss << eigenPrintWithPreamble(self.Gth, "\n Gth: ") //
230 << eigenPrintWithPreamble(self.Gx, "\n Gx: ") //
231 << eigenPrintWithPreamble(self.Gu, "\n Gu: ") //
232 << eigenPrintWithPreamble(self.gamma, "\n gamma: ");
233 }
234#endif
235 oss << "\n}";
236 return oss;
237}
238
239#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
240extern template struct LqrKnotTpl<context::Scalar>;
241extern template struct LqrProblemTpl<context::Scalar>;
242#endif
243
244} // namespace gar
245} // namespace aligator
allocator_type get_allocator() const noexcept
A convenience subclass of std::pmr::polymorphic_allocator for bytes.
Definition allocator.hpp:16
Math utilities.
std::ostream & operator<<(std::ostream &oss, const LqrKnotTpl< Scalar > &self)
bool lqrKnotsSameDim(const LqrKnotTpl< Scalar > &lhs, const LqrKnotTpl< Scalar > &rhs)
Main package namespace.
unsigned int uint
Definition logger.hpp:10
auto eigenPrintWithPreamble(const Eigen::EigenBase< D > &mat, const std::string &text, Eigen::IOFormat ft=EIGEN_DEFAULT_IO_FORMAT)
Definition math.hpp:131
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.