Drake
eigen_types.h
Go to the documentation of this file.
1 #pragma once
2 
11 
12 #include <memory>
13 
14 #include <Eigen/Dense>
15 
16 #include "drake/common/constants.h"
18 
19 namespace drake {
20 
22 template <typename Scalar>
23 using Vector1 = Eigen::Matrix<Scalar, 1, 1>;
24 
26 using Vector1d = Eigen::Matrix<double, 1, 1>;
27 
29 template <typename Scalar>
30 using Vector2 = Eigen::Matrix<Scalar, 2, 1>;
31 
33 template <typename Scalar>
34 using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
35 
37 template <typename Scalar>
38 using Vector4 = Eigen::Matrix<Scalar, 4, 1>;
39 
41 template <typename Scalar>
42 using Vector6 = Eigen::Matrix<Scalar, 6, 1>;
43 
45 template <typename Scalar>
46 using VectorX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
47 
50 template <typename Scalar>
51 using VectorUpTo6 = Eigen::Matrix<Scalar, Eigen::Dynamic, 1, 0, 6, 1>;
52 
54 template <typename Scalar>
55 using Matrix2 = Eigen::Matrix<Scalar, 2, 2>;
56 
58 template <typename Scalar>
59 using Matrix3 = Eigen::Matrix<Scalar, 3, 3>;
60 
62 template <typename Scalar>
63 using Matrix4 = Eigen::Matrix<Scalar, 4, 4>;
64 
66 template <typename Scalar>
67 using Matrix6 = Eigen::Matrix<Scalar, 6, 6>;
68 
70 template <typename Scalar>
71 using Matrix2X = Eigen::Matrix<Scalar, 2, Eigen::Dynamic>;
72 
74 template <typename Scalar>
75 using Matrix3X = Eigen::Matrix<Scalar, 3, Eigen::Dynamic>;
76 
78 template <typename Scalar>
79 using Matrix4X = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>;
80 
82 template <typename Scalar>
83 using Matrix6X = Eigen::Matrix<Scalar, 6, Eigen::Dynamic>;
84 
86 template <typename Scalar>
87 using MatrixX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
88 
92 template <typename Scalar>
93 using MatrixUpTo6 =
94 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, 0, 6, 6>;
95 
97 template <typename Scalar>
98 using Quaternion = Eigen::Quaternion<Scalar>;
99 
101 template <typename Scalar>
102 using AngleAxis = Eigen::AngleAxis<Scalar>;
103 
105 template <typename Scalar>
106 using Isometry3 = Eigen::Transform<Scalar, 3, Eigen::Isometry>;
107 
109 template <typename Scalar>
110 using Translation3 = Eigen::Translation<Scalar, 3>;
111 
113 using VectorUpTo73d = Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 73, 1>;
114 
116 template <typename Scalar>
117 using TwistVector = Eigen::Matrix<Scalar, kTwistSize, 1>;
118 
120 template <typename Scalar>
121 using TwistMatrix = Eigen::Matrix<Scalar, kTwistSize, Eigen::Dynamic>;
122 
124 template <typename Scalar>
125 using SquareTwistMatrix = Eigen::Matrix<Scalar, kTwistSize, kTwistSize>;
126 
131 template <typename Scalar>
132 using WrenchVector = Eigen::Matrix<Scalar, 6, 1>;
133 
138 template <typename Scalar>
139 using SpatialForce = Eigen::Matrix<Scalar, 6, 1>;
140 
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 
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 
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 
263 
274 template <typename PlainObjectType>
275 class EigenPtr {
276  public:
277  typedef Eigen::Ref<PlainObjectType> RefType;
278 
279  EigenPtr() : EigenPtr(nullptr) {}
280 
282  // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
283  EigenPtr(std::nullptr_t) {}
284 
286  // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
287  EigenPtr(const EigenPtr& other) { assign(other); }
288 
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 
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 
319  RefType& operator*() const { return *get_reference(); }
320 
322  RefType* operator->() const { return get_reference(); }
323 
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
int b
Definition: rgbd_camera.cc:91
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...