Drake
eigen_types.h
Go to the documentation of this file.
1 #pragma once
2 
3 /// @file
4 /// This file contains abbreviated definitions for certain specializations of
5 /// Eigen::Matrix that are commonly used in Drake.
6 /// These convenient definitions are templated on the scalar type of the Eigen
7 /// object. While Drake uses `<T>` for scalar types across the entire code base
8 /// we decided in this file to use `<Scalar>` to be more consistent with the
9 /// usage of `<Scalar>` in Eigen's code base.
10 /// @see also eigen_autodiff_types.h
11 
12 #include <memory>
13 
14 #include <Eigen/Dense>
15 
16 #include "drake/common/constants.h"
17 #include "drake/common/drake_copyable.h"
18 
19 namespace drake {
20 
21 /// A column vector of size 1 (that is, a scalar), templated on scalar type.
22 template <typename Scalar>
23 using Vector1 = Eigen::Matrix<Scalar, 1, 1>;
24 
25 /// A column vector of size 1 of doubles.
26 using Vector1d = Eigen::Matrix<double, 1, 1>;
27 
28 /// A column vector of size 2, templated on scalar type.
29 template <typename Scalar>
30 using Vector2 = Eigen::Matrix<Scalar, 2, 1>;
31 
32 /// A column vector of size 3, templated on scalar type.
33 template <typename Scalar>
34 using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
35 
36 /// A column vector of size 4, templated on scalar type.
37 template <typename Scalar>
38 using Vector4 = Eigen::Matrix<Scalar, 4, 1>;
39 
40 /// A column vector of size 6.
41 template <typename Scalar>
42 using Vector6 = Eigen::Matrix<Scalar, 6, 1>;
43 
44 /// A column vector of any size, templated on scalar type.
45 template <typename Scalar>
46 using VectorX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
47 
48 /// A vector of dynamic size templated on scalar type, up to a maximum of 6
49 /// elements.
50 template <typename Scalar>
51 using VectorUpTo6 = Eigen::Matrix<Scalar, Eigen::Dynamic, 1, 0, 6, 1>;
52 
53 /// A row vector of size 2, templated on scalar type.
54 template <typename Scalar>
55 using RowVector2 = Eigen::Matrix<Scalar, 1, 2>;
56 
57 /// A row vector of size 3, templated on scalar type.
58 template <typename Scalar>
59 using RowVector3 = Eigen::Matrix<Scalar, 1, 3>;
60 
61 /// A row vector of size 4, templated on scalar type.
62 template <typename Scalar>
63 using RowVector4 = Eigen::Matrix<Scalar, 1, 4>;
64 
65 /// A row vector of size 6.
66 template <typename Scalar>
67 using RowVector6 = Eigen::Matrix<Scalar, 1, 6>;
68 
69 /// A row vector of any size, templated on scalar type.
70 template <typename Scalar>
71 using RowVectorX = Eigen::Matrix<Scalar, 1, Eigen::Dynamic>;
72 
73 
74 /// A matrix of 2 rows and 2 columns, templated on scalar type.
75 template <typename Scalar>
76 using Matrix2 = Eigen::Matrix<Scalar, 2, 2>;
77 
78 /// A matrix of 3 rows and 3 columns, templated on scalar type.
79 template <typename Scalar>
80 using Matrix3 = Eigen::Matrix<Scalar, 3, 3>;
81 
82 /// A matrix of 4 rows and 4 columns, templated on scalar type.
83 template <typename Scalar>
84 using Matrix4 = Eigen::Matrix<Scalar, 4, 4>;
85 
86 /// A matrix of 6 rows and 6 columns, templated on scalar type.
87 template <typename Scalar>
88 using Matrix6 = Eigen::Matrix<Scalar, 6, 6>;
89 
90 /// A matrix of 2 rows, dynamic columns, templated on scalar type.
91 template <typename Scalar>
92 using Matrix2X = Eigen::Matrix<Scalar, 2, Eigen::Dynamic>;
93 
94 /// A matrix of 3 rows, dynamic columns, templated on scalar type.
95 template <typename Scalar>
96 using Matrix3X = Eigen::Matrix<Scalar, 3, Eigen::Dynamic>;
97 
98 /// A matrix of 4 rows, dynamic columns, templated on scalar type.
99 template <typename Scalar>
100 using Matrix4X = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>;
101 
102 /// A matrix of 6 rows, dynamic columns, templated on scalar type.
103 template <typename Scalar>
104 using Matrix6X = Eigen::Matrix<Scalar, 6, Eigen::Dynamic>;
105 
106 /// A matrix of dynamic size, templated on scalar type.
107 template <typename Scalar>
108 using MatrixX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
109 
110 /// A matrix of dynamic size templated on scalar type, up to a maximum of 6 rows
111 /// and 6 columns. Rectangular matrices, with different number of rows and
112 /// columns, are allowed.
113 template <typename Scalar>
114 using MatrixUpTo6 =
115 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, 0, 6, 6>;
116 
117 /// A quaternion templated on scalar type.
118 template <typename Scalar>
119 using Quaternion = Eigen::Quaternion<Scalar>;
120 
121 /// An AngleAxis templated on scalar type.
122 template <typename Scalar>
123 using AngleAxis = Eigen::AngleAxis<Scalar>;
124 
125 /// An Isometry templated on scalar type.
126 template <typename Scalar>
127 using Isometry3 = Eigen::Transform<Scalar, 3, Eigen::Isometry>;
128 
129 /// A translation in 3D templated on scalar type.
130 template <typename Scalar>
131 using Translation3 = Eigen::Translation<Scalar, 3>;
132 
133 /// A column vector consisting of one twist.
134 template <typename Scalar>
135 using TwistVector = Eigen::Matrix<Scalar, kTwistSize, 1>;
136 
137 /// A matrix with one twist per column, and dynamically many columns.
138 template <typename Scalar>
139 using TwistMatrix = Eigen::Matrix<Scalar, kTwistSize, Eigen::Dynamic>;
140 
141 /// A six-by-six matrix.
142 template <typename Scalar>
143 using SquareTwistMatrix = Eigen::Matrix<Scalar, kTwistSize, kTwistSize>;
144 
145 /// A column vector consisting of one wrench (spatial force) = `[r X f; f]`,
146 /// where f is a force (translational force) applied at a point `P` and `r` is
147 /// the position vector from a point `O` (called the "moment center") to point
148 /// `P`.
149 template <typename Scalar>
150 using WrenchVector = Eigen::Matrix<Scalar, 6, 1>;
151 
152 /// A column vector consisting of a concatenated rotational and translational
153 /// force. The wrench is a special case of a SpatialForce. For a general
154 /// SpatialForce the rotational force can be a pure torque or the accumulation
155 /// of moments and need not necessarily be a function of the force term.
156 template <typename Scalar>
157 using SpatialForce = Eigen::Matrix<Scalar, 6, 1>;
158 
159 /// EigenSizeMinPreferDynamic<a, b>::value gives the min between compile-time
160 /// sizes @p a and @p b. 0 has absolute priority, followed by 1, followed by
161 /// Dynamic, followed by other finite values.
162 ///
163 /// Note that this is a type-trait version of EIGEN_SIZE_MIN_PREFER_DYNAMIC
164 /// macro in "Eigen/Core/util/Macros.h".
165 template <int a, int b>
167  // clang-format off
168  static constexpr int value = (a == 0 || b == 0) ? 0 :
169  (a == 1 || b == 1) ? 1 :
170  (a == Eigen::Dynamic || b == Eigen::Dynamic) ? Eigen::Dynamic :
171  a <= b ? a : b;
172  // clang-format on
173 };
174 
175 /// EigenSizeMinPreferFixed is a variant of EigenSizeMinPreferDynamic. The
176 /// difference is that finite values now have priority over Dynamic, so that
177 /// EigenSizeMinPreferFixed<3, Dynamic>::value gives 3.
178 ///
179 /// Note that this is a type-trait version of EIGEN_SIZE_MIN_PREFER_FIXED macro
180 /// in "Eigen/Core/util/Macros.h".
181 template <int a, int b>
183  // clang-format off
184  static constexpr int value = (a == 0 || b == 0) ? 0 :
185  (a == 1 || b == 1) ? 1 :
186  (a == Eigen::Dynamic && b == Eigen::Dynamic) ? Eigen::Dynamic :
187  (a == Eigen::Dynamic) ? b :
188  (b == Eigen::Dynamic) ? a :
189  a <= b ? a : b;
190  // clang-format on
191 };
192 
193 /// MultiplyEigenSizes<a, b> gives a * b if both of a and b are fixed
194 /// sizes. Otherwise it gives Eigen::Dynamic.
195 template <int a, int b>
197  static constexpr int value =
198  (a == Eigen::Dynamic || b == Eigen::Dynamic) ? Eigen::Dynamic : a * b;
199 };
200 
201 /*
202  * Determines if a type is derived from EigenBase<> (e.g. ArrayBase<>,
203  * MatrixBase<>).
204  */
205 template <typename Derived>
206 struct is_eigen_type : std::is_base_of<Eigen::EigenBase<Derived>, Derived> {};
207 
208 /*
209  * Determines if an EigenBase<> has a specific scalar type.
210  */
211 template <typename Derived, typename Scalar>
213  : std::integral_constant<
214  bool, is_eigen_type<Derived>::value &&
215  std::is_same<typename Derived::Scalar, Scalar>::value> {};
216 
217 /*
218  * Determines if an EigenBase<> type is a compile-time (column) vector.
219  * This will not check for run-time size.
220  */
221 template <typename Derived>
223  : std::integral_constant<bool, is_eigen_type<Derived>::value &&
224  Derived::ColsAtCompileTime == 1> {};
225 
226 /*
227  * Determines if an EigenBase<> type is a compile-time (column) vector of a
228  * scalar type. This will not check for run-time size.
229  */
230 template <typename Derived, typename Scalar>
232  : std::integral_constant<
233  bool, is_eigen_scalar_same<Derived, Scalar>::value &&
234  is_eigen_vector<Derived>::value> {};
235 
236 /*
237  * Determines if a EigenBase<> type is a compile-time non-column-vector matrix
238  * of a scalar type. This will not check for run-time size.
239  * @note For an EigenBase<> of the correct Scalar type, this logic is
240  * exclusive to is_eigen_vector_of<> such that distinct specializations are not
241  * ambiguous.
242  */
243 // TODO(eric.cousineau): A 1x1 matrix will be disqualified in this case, and
244 // this logic will qualify it as a vector. Address the downstream logic if this
245 // becomes an issue.
246 template <typename Derived, typename Scalar>
248  : std::integral_constant<
249  bool, is_eigen_scalar_same<Derived, Scalar>::value &&
250  !is_eigen_vector<Derived>::value> {};
251 
252 // TODO(eric.cousineau): Add alias is_eigen_matrix_of = is_eigen_scalar_same if
253 // appropriate.
254 
255 /// This wrapper class provides a way to write non-template functions taking raw
256 /// pointers to Eigen objects as parameters while limiting the number of copies,
257 /// similar to `Eigen::Ref`. Internally, it keeps an instance of `Eigen::Ref<T>`
258 /// and provides access to it via `operator*` and `operator->`.
259 ///
260 /// The motivation of this class is to follow <a
261 /// href="https://google.github.io/styleguide/cppguide.html#Reference_Arguments">GSG's
262 /// "output arguments should be pointers" rule</a> while taking advantage of
263 /// using `Eigen::Ref`. Here is an example.
264 ///
265 /// @code
266 /// // This function is taking an Eigen::Ref of a matrix and modifies it in
267 /// // the body. This violates GSG's rule on output parameters.
268 /// void foo(Eigen::Ref<Eigen::MatrixXd> M) {
269 /// M(0, 0) = 0;
270 /// }
271 /// // At Call-site, we have:
272 /// foo(M);
273 /// foo(M.block(0, 0, 2, 2));
274 ///
275 /// // We can rewrite the above function into the following using EigenPtr.
276 /// void foo(EigenPtr<Eigen::MatrixXd> M) {
277 /// (*M)(0, 0) = 0;
278 /// }
279 /// // Note that, call sites should be changed to:
280 /// foo(&M);
281 ///
282 /// // We need tmp to avoid taking the address of a temporary object such as the
283 /// // return value of .block().
284 /// auto tmp = M.block(0, 0, 2, 2);
285 /// foo(&tmp);
286 /// @endcode
287 ///
288 /// Notice that methods taking an EigenPtr can mutate the entries of a matrix as
289 /// in method `foo()` in the example code above, but cannot change its size.
290 /// This is because `operator*` and `operator->` return an `Eigen::Ref<T>`
291 /// object and only plain matrices/arrays can be resized and not expressions.
292 /// This **is** the desired behavior, since resizing the block of a matrix or
293 /// even a more general expression should not be allowed. If you do want to be
294 /// able to resize a mutable matrix argument, then you must pass it as a
295 /// `Matrix<T>*`, like so:
296 /// @code
297 /// void bar(Eigen::MatrixXd* M) {
298 /// DRAKE_THROW_UNLESS(M != nullptr);
299 /// // In this case this method only works with 4x3 matrices.
300 /// if (M->rows() != 4 && M->cols() != 3) {
301 /// M->resize(4, 3);
302 /// }
303 /// (*M)(0, 0) = 0;
304 /// }
305 /// @endcode
306 ///
307 /// @note This class provides a way to avoid the `const_cast` hack introduced in
308 /// <a
309 /// href="https://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html#TopicPlainFunctionsFailing">Eigen's
310 /// documentation</a>.
311 template <typename PlainObjectType>
312 class EigenPtr {
313  public:
314  typedef Eigen::Ref<PlainObjectType> RefType;
315 
316  EigenPtr() : EigenPtr(nullptr) {}
317 
318  /// Overload for `nullptr`.
319  // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
320  EigenPtr(std::nullptr_t) {}
321 
322  /// Constructs with a reference to the given matrix type.
323  // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
324  EigenPtr(const EigenPtr& other) { assign(other); }
325 
326  /// Constructs with a reference to another matrix type.
327  /// May be `nullptr`.
328  template <typename PlainObjectTypeIn>
329  // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
330  EigenPtr(PlainObjectTypeIn* m) {
331  if (m) {
332  m_.reset(new RefType(*m));
333  }
334  }
335 
336  /// Constructs from another EigenPtr.
337  template <typename PlainObjectTypeIn>
338  // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
340  // Cannot directly construct `m_` from `other.m_`.
341  assign(other);
342  }
343 
344  EigenPtr& operator=(const EigenPtr& other) {
345  // We must explicitly override this version of operator=.
346  // The template below will not take precedence over this one.
347  return assign(other);
348  }
349 
350  template <typename PlainObjectTypeIn>
352  return assign(other);
353  }
354 
355  /// @throws std::runtime_error if this is a null dereference.
356  RefType& operator*() const { return *get_reference(); }
357 
358  /// @throws std::runtime_error if this is a null dereference.
359  RefType* operator->() const { return get_reference(); }
360 
361  /// Returns whether or not this contains a valid reference.
362  operator bool() const { return is_valid(); }
363 
364  bool operator==(std::nullptr_t) const { return !is_valid(); }
365 
366  bool operator!=(std::nullptr_t) const { return is_valid(); }
367 
368  private:
369  // Use unique_ptr<> so that we may "reconstruct" the reference, making this
370  // a pointer-like type.
371  // TODO(eric.cousineau): Consider using a stack-based implementation if
372  // performance is a concern, possibly with a mutable member.
373  std::unique_ptr<RefType> m_;
374 
375  // Consolidate assignment here, so that both the copy constructor and the
376  // construction from another type may be used.
377  template <typename PlainObjectTypeIn>
378  EigenPtr& assign(const EigenPtr<PlainObjectTypeIn>& other) {
379  if (other) {
380  m_.reset(new RefType(*other));
381  } else {
382  m_.reset();
383  }
384  return *this;
385  }
386 
387  // Consolidate getting a reference here.
388  RefType* get_reference() const {
389  if (!m_) throw std::runtime_error("EigenPtr: nullptr dereference");
390  return m_.get();
391  }
392 
393  bool is_valid() const {
394  return static_cast<bool>(m_);
395  }
396 };
397 
398 } // namespace drake
EigenSizeMinPreferFixed is a variant of EigenSizeMinPreferDynamic.
Definition: eigen_types.h:182
Definition: eigen_types.h:212
Eigen::Transform< Scalar, 3, Eigen::Isometry > Isometry3
An Isometry templated on scalar type.
Definition: eigen_types.h:127
Eigen::Quaternion< Scalar > Quaternion
A quaternion templated on scalar type.
Definition: eigen_types.h:119
RefType * operator->() const
Definition: eigen_types.h:359
Eigen::AngleAxis< Scalar > AngleAxis
An AngleAxis templated on scalar type.
Definition: eigen_types.h:123
RefType & operator*() const
Definition: eigen_types.h:356
bool operator!=(std::nullptr_t) const
Definition: eigen_types.h:366
Eigen::Matrix< Scalar, 6, 6 > Matrix6
A matrix of 6 rows and 6 columns, templated on scalar type.
Definition: eigen_types.h:88
Eigen::Matrix< Scalar, 2, Eigen::Dynamic > Matrix2X
A matrix of 2 rows, dynamic columns, templated on scalar type.
Definition: eigen_types.h:92
Eigen::Matrix< Scalar, 4, 1 > Vector4
A column vector of size 4, templated on scalar type.
Definition: eigen_types.h:38
Definition: automotive_demo.cc:90
Eigen::Matrix< Scalar, 4, Eigen::Dynamic > Matrix4X
A matrix of 4 rows, dynamic columns, templated on scalar type.
Definition: eigen_types.h:100
Eigen::Matrix< Scalar, kTwistSize, Eigen::Dynamic > TwistMatrix
A matrix with one twist per column, and dynamically many columns.
Definition: eigen_types.h:139
EigenSizeMinPreferDynamic<a, b>::value gives the min between compile-time sizes a and b...
Definition: eigen_types.h:166
Eigen::Matrix< Scalar, 1, 2 > RowVector2
A row vector of size 2, templated on scalar type.
Definition: eigen_types.h:55
EigenPtr(std::nullptr_t)
Overload for nullptr.
Definition: eigen_types.h:320
Definition: eigen_types.h:247
Eigen::Matrix< Scalar, 1, 4 > RowVector4
A row vector of size 4, templated on scalar type.
Definition: eigen_types.h:63
Eigen::Translation< Scalar, 3 > Translation3
A translation in 3D templated on scalar type.
Definition: eigen_types.h:131
Eigen::Matrix< Scalar, 2, 1 > Vector2
A column vector of size 2, templated on scalar type.
Definition: eigen_types.h:30
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorX
A column vector of any size, templated on scalar type.
Definition: eigen_types.h:46
MultiplyEigenSizes<a, b> gives a * b if both of a and b are fixed sizes.
Definition: eigen_types.h:196
Eigen::Matrix< Scalar, 6, Eigen::Dynamic > Matrix6X
A matrix of 6 rows, dynamic columns, templated on scalar type.
Definition: eigen_types.h:104
EigenPtr(PlainObjectTypeIn *m)
Constructs with a reference to another matrix type.
Definition: eigen_types.h:330
Eigen::Matrix< Scalar, 1, Eigen::Dynamic > RowVectorX
A row vector of any size, templated on scalar type.
Definition: eigen_types.h:71
Eigen::Matrix< Scalar, 2, 2 > Matrix2
A matrix of 2 rows and 2 columns, templated on scalar type.
Definition: eigen_types.h:76
Eigen::Matrix< Scalar, 1, 1 > Vector1
A column vector of size 1 (that is, a scalar), templated on scalar type.
Definition: eigen_types.h:23
Eigen::Matrix< Scalar, 1, 3 > RowVector3
A row vector of size 3, templated on scalar type.
Definition: eigen_types.h:59
EigenPtr(const EigenPtr &other)
Constructs with a reference to the given matrix type.
Definition: eigen_types.h:324
Eigen::Matrix< Scalar, 3, 1 > Vector3
A column vector of size 3, templated on scalar type.
Definition: eigen_types.h:34
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > MatrixX
A matrix of dynamic size, templated on scalar type.
Definition: eigen_types.h:108
Eigen::Matrix< Scalar, kTwistSize, 1 > TwistVector
A column vector consisting of one twist.
Definition: eigen_types.h:135
Eigen::Matrix< Scalar, 6, 1 > SpatialForce
A column vector consisting of a concatenated rotational and translational force.
Definition: eigen_types.h:157
Eigen::Matrix< Scalar, 6, 1 > WrenchVector
A column vector consisting of one wrench (spatial force) = [r X f; f], where f is a force (translatio...
Definition: eigen_types.h:150
Definition: eigen_types.h:222
bool operator==(std::nullptr_t) const
Definition: eigen_types.h:364
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, 0, 6, 1 > VectorUpTo6
A vector of dynamic size templated on scalar type, up to a maximum of 6 elements. ...
Definition: eigen_types.h:51
This wrapper class provides a way to write non-template functions taking raw pointers to Eigen object...
Definition: eigen_types.h:312
EigenPtr & operator=(const EigenPtr &other)
Definition: eigen_types.h:344
EigenPtr & operator=(const EigenPtr< PlainObjectTypeIn > &other)
Definition: eigen_types.h:351
EigenPtr(const EigenPtr< PlainObjectTypeIn > &other)
Constructs from another EigenPtr.
Definition: eigen_types.h:339
Eigen::Matrix< Scalar, 6, 1 > Vector6
A column vector of size 6.
Definition: eigen_types.h:42
Eigen::Ref< PlainObjectType > RefType
Definition: eigen_types.h:314
EigenPtr()
Definition: eigen_types.h:316
Definition: eigen_types.h:231
Eigen::Matrix< Scalar, 1, 6 > RowVector6
A row vector of size 6.
Definition: eigen_types.h:67
Eigen::Matrix< Scalar, 4, 4 > Matrix4
A matrix of 4 rows and 4 columns, templated on scalar type.
Definition: eigen_types.h:84
Eigen::Matrix< Scalar, 3, 3 > Matrix3
A matrix of 3 rows and 3 columns, templated on scalar type.
Definition: eigen_types.h:80
Eigen::Matrix< double, 1, 1 > Vector1d
A column vector of size 1 of doubles.
Definition: eigen_types.h:26
static constexpr int value
Definition: eigen_types.h:168
Eigen::Matrix< Scalar, 3, Eigen::Dynamic > Matrix3X
A matrix of 3 rows, dynamic columns, templated on scalar type.
Definition: eigen_types.h:96
Eigen::Matrix< Scalar, kTwistSize, kTwistSize > SquareTwistMatrix
A six-by-six matrix.
Definition: eigen_types.h:143
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, 0, 6, 6 > MatrixUpTo6
A matrix of dynamic size templated on scalar type, up to a maximum of 6 rows and 6 columns...
Definition: eigen_types.h:115
Definition: eigen_types.h:206