aligator  0.16.0
A versatile and efficient C++ library for real-time constrained trajectory optimization.
Loading...
Searching...
No Matches
math.hpp
Go to the documentation of this file.
1
4#pragma once
5
6#include <Eigen/Core>
7
8#define ALIGATOR_DYNAMIC_TYPEDEFS(Scalar) \
9 using VectorXs = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>; \
10 using MatrixXs = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>; \
11 using VectorOfVectors = std::vector<VectorXs>; \
12 using VectorRef = Eigen::Ref<VectorXs>; \
13 using MatrixRef = Eigen::Ref<MatrixXs>; \
14 using ConstVectorRef = Eigen::Ref<const VectorXs>; \
15 using ConstMatrixRef = Eigen::Ref<const MatrixXs>; \
16 using Vector3s = Eigen::Matrix<Scalar, 3, 1>; \
17 using Vector6s = Eigen::Matrix<Scalar, 6, 1>; \
18 using Matrix3Xs = Eigen::Matrix<Scalar, 3, Eigen::Dynamic>; \
19 using Matrix6Xs = Eigen::Matrix<Scalar, 6, Eigen::Dynamic>; \
20 using Matrix6s = Eigen::Matrix<Scalar, 6, 6>
21
22#define ALIGATOR_DYNAMIC_TYPEDEFS_WITH_ROW_TYPES(Scalar) \
23 ALIGATOR_DYNAMIC_TYPEDEFS(Scalar); \
24 using RowMatrixXs = typename Eigen::Transpose<MatrixXs>::PlainObject; \
25 using RowMatrixRef = Eigen::Ref<RowMatrixXs>; \
26 using ConstRowMatrixRef = Eigen::Ref<const RowMatrixXs>
27
28#ifdef ALIGATOR_EIGEN_CHECK_MALLOC
31#define ALIGATOR_EIGEN_ALLOW_MALLOC(allowed) \
32 ::Eigen::internal::set_is_malloc_allowed(allowed)
37#define ALIGATOR_NOMALLOC_SCOPED \
38 const ::aligator::internal::scoped_nomalloc ___aligator_nomalloc_zone {}
40#define ALIGATOR_NOMALLOC_RESTORE \
41 ALIGATOR_EIGEN_ALLOW_MALLOC(::aligator::internal::get_cached_malloc_status())
42#else
43#define ALIGATOR_EIGEN_ALLOW_MALLOC(allowed)
44#define ALIGATOR_NOMALLOC_SCOPED
45#define ALIGATOR_NOMALLOC_RESTORE
46#endif
47
49#define ALIGATOR_NOMALLOC_BEGIN ALIGATOR_EIGEN_ALLOW_MALLOC(false)
51#define ALIGATOR_NOMALLOC_END ALIGATOR_EIGEN_ALLOW_MALLOC(true)
52
53namespace aligator {
54
59template <template <class> class Base, typename Derived> struct is_tpl_base_of {
60 static constexpr std::false_type f(const void *);
61 template <typename OtherDerived>
62 static constexpr std::true_type f(const Base<OtherDerived> *);
63 static constexpr bool value =
64 decltype(f(std::declval<std::remove_reference_t<Derived> *>()))::value;
65};
66
67template <template <class> class Base, typename Derived>
69
70template <typename T>
71struct is_eigen : std::bool_constant<is_tpl_base_of_v<Eigen::EigenBase, T>> {};
72
73template <typename T> inline constexpr bool is_eigen_v = is_eigen<T>::value;
74
75template <typename T>
76using enable_if_eigen_t = std::enable_if_t<is_eigen_v<std::decay_t<T>>>;
77
78template <typename T>
80
81template <typename T, typename T2 = void>
82using enable_if_eigen_dense_t = std::enable_if_t<is_eigen_dense_v<T>, T2>;
83
84template <typename T>
85inline constexpr bool is_eigen_matrix_v =
87
88#ifdef ALIGATOR_EIGEN_CHECK_MALLOC
89namespace internal {
90thread_local static bool g_cached_malloc_status = true;
91
92inline void set_malloc_status(bool status) { g_cached_malloc_status = status; }
93
94inline void save_malloc_status() {
95 set_malloc_status(
96#ifdef ALIGATOR_EIGEN_CHECK_MALLOC
97 ::Eigen::internal::is_malloc_allowed()
98#else
99 false
100#endif
101 );
102}
103
104inline bool get_cached_malloc_status() { return g_cached_malloc_status; }
105
106struct scoped_nomalloc {
107 scoped_nomalloc(const scoped_nomalloc &) = delete;
108 scoped_nomalloc(scoped_nomalloc &&) = delete;
109 ALIGATOR_INLINE scoped_nomalloc() {
110 save_malloc_status();
112 }
113 // reset to value from before the scope
114 ~scoped_nomalloc() { ALIGATOR_NOMALLOC_RESTORE; }
115};
116
117} // namespace internal
118#endif
119
122template <typename _Scalar> struct math_types {
123 using Scalar = _Scalar;
125};
126
130template <typename D>
131auto eigenPrintWithPreamble(const Eigen::EigenBase<D> &mat,
132 const std::string &text,
133 Eigen::IOFormat ft = EIGEN_DEFAULT_IO_FORMAT) {
134 ft.matPrefix = text;
135 ft.rowSpacer = "";
136 int i = int(text.length()) - 1;
137 while (i >= 0) {
138 if (text[size_t(i)] != '\n')
139 ft.rowSpacer += ' ';
140 i--;
141 }
142 return mat.derived().format(ft);
143}
144
146namespace math {
147
148template <typename MatType>
149typename MatType::Scalar infty_norm(const Eigen::MatrixBase<MatType> &z) {
150 if (z.rows() == 0 || z.cols() == 0) {
151 return 0.;
152 } else {
153 return z.template lpNorm<Eigen::Infinity>();
154 }
155}
156
157template <typename MatType>
158typename MatType::Scalar infty_norm(const std::vector<MatType> &z) {
159 const std::size_t n = z.size();
160 typename MatType::Scalar out = 0.;
161 for (std::size_t i = 0; i < n; i++) {
162 out = std::max(out, infty_norm(z[i]));
163 }
164 return out;
165}
166
168template <typename Scalar> inline bool check_scalar(const Scalar value) {
169 return std::isnan(value) || std::isinf(value);
170}
171
173template <typename T> bool check_value(const std::vector<T> &xs) {
174 const std::size_t n = xs.size();
175 for (std::size_t i = 0; i < n; i++) {
176 if (check_value<T>(xs[i]))
177 return true;
178 }
179 return false;
180}
181
182template <typename T, typename = std::enable_if_t<std::is_scalar<T>::value>>
183bool check_value(const T &x) {
184 static_assert(std::is_scalar<T>::value, "Parameter T should be scalar.");
185 return check_scalar(x);
186}
187
188template <typename MatrixType>
189bool check_value(const Eigen::MatrixBase<MatrixType> &x) {
190 return (x.hasNaN() || (!x.allFinite()));
191}
192
196template <typename Scalar>
197bool scalar_close(const Scalar a, const Scalar b,
198 const Scalar prec = std::numeric_limits<Scalar>::epsilon()) {
199 return std::abs(a - b) < prec * (1 + std::max(std::abs(a), std::abs(b)));
200}
201
202template <typename T> T sign(const T &x) {
203 static_assert(std::is_scalar<T>::value, "Parameter T should be scalar.");
204 return T((x > T(0)) - (x < T(0)));
205}
206
208template <typename Derived, unsigned int UpLo = Eigen::Lower>
209void make_symmetric(const Eigen::MatrixBase<Derived> &matrix) {
210 Derived &mat = matrix.const_cast_derived();
211 // symmetrize upper part
212 Eigen::SelfAdjointView<Derived, UpLo> view{mat};
213 mat = view;
214}
215
216template <typename T> void setZero(std::vector<T> &mats) {
217 for (std::size_t i = 0; i < mats.size(); i++) {
218 mats[i].setZero();
219 }
220}
221
223template <typename TA, typename AA, typename TB, typename BA, typename OutType,
224 typename AOut, typename Scalar>
225void vectorMultiplyAdd(const std::vector<TA, AA> &a,
226 const std::vector<TB, BA> &b,
227 std::vector<OutType, AOut> &c, const Scalar alpha) {
228 assert(a.size() == b.size());
229 assert(a.size() == c.size());
230 const std::size_t N = a.size();
231 for (std::size_t i = 0; i < N; i++) {
232 c[i] = a[i] + alpha * b[i];
233 }
234}
235
236} // namespace math
237} // namespace aligator
#define ALIGATOR_INLINE
Definition fwd.hpp:27
#define EIGEN_DEFAULT_IO_FORMAT
Definition fwd.hpp:9
#define ALIGATOR_NOMALLOC_RESTORE
Definition math.hpp:45
#define ALIGATOR_EIGEN_ALLOW_MALLOC(allowed)
Definition math.hpp:43
Math utilities.
Definition math.hpp:146
void setZero(std::vector< T > &mats)
Definition math.hpp:216
MatType::Scalar infty_norm(const Eigen::MatrixBase< MatType > &z)
Definition math.hpp:149
void make_symmetric(const Eigen::MatrixBase< Derived > &matrix)
Symmetrize a matrix using its lower triangular part.
Definition math.hpp:209
bool check_scalar(const Scalar value)
Check that a scalar is neither inf, nor NaN.
Definition math.hpp:168
bool check_value(const std::vector< T > &xs)
Check if a std::vector of numerical objects has invalid values.
Definition math.hpp:173
T sign(const T &x)
Definition math.hpp:202
bool scalar_close(const Scalar a, const Scalar b, const Scalar prec=std::numeric_limits< Scalar >::epsilon())
Tests whether a and b are close, within absolute and relative precision prec.
Definition math.hpp:197
void vectorMultiplyAdd(const std::vector< TA, AA > &a, const std::vector< TB, BA > &b, std::vector< OutType, AOut > &c, const Scalar alpha)
Compute zi = xi + alpha * yi for all i.
Definition math.hpp:225
Main package namespace.
constexpr bool is_eigen_dense_v
Definition math.hpp:79
constexpr bool is_eigen_v
Definition math.hpp:73
constexpr bool is_tpl_base_of_v
Definition math.hpp:68
std::enable_if_t< is_eigen_dense_v< T >, T2 > enable_if_eigen_dense_t
Definition math.hpp:82
std::enable_if_t< is_eigen_v< std::decay_t< T > > > enable_if_eigen_t
Definition math.hpp:76
auto eigenPrintWithPreamble(const Eigen::EigenBase< D > &mat, const std::string &text, Eigen::IOFormat ft=EIGEN_DEFAULT_IO_FORMAT)
Definition math.hpp:131
constexpr bool is_eigen_matrix_v
Definition math.hpp:85
static constexpr std::false_type f(const void *)
static constexpr bool value
Definition math.hpp:63
static constexpr std::true_type f(const Base< OtherDerived > *)
Typedefs for math (Eigen vectors, matrices) depending on scalar type.
Definition math.hpp:122
ALIGATOR_DYNAMIC_TYPEDEFS(_Scalar)