aligator  0.16.0
A versatile and efficient C++ library for real-time constrained trajectory optimization.
Loading...
Searching...
No Matches
cartesian-product.hpp
Go to the documentation of this file.
1#pragma once
2
5
6#include <type_traits>
7
8namespace aligator {
9
12template <typename _Scalar>
14public:
15 using Scalar = _Scalar;
16
19 using Base::ndx;
20 using Base::nx;
21
22 const Base &getComponent(std::size_t i) const { return *m_components[i]; }
23
24 inline std::size_t numComponents() const { return m_components.size(); }
25
26 template <class Concrete> inline void addComponent(const Concrete &c) {
27 static_assert(
28 std::is_base_of_v<Base, Concrete> ||
29 std::is_same_v<Concrete, xyz::polymorphic<Base>>,
30 "Input type should either be derived from ManifoldAbstractTpl or be "
31 "polymorphic<ManifoldAbstractTpl>.");
32 this->nx_ += _get_nx(c);
33 this->ndx_ += _get_ndx(c);
34 m_components.emplace_back(c);
35 }
36
37 template <class Concrete> inline void addComponent(Concrete &&c) {
38 static_assert(
39 std::is_base_of_v<Base, Concrete> ||
40 std::is_same_v<Concrete, xyz::polymorphic<Base>>,
41 "Input type should either be derived from ManifoldAbstractTpl or be "
42 "polymorphic<ManifoldAbstractTpl>.");
43 this->nx_ += _get_nx(c);
44 this->ndx_ += _get_ndx(c);
45 m_components.emplace_back(std::move(c));
46 }
47
48 inline void addComponent(const CartesianProductTpl &other) {
49 for (const auto &c : other.m_components) {
50 this->addComponent(c);
51 }
52 }
53
55 : Base(0, 0)
56 , m_components() {}
61
62 CartesianProductTpl(const std::vector<xyz::polymorphic<Base>> &components)
63 : Base(0, 0)
64 , m_components(components) {
65 this->_calc_dims();
66 }
67
68 CartesianProductTpl(std::vector<xyz::polymorphic<Base>> &&components)
69 : Base(0, 0)
70 , m_components(std::move(components)) {
71 this->_calc_dims();
72 }
73
74 CartesianProductTpl(std::initializer_list<xyz::polymorphic<Base>> components)
75 : Base(0, 0)
76 , m_components(components) {
77 this->_calc_dims();
78 }
79
80 CartesianProductTpl(const xyz::polymorphic<Base> &left,
81 const xyz::polymorphic<Base> &right)
82 : Base(left->nx() + right->nx(), left->ndx() + right->ndx())
83 , m_components{left, right} {}
84
85 bool isNormalized(const ConstVectorRef &x) const;
86
87 template <class VectorType, class U = std::remove_const_t<VectorType>>
88 std::vector<U> split_impl(VectorType &x) const;
89
90 template <class VectorType, class U = std::remove_const_t<VectorType>>
91 std::vector<U> split_vector_impl(VectorType &v) const;
92
93 [[nodiscard]] std::vector<VectorRef> split(VectorRef x) const {
94 return split_impl<VectorRef>(x);
95 }
96
97 [[nodiscard]] std::vector<ConstVectorRef>
98 split(const ConstVectorRef &x) const {
100 }
101
102 [[nodiscard]] std::vector<VectorRef> split_vector(VectorRef v) const {
104 }
105
106 [[nodiscard]] std::vector<ConstVectorRef>
107 split_vector(const ConstVectorRef &v) const {
109 }
110
111 [[nodiscard]] VectorXs merge(const std::vector<VectorXs> &xs) const;
112
113 [[nodiscard]] VectorXs merge_vector(const std::vector<VectorXs> &vs) const;
114
115protected:
116 std::vector<xyz::polymorphic<Base>> m_components;
117
118 void _calc_dims() {
119 this->nx_ = 0u;
120 this->ndx_ = 0u;
121 for (const auto &c : m_components) {
122 this->nx_ += c->nx();
123 this->ndx_ += c->ndx();
124 }
125 }
126
127 template <class Concrete> static int _get_nx(const Concrete &c) {
128 return c.nx();
129 }
130 template <class Concrete> static int _get_ndx(const Concrete &c) {
131 return c.ndx();
132 }
133 static int _get_nx(const xyz::polymorphic<Base> &c) { return c->nx(); }
134 static int _get_ndx(const xyz::polymorphic<Base> &c) { return c->ndx(); }
135
136 void neutral_impl(VectorRef out) const;
137
138 void rand_impl(VectorRef out) const;
139
140 void integrate_impl(const ConstVectorRef &x, const ConstVectorRef &v,
141 VectorRef out) const;
142
143 void difference_impl(const ConstVectorRef &x0, const ConstVectorRef &x1,
144 VectorRef out) const;
145
146 void Jintegrate_impl(const ConstVectorRef &x, const ConstVectorRef &v,
147 MatrixRef Jout, int arg) const;
148
149 void JintegrateTransport_impl(const ConstVectorRef &x,
150 const ConstVectorRef &v, MatrixRef Jout,
151 int arg) const;
152
153 void Jdifference_impl(const ConstVectorRef &x0, const ConstVectorRef &x1,
154 MatrixRef Jout, int arg) const;
155};
156
157template <typename T>
158auto operator*(const xyz::polymorphic<ManifoldAbstractTpl<T>> &left,
159 const xyz::polymorphic<ManifoldAbstractTpl<T>> &right) {
160 return CartesianProductTpl<T>(left, right);
161}
162
163template <typename T>
165 const xyz::polymorphic<ManifoldAbstractTpl<T>> &right) {
166 CartesianProductTpl<T> out(left);
167 out.addComponent(right);
168 return out;
169}
170
171template <typename T>
172auto operator*(const xyz::polymorphic<ManifoldAbstractTpl<T>> &left,
173 const CartesianProductTpl<T> &right) {
174 return right * left;
175}
176
177template <typename T>
179 const CartesianProductTpl<T> &right) {
180 CartesianProductTpl<T> out{left};
181 out.addComponent(right);
182 return out;
183}
184
185} // namespace aligator
186
187// implementation details
188#include "aligator/modelling/spaces/cartesian-product.hxx"
Main package namespace.
xyz::polymorphic< CostStackTpl< T > > operator*(T u, xyz::polymorphic< CostStackTpl< T > > &&c1)
The cartesian product of two or more manifolds.
static int _get_ndx(const xyz::polymorphic< Base > &c)
CartesianProductTpl(const std::vector< xyz::polymorphic< Base > > &components)
CartesianProductTpl(const CartesianProductTpl &)=default
void Jintegrate_impl(const ConstVectorRef &x, const ConstVectorRef &v, MatrixRef Jout, int arg) const
const Base & getComponent(std::size_t i) const
std::vector< U > split_impl(VectorType &x) const
std::vector< VectorRef > split_vector(VectorRef v) const
std::vector< VectorRef > split(VectorRef x) const
static int _get_nx(const Concrete &c)
static int _get_ndx(const Concrete &c)
void neutral_impl(VectorRef out) const
void rand_impl(VectorRef out) const
CartesianProductTpl(std::vector< xyz::polymorphic< Base > > &&components)
CartesianProductTpl & operator=(CartesianProductTpl &&)=default
VectorXs merge(const std::vector< VectorXs > &xs) const
bool isNormalized(const ConstVectorRef &x) const
Check if the input vector x is a viable element of the manifold.
static int _get_nx(const xyz::polymorphic< Base > &c)
void difference_impl(const ConstVectorRef &x0, const ConstVectorRef &x1, VectorRef out) const
Implementation of the manifold retraction operation.
VectorXs merge_vector(const std::vector< VectorXs > &vs) const
std::vector< ConstVectorRef > split(const ConstVectorRef &x) const
CartesianProductTpl & operator=(const CartesianProductTpl &)=default
void integrate_impl(const ConstVectorRef &x, const ConstVectorRef &v, VectorRef out) const
Perform the manifold integration operation.
void Jdifference_impl(const ConstVectorRef &x0, const ConstVectorRef &x1, MatrixRef Jout, int arg) const
void JintegrateTransport_impl(const ConstVectorRef &x, const ConstVectorRef &v, MatrixRef Jout, int arg) const
CartesianProductTpl(const xyz::polymorphic< Base > &left, const xyz::polymorphic< Base > &right)
CartesianProductTpl(std::initializer_list< xyz::polymorphic< Base > > components)
void addComponent(const Concrete &c)
std::vector< ConstVectorRef > split_vector(const ConstVectorRef &v) const
void addComponent(const CartesianProductTpl &other)
CartesianProductTpl(CartesianProductTpl &&)=default
std::vector< U > split_vector_impl(VectorType &v) const
std::vector< xyz::polymorphic< Base > > m_components
Base class for manifolds, to use in cost funcs, solvers...
ManifoldAbstractTpl(int nx, int ndx)