aligator  0.16.0
A versatile and efficient C++ library for real-time constrained trajectory optimization.
Loading...
Searching...
No Matches
vector-space.hpp
Go to the documentation of this file.
1
3#pragma once
4
6
7#include <type_traits>
8
9namespace aligator {
10
12template <typename _Scalar, int _Dim>
14 using Scalar = _Scalar;
15 static constexpr int Dim = _Dim;
18 using Base::ndx;
19 using Base::nx;
20
22 explicit VectorSpaceTpl(const int dim)
23 : Base(dim, dim) {
24 static_assert(
25 Dim == Eigen::Dynamic,
26 "This constructor is only valid if the dimension is dynamic.");
27 }
28
29 int dim() const { return this->nx_; }
30
35 template <int N = Dim,
36 typename = typename std::enable_if_t<N != Eigen::Dynamic>>
37 explicit VectorSpaceTpl()
38 : Base(Dim, Dim) {}
39
41 template <int OtherDim>
43 : Base(other.nx_, other.nx_) {
44 static_assert((Dim == OtherDim) || (Dim == Eigen::Dynamic));
45 }
46
47protected:
49
50 /* Integrate */
51
52 void integrate_impl(const ConstVectorRef &x, const ConstVectorRef &v,
53 VectorRef out) const {
54 out = x + v;
55 }
56
57 void Jintegrate_impl(const ConstVectorRef &, const ConstVectorRef &,
58 MatrixRef Jout, int) const {
59 Jout.setIdentity();
60 }
61
62 void JintegrateTransport_impl(const ConstVectorRef &, const ConstVectorRef &,
63 MatrixRef, int) const {}
64
65 /* Difference */
66
67 void difference_impl(const ConstVectorRef &x0, const ConstVectorRef &x1,
68 VectorRef out) const {
69 out = x1 - x0;
70 }
71
72 void Jdifference_impl(const ConstVectorRef &, const ConstVectorRef &,
73 MatrixRef Jout, int arg) const {
74 switch (arg) {
75 case 0:
76 Jout.setIdentity() *= -1;
77 break;
78 case 1:
79 Jout.setIdentity();
80 break;
81 default:
82 ALIGATOR_DOMAIN_ERROR("Wrong arg value.");
83 }
84 }
85
86 void interpolate_impl(const ConstVectorRef &x0, const ConstVectorRef &x1,
87 const Scalar &u, VectorRef out) const {
88 out = u * x1 + (static_cast<Scalar>(1.) - u) * x0;
89 }
90};
91
92} // namespace aligator
#define ALIGATOR_DOMAIN_ERROR(...)
Main package namespace.
ManifoldAbstractTpl(int nx, int ndx)
void interpolate_impl(const ConstVectorRef &x0, const ConstVectorRef &x1, const Scalar &u, VectorRef out) const
Interpolation operation.
VectorSpaceTpl(const int dim)
Default constructor where the dimension is supplied.
VectorSpaceTpl()
Default constructor without arguments.
void Jdifference_impl(const ConstVectorRef &, const ConstVectorRef &, MatrixRef Jout, int arg) const
void integrate_impl(const ConstVectorRef &x, const ConstVectorRef &v, VectorRef out) const
Perform the manifold integration operation.
void JintegrateTransport_impl(const ConstVectorRef &, const ConstVectorRef &, MatrixRef, int) const
VectorSpaceTpl(const VectorSpaceTpl< Scalar, OtherDim > &other)
Build from VectorSpaceTpl of different dimension.
void difference_impl(const ConstVectorRef &x0, const ConstVectorRef &x1, VectorRef out) const
Implementation of the manifold retraction operation.
void Jintegrate_impl(const ConstVectorRef &, const ConstVectorRef &, MatrixRef Jout, int) const