aligator  0.16.0
A versatile and efficient C++ library for real-time constrained trajectory optimization.
Loading...
Searching...
No Matches
manifold-base.hpp
Go to the documentation of this file.
1
3#pragma once
4
5#include "aligator/fwd.hpp"
6
7namespace aligator {
8
11template <typename _Scalar> struct ManifoldAbstractTpl {
12public:
13 using Scalar = _Scalar;
17
20 , ndx_(ndx) {}
22 virtual ~ManifoldAbstractTpl() = default;
23
24 /// @brief Get manifold representation dimension.
25 inline int nx() const { return nx_; }
27 inline int ndx() const { return ndx_; }
29 /// @brief Get the neutral element \f$e \in M\f$ from the manifold (if this
30 /// makes sense).
31 [[nodiscard]] VectorXs neutral() const {
32 VectorXs out(nx());
33 neutral_impl(out);
34 return out;
35 }
36
38 void neutral(VectorRef out) const { neutral_impl(out); }
39
41 [[nodiscard]] VectorXs rand() const {
42 VectorXs out(nx());
43 rand_impl(out);
44 return out;
45 }
46
48 void rand(VectorRef out) const { rand_impl(out); }
49
52 virtual bool isNormalized(const ConstVectorRef & /*x*/) const { return true; }
53
56 return TangentSpaceType(this->ndx());
57 }
58
60
62 void integrate(const ConstVectorRef &x, const ConstVectorRef &v,
63 VectorRef out) const;
64
66 void Jintegrate(const ConstVectorRef &x, const ConstVectorRef &v,
67 MatrixRef Jout, int arg) const;
68
71 void JintegrateTransport(const ConstVectorRef &x, const ConstVectorRef &v,
72 MatrixRef Jout, int arg) const {
73 assert(Jout.rows() == v.size());
74 JintegrateTransport_impl(x, v, Jout, arg);
75 }
76
78 void difference(const ConstVectorRef &x0, const ConstVectorRef &x1,
79 VectorRef out) const;
80
82 void Jdifference(const ConstVectorRef &x0, const ConstVectorRef &x1,
83 MatrixRef Jout, int arg) const;
84
85 void interpolate(const ConstVectorRef &x0, const ConstVectorRef &x1,
86 const Scalar &u, VectorRef out) const;
87
90
94 [[nodiscard]] VectorXs integrate(const ConstVectorRef &x,
95 const ConstVectorRef &v) const {
96 VectorXs out(nx());
97 integrate_impl(x, v, out);
98 return out;
99 }
100
104 [[nodiscard]] VectorXs difference(const ConstVectorRef &x0,
105 const ConstVectorRef &x1) const {
106 VectorXs out(ndx());
107 difference_impl(x0, x1, out);
108 return out;
109 }
110
112 [[nodiscard]] VectorXs interpolate(const ConstVectorRef &x0,
113 const ConstVectorRef &x1,
114 const Scalar &u) const {
115 VectorXs out(nx());
116 interpolate_impl(x0, x1, u, out);
117 return out;
118 }
119
121
122protected:
123 int nx_;
124 int ndx_;
125
127 virtual void integrate_impl(const ConstVectorRef &x, const ConstVectorRef &v,
128 VectorRef out) const = 0;
129
130 virtual void Jintegrate_impl(const ConstVectorRef &x, const ConstVectorRef &v,
131 MatrixRef Jout, int arg) const = 0;
132
133 virtual void JintegrateTransport_impl(const ConstVectorRef &x,
134 const ConstVectorRef &v, MatrixRef Jout,
135 int arg) const = 0;
136
138 virtual void difference_impl(const ConstVectorRef &x0,
139 const ConstVectorRef &x1,
140 VectorRef out) const = 0;
141
142 virtual void Jdifference_impl(const ConstVectorRef &x0,
143 const ConstVectorRef &x1, MatrixRef Jout,
144 int arg) const = 0;
145
147 virtual void interpolate_impl(const ConstVectorRef &x0,
148 const ConstVectorRef &x1, const Scalar &u,
149 VectorRef out) const {
150 // default implementation
151 integrate(x0, u * difference(x0, x1), out);
152 }
153
154 virtual void neutral_impl(VectorRef out) const {
155 assert(out.size() == nx());
156 out.setZero();
157 }
158
159 virtual void rand_impl(VectorRef out) const {
160 assert(out.size() == nx());
161 out.setRandom();
162 }
163};
164
165} // namespace aligator
166
167#include "aligator/core/manifold-base.hxx"
Forward declarations.
Main package namespace.
VectorSpaceTpl< Scalar, Eigen::Dynamic > TangentSpaceType
virtual void Jdifference_impl(const ConstVectorRef &x0, const ConstVectorRef &x1, MatrixRef Jout, int arg) const =0
VectorXs rand() const
Sample a random point on the manifold.
ManifoldAbstractTpl(int nx, int ndx)
void neutral(VectorRef out) const
Get the neutral element from the manifold (if this makes sense).
ALIGATOR_DYNAMIC_TYPEDEFS(Scalar)
Scalar type.
void difference(const ConstVectorRef &x0, const ConstVectorRef &x1, VectorRef out) const
Manifold difference/retraction operation .
virtual void JintegrateTransport_impl(const ConstVectorRef &x, const ConstVectorRef &v, MatrixRef Jout, int arg) const =0
void Jintegrate(const ConstVectorRef &x, const ConstVectorRef &v, MatrixRef Jout, int arg) const
Jacobian of the integation operation.
virtual void rand_impl(VectorRef out) const
void interpolate(const ConstVectorRef &x0, const ConstVectorRef &x1, const Scalar &u, VectorRef out) const
void rand(VectorRef out) const
Sample a random point on the manifold.
virtual ~ManifoldAbstractTpl()=default
virtual void interpolate_impl(const ConstVectorRef &x0, const ConstVectorRef &x1, const Scalar &u, VectorRef out) const
Interpolation operation.
void integrate(const ConstVectorRef &x, const ConstVectorRef &v, VectorRef out) const
Manifold integration operation .
VectorXs neutral() const
Get the neutral element from the manifold (if this makes sense).
VectorXs integrate(const ConstVectorRef &x, const ConstVectorRef &v) const
Manifold integration operation .
void JintegrateTransport(const ConstVectorRef &x, const ConstVectorRef &v, MatrixRef Jout, int arg) const
Perform the parallel transport operation.
VectorXs interpolate(const ConstVectorRef &x0, const ConstVectorRef &x1, const Scalar &u) const
Interpolation operation.
TangentSpaceType tangentSpace() const
Return an object representing the tangent space as a manifold.
virtual void difference_impl(const ConstVectorRef &x0, const ConstVectorRef &x1, VectorRef out) const =0
Implementation of the manifold retraction operation.
virtual void integrate_impl(const ConstVectorRef &x, const ConstVectorRef &v, VectorRef out) const =0
Perform the manifold integration operation.
void Jdifference(const ConstVectorRef &x0, const ConstVectorRef &x1, MatrixRef Jout, int arg) const
Jacobian of the retraction operation.
virtual void Jintegrate_impl(const ConstVectorRef &x, const ConstVectorRef &v, MatrixRef Jout, int arg) const =0
virtual void neutral_impl(VectorRef out) const
VectorXs difference(const ConstVectorRef &x0, const ConstVectorRef &x1) const
Manifold difference/retraction operation .
virtual bool isNormalized(const ConstVectorRef &) const
Check if the input vector x is a viable element of the manifold.
Standard Euclidean vector space.