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"
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 matrix of 2 rows and 2 columns, templated on scalar type.
54 template <typename Scalar>
55 using Matrix2 = Eigen::Matrix<Scalar, 2, 2>;
56 
57 /// A matrix of 3 rows and 3 columns, templated on scalar type.
58 template <typename Scalar>
59 using Matrix3 = Eigen::Matrix<Scalar, 3, 3>;
60 
61 /// A matrix of 4 rows and 4 columns, templated on scalar type.
62 template <typename Scalar>
63 using Matrix4 = Eigen::Matrix<Scalar, 4, 4>;
64 
65 /// A matrix of 6 rows and 6 columns, templated on scalar type.
66 template <typename Scalar>
67 using Matrix6 = Eigen::Matrix<Scalar, 6, 6>;
68 
69 /// A matrix of 2 rows, dynamic columns, templated on scalar type.
70 template <typename Scalar>
71 using Matrix2X = Eigen::Matrix<Scalar, 2, Eigen::Dynamic>;
72 
73 /// A matrix of 3 rows, dynamic columns, templated on scalar type.
74 template <typename Scalar>
75 using Matrix3X = Eigen::Matrix<Scalar, 3, Eigen::Dynamic>;
76 
77 /// A matrix of 4 rows, dynamic columns, templated on scalar type.
78 template <typename Scalar>
79 using Matrix4X = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>;
80 
81 /// A matrix of 6 rows, dynamic columns, templated on scalar type.
82 template <typename Scalar>
83 using Matrix6X = Eigen::Matrix<Scalar, 6, Eigen::Dynamic>;
84 
85 /// A matrix of dynamic size, templated on scalar type.
86 template <typename Scalar>
87 using MatrixX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
88 
89 /// A matrix of dynamic size templated on scalar type, up to a maximum of 6 rows
90 /// and 6 columns. Rectangular matrices, with different number of rows and
91 /// columns, are allowed.
92 template <typename Scalar>
93 using MatrixUpTo6 =
94 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, 0, 6, 6>;
95 
96 /// A quaternion templated on scalar type.
97 template <typename Scalar>
98 using Quaternion = Eigen::Quaternion<Scalar>;
99 
100 /// An AngleAxis templated on scalar type.
101 template <typename Scalar>
102 using AngleAxis = Eigen::AngleAxis<Scalar>;
103 
104 /// An Isometry templated on scalar type.
105 template <typename Scalar>
106 using Isometry3 = Eigen::Transform<Scalar, 3, Eigen::Isometry>;
107 
108 /// A translation in 3D templated on scalar type.
109 template <typename Scalar>
110 using Translation3 = Eigen::Translation<Scalar, 3>;
111 
112 /// A column vector of dynamic size, up to a maximum of 73 elements.
113 using VectorUpTo73d = Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 73, 1>;
114 
115 /// A column vector consisting of one twist.
116 template <typename Scalar>
117 using TwistVector = Eigen::Matrix<Scalar, kTwistSize, 1>;
118 
119 /// A matrix with one twist per column, and dynamically many columns.
120 template <typename Scalar>
121 using TwistMatrix = Eigen::Matrix<Scalar, kTwistSize, Eigen::Dynamic>;
122 
123 /// A six-by-six matrix.
124 template <typename Scalar>
125 using SquareTwistMatrix = Eigen::Matrix<Scalar, kTwistSize, kTwistSize>;
126 
127 /// A column vector consisting of one wrench (spatial force) = `[r X f; f]`,
128 /// where f is a force (translational force) applied at a point `P` and `r` is
129 /// the position vector from a point `O` (called the "moment center") to point
130 /// `P`.
131 template <typename Scalar>
132 using WrenchVector = Eigen::Matrix<Scalar, 6, 1>;
133 
134 /// A column vector consisting of a concatenated rotational and translational
135 /// force. The wrench is a special case of a SpatialForce. For a general
136 /// SpatialForce the rotational force can be a pure torque or the accumulation
137 /// of moments and need not necessarily be a function of the force term.
138 template <typename Scalar>
139 using SpatialForce = Eigen::Matrix<Scalar, 6, 1>;
140 
141 /// EigenSizeMinPreferDynamic<a, b>::value gives the min between compile-time
142 /// sizes @p a and @p b. 0 has absolute priority, followed by 1, followed by
143 /// Dynamic, followed by other finite values.
144 ///
145 /// Note that this is a type-trait version of EIGEN_SIZE_MIN_PREFER_DYNAMIC
146 /// macro in "Eigen/Core/util/Macros.h".
147 template <int a, int b>
149  // clang-format off
150  static constexpr int value = (a == 0 || b == 0) ? 0 :
151  (a == 1 || b == 1) ? 1 :
152  (a == Eigen::Dynamic || b == Eigen::Dynamic) ? Eigen::Dynamic :
153  a <= b ? a : b;
154  // clang-format on
155 };
156 
157 /// EigenSizeMinPreferFixed is a variant of EigenSizeMinPreferDynamic. The
158 /// difference is that finite values now have priority over Dynamic, so that
159 /// EigenSizeMinPreferFixed<3, Dynamic>::value gives 3.
160 ///
161 /// Note that this is a type-trait version of EIGEN_SIZE_MIN_PREFER_FIXED macro
162 /// in "Eigen/Core/util/Macros.h".
163 template <int a, int b>
165  // clang-format off
166  static constexpr int value = (a == 0 || b == 0) ? 0 :
167  (a == 1 || b == 1) ? 1 :
168  (a == Eigen::Dynamic && b == Eigen::Dynamic) ? Eigen::Dynamic :
169  (a == Eigen::Dynamic) ? b :
170  (b == Eigen::Dynamic) ? a :
171  a <= b ? a : b;
172  // clang-format on
173 };
174 
175 /// MultiplyEigenSizes<a, b> gives a * b if both of a and b are fixed
176 /// sizes. Otherwise it gives Eigen::Dynamic.
177 template <int a, int b>
179  static constexpr int value =
180  (a == Eigen::Dynamic || b == Eigen::Dynamic) ? Eigen::Dynamic : a * b;
181 };
182 
183 /*
184  * Determines if a type is derived from EigenBase<> (e.g. ArrayBase<>,
185  * MatrixBase<>).
186  */
187 template <typename Derived>
188 struct is_eigen_type : std::is_base_of<Eigen::EigenBase<Derived>, Derived> {};
189 
190 /*
191  * Determines if an EigenBase<> has a specific scalar type.
192  */
193 template <typename Derived, typename Scalar>
195  : std::integral_constant<
196  bool, is_eigen_type<Derived>::value &&
197  std::is_same<typename Derived::Scalar, Scalar>::value> {};
198 
199 /*
200  * Determines if an EigenBase<> type is a compile-time (column) vector.
201  * This will not check for run-time size.
202  */
203 template <typename Derived>
205  : std::integral_constant<bool, is_eigen_type<Derived>::value &&
206  Derived::ColsAtCompileTime == 1> {};
207 
208 /*
209  * Determines if an EigenBase<> type is a compile-time (column) vector of a
210  * scalar type. This will not check for run-time size.
211  */
212 template <typename Derived, typename Scalar>
214  : std::integral_constant<
215  bool, is_eigen_scalar_same<Derived, Scalar>::value &&
216  is_eigen_vector<Derived>::value> {};
217 
218 /*
219  * Determines if a EigenBase<> type is a compile-time non-column-vector matrix
220  * of a scalar type. This will not check for run-time size.
221  * @note For an EigenBase<> of the correct Scalar type, this logic is
222  * exclusive to is_eigen_vector_of<> such that distinct specializations are not
223  * ambiguous.
224  */
225 // TODO(eric.cousineau): A 1x1 matrix will be disqualified in this case, and
226 // this logic will qualify it as a vector. Address the downstream logic if this
227 // becomes an issue.
228 template <typename Derived, typename Scalar>
230  : std::integral_constant<
231  bool, is_eigen_scalar_same<Derived, Scalar>::value &&
232  !is_eigen_vector<Derived>::value> {};
233 
234 // TODO(eric.cousineau): Add alias is_eigen_matrix_of = is_eigen_scalar_same if
235 // appropriate.
236 
237 /// This wrapper class provides a way to write non-template functions taking raw
238 /// pointers to Eigen objects as parameters while limiting the number of copies,
239 /// similar to `Eigen::Ref`. Internally, it keeps an instance of `Eigen::Ref<T>`
240 /// and provides access to it via `operator*` and `operator->`.
241 ///
242 /// The motivation of this class is to follow <a
243 /// href="https://google.github.io/styleguide/cppguide.html#Reference_Arguments">GSG's
244 /// "output arguments should be pointers" rule</a> while taking advantage of
245 /// using `Eigen::Ref`. Here is an example.
246 ///
247 /// @code
248 /// // This function is taking an Eigen::Ref of a matrix and modifies it in
249 /// // the body. This violates GSG's rule on output parameters.
250 /// void foo(Eigen::Ref<Eigen::MatrixXd> M) {
251 /// M(0, 0) = 0;
252 /// }
253 /// // At Call-site, we have:
254 /// foo(M);
255 /// foo(M.block(0, 0, 2, 2));
256 ///
257 /// // We can rewrite the above function into the following using EigenPtr.
258 /// void foo(EigenPtr<Eigen::MatrixXd> M) {
259 /// (*M)(0, 0) = 0;
260 /// }
261 /// // Note that, call sites should be changed to:
262 /// foo(&M);
263 
264 /// // We need tmp to avoid taking the address of a temporary object such as the
265 /// // return value of .block().
266 /// auto tmp = M.block(0, 0, 2, 2);
267 /// foo(&tmp);
268 /// @endcode
269 ///
270 /// @note This class provides a way to avoid the `const_cast` hack introduced in
271 /// <a
272 /// href="https://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html#TopicPlainFunctionsFailing">Eigen's
273 /// documentation</a>.
274 template <typename PlainObjectType>
275 class EigenPtr {
276  public:
277  typedef Eigen::Ref<PlainObjectType> RefType;
278 
279  EigenPtr() : EigenPtr(nullptr) {}
280 
281  /// Overload for `nullptr`.
282  // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
283  EigenPtr(std::nullptr_t) {}
284 
285  /// Constructs with a reference to the given matrix type.
286  // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
287  EigenPtr(const EigenPtr& other) { assign(other); }
288 
289  /// Constructs with a reference to another matrix type.
290  /// May be `nullptr`.
291  template <typename PlainObjectTypeIn>
292  // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
293  EigenPtr(PlainObjectTypeIn* m) {
294  if (m) {
295  m_.reset(new RefType(*m));
296  }
297  }
298 
299  /// Constructs from another EigenPtr.
300  template <typename PlainObjectTypeIn>
301  // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
303  // Cannot directly construct `m_` from `other.m_`.
304  assign(other);
305  }
306 
307  EigenPtr& operator=(const EigenPtr& other) {
308  // We must explicitly override this version of operator=.
309  // The template below will not take precedence over this one.
310  return assign(other);
311  }
312 
313  template <typename PlainObjectTypeIn>
315  return assign(other);
316  }
317 
318  /// @throws std::runtime_error if this is a null dereference.
319  RefType& operator*() const { return *get_reference(); }
320 
321  /// @throws std::runtime_error if this is a null dereference.
322  RefType* operator->() const { return get_reference(); }
323 
324  /// Returns whether or not this contains a valid reference.
325  operator bool() const { return is_valid(); }
326 
327  bool operator==(std::nullptr_t) const { return !is_valid(); }
328 
329  bool operator!=(std::nullptr_t) const { return is_valid(); }
330 
331  private:
332  // Use unique_ptr<> so that we may "reconstruct" the reference, making this
333  // a pointer-like type.
334  // TODO(eric.cousineau): Consider using a stack-based implementation if
335  // performance is a concern, possibly with a mutable member.
336  std::unique_ptr<RefType> m_;
337 
338  // Consolidate assignment here, so that both the copy constructor and the
339  // construction from another type may be used.
340  template <typename PlainObjectTypeIn>
341  EigenPtr& assign(const EigenPtr<PlainObjectTypeIn>& other) {
342  if (other) {
343  m_.reset(new RefType(*other));
344  } else {
345  m_.reset();
346  }
347  return *this;
348  }
349 
350  // Consolidate getting a reference here.
351  RefType* get_reference() const {
352  if (!m_) throw std::runtime_error("EigenPtr: nullptr dereference");
353  return m_.get();
354  }
355 
356  bool is_valid() const {
357  return static_cast<bool>(m_);
358  }
359 };
360 
361 } // namespace drake
EigenSizeMinPreferFixed is a variant of EigenSizeMinPreferDynamic.
Definition: eigen_types.h:164
Eigen::Matrix< double, Eigen::Dynamic, 1, 0, 73, 1 > VectorUpTo73d
A column vector of dynamic size, up to a maximum of 73 elements.
Definition: eigen_types.h:113
Definition: eigen_types.h:194
Eigen::Transform< Scalar, 3, Eigen::Isometry > Isometry3
An Isometry templated on scalar type.
Definition: eigen_types.h:106
Eigen::Quaternion< Scalar > Quaternion
A quaternion templated on scalar type.
Definition: eigen_types.h:98
RefType * operator->() const
Definition: eigen_types.h:322
Eigen::AngleAxis< Scalar > AngleAxis
An AngleAxis templated on scalar type.
Definition: eigen_types.h:102
RefType & operator*() const
Definition: eigen_types.h:319
bool operator!=(std::nullptr_t) const
Definition: eigen_types.h:329
Eigen::Matrix< Scalar, 6, 6 > Matrix6
A matrix of 6 rows and 6 columns, templated on scalar type.
Definition: eigen_types.h:67
Eigen::Matrix< Scalar, 2, Eigen::Dynamic > Matrix2X
A matrix of 2 rows, dynamic columns, templated on scalar type.
Definition: eigen_types.h:71
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:88
Eigen::Matrix< Scalar, 4, Eigen::Dynamic > Matrix4X
A matrix of 4 rows, dynamic columns, templated on scalar type.
Definition: eigen_types.h:79
Eigen::Matrix< Scalar, kTwistSize, Eigen::Dynamic > TwistMatrix
A matrix with one twist per column, and dynamically many columns.
Definition: eigen_types.h:121
EigenSizeMinPreferDynamic<a, b>::value gives the min between compile-time sizes a and b...
Definition: eigen_types.h:148
EigenPtr(std::nullptr_t)
Overload for nullptr.
Definition: eigen_types.h:283
Definition: eigen_types.h:229
Eigen::Translation< Scalar, 3 > Translation3
A translation in 3D templated on scalar type.
Definition: eigen_types.h:110
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:178
Eigen::Matrix< Scalar, 6, Eigen::Dynamic > Matrix6X
A matrix of 6 rows, dynamic columns, templated on scalar type.
Definition: eigen_types.h:83
EigenPtr(PlainObjectTypeIn *m)
Constructs with a reference to another matrix type.
Definition: eigen_types.h:293
Eigen::Matrix< Scalar, 2, 2 > Matrix2
A matrix of 2 rows and 2 columns, templated on scalar type.
Definition: eigen_types.h:55
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
EigenPtr(const EigenPtr &other)
Constructs with a reference to the given matrix type.
Definition: eigen_types.h:287
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:87
Eigen::Matrix< Scalar, kTwistSize, 1 > TwistVector
A column vector consisting of one twist.
Definition: eigen_types.h:117
Eigen::Matrix< Scalar, 6, 1 > SpatialForce
A column vector consisting of a concatenated rotational and translational force.
Definition: eigen_types.h:139
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:132
Definition: eigen_types.h:204
bool operator==(std::nullptr_t) const
Definition: eigen_types.h:327
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:275
EigenPtr & operator=(const EigenPtr &other)
Definition: eigen_types.h:307
EigenPtr & operator=(const EigenPtr< PlainObjectTypeIn > &other)
Definition: eigen_types.h:314
EigenPtr(const EigenPtr< PlainObjectTypeIn > &other)
Constructs from another EigenPtr.
Definition: eigen_types.h:302
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:277
EigenPtr()
Definition: eigen_types.h:279
Definition: eigen_types.h:213
Eigen::Matrix< Scalar, 4, 4 > Matrix4
A matrix of 4 rows and 4 columns, templated on scalar type.
Definition: eigen_types.h:63
Eigen::Matrix< Scalar, 3, 3 > Matrix3
A matrix of 3 rows and 3 columns, templated on scalar type.
Definition: eigen_types.h:59
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:150
Eigen::Matrix< Scalar, 3, Eigen::Dynamic > Matrix3X
A matrix of 3 rows, dynamic columns, templated on scalar type.
Definition: eigen_types.h:75
Eigen::Matrix< Scalar, kTwistSize, kTwistSize > SquareTwistMatrix
A six-by-six matrix.
Definition: eigen_types.h:125
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:94
Definition: eigen_types.h:188
Provides careful macros to selectively enable or disable the special member functions for copy-constr...