spatial-dyn
spatial_motion_base.h
1 
10 #ifndef EIGEN_SPATIAL_MOTION_BASE_H_
11 #define EIGEN_SPATIAL_MOTION_BASE_H_
12 
13 #include "spatial_declarations.h"
14 
15 namespace spatial_dyn {
16 
17 template<typename Derived>
18 class SpatialMotionBase : public Eigen::DenseBase<Derived> {
19 
20  public:
21  typedef SpatialMotionBase StorageBaseType;
22  typedef typename Eigen::internal::traits<Derived>::StorageKind StorageKind;
23  typedef typename Eigen::internal::traits<Derived>::StorageIndex StorageIndex;
24  typedef typename Eigen::internal::traits<Derived>::Scalar Scalar;
25  typedef typename Eigen::internal::packet_traits<Scalar>::type PacketScalar;
26  typedef typename Eigen::NumTraits<Scalar>::Real RealScalar;
27 
28  typedef Eigen::DenseBase<Derived> Base;
29  using Base::RowsAtCompileTime;
30  using Base::ColsAtCompileTime;
31  using Base::SizeAtCompileTime;
32  using Base::MaxRowsAtCompileTime;
33  using Base::MaxColsAtCompileTime;
34  using Base::MaxSizeAtCompileTime;
35  using Base::IsVectorAtCompileTime;
36  using Base::Flags;
37 
38  using Base::derived;
39  using Base::const_cast_derived;
40  using Base::rows;
41  using Base::cols;
42  using Base::size;
43  using Base::coeff;
44  using Base::coeffRef;
45  using Base::lazyAssign;
46  using Base::eval;
47  using Base::operator+=;
48  using Base::operator-=;
49  using Base::operator*=;
50  using Base::operator/=;
51 
52  typedef typename Base::CoeffReturnType CoeffReturnType;
53  typedef typename Base::ConstTransposeReturnType ConstTransposeReturnType;
54  typedef typename Base::RowXpr RowXpr;
55  typedef typename Base::ColXpr ColXpr;
56 
57  typedef SpatialMotion<Scalar,6> SquareMatrixType;
58 
59  typedef SpatialMotion<Scalar, ColsAtCompileTime,
60  Eigen::AutoAlign |
61  (Flags & Eigen::RowMajorBit ? Eigen::RowMajor : Eigen::ColMajor),
62  MaxColsAtCompileTime> PlainObject;
63 
64  typedef Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<Scalar>,
65  PlainObject> ConstantReturnType;
66 
67  typedef Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<Scalar>,
68  PlainObject> IdentityReturnType;
69 
70  typedef Eigen::Block<const Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<Scalar>,
71  SquareMatrixType>,
72  6, ColsAtCompileTime> BasisReturnType;
73 
74  typedef Eigen::Block<Eigen::MatrixWrapper<Derived>, 3, ColsAtCompileTime> CartesianBlockType;
75 
76  typedef const Eigen::Block<const Eigen::MatrixWrapper<const Derived>, 3,
77  ColsAtCompileTime> ConstCartesianBlockType;
78 
79  template<typename OtherDerived>
80  struct SpatialOpTraits {
81  typedef typename Eigen::ScalarBinaryOpTraits<Scalar,
82  typename OtherDerived::Scalar>::ReturnType ScalarT;
83  typedef SpatialMotion<ScalarT, ColsAtCompileTime> MotionReturnType;
84  typedef SpatialForce<ScalarT, ColsAtCompileTime> ForceReturnType;
85  };
86 
87  // #include "Eigen/src/plugins/CommonCwiseUnaryOps.h"
88  typedef Eigen::CwiseUnaryOp<Eigen::internal::scalar_opposite_op<Scalar>,
89  const Derived> NegativeReturnType;
90 
91  const NegativeReturnType operator-() const;
92 
93  template<class NewType>
94  struct CastXpr {
95  typedef typename Eigen::internal::cast_return_type<Derived,
96  const Eigen::CwiseUnaryOp<Eigen::internal::scalar_cast_op<Scalar, NewType>,
97  const Derived>>::type Type;
98  };
99 
100  template<typename NewType>
101  typename CastXpr<NewType>::Type cast() const;
102 
103  template<typename CustomUnaryOp>
104  inline const Eigen::CwiseUnaryOp<CustomUnaryOp, const Derived>
105  unaryExpr(const CustomUnaryOp& func = CustomUnaryOp()) const;
106 
107  template<typename CustomViewOp>
108  inline const Eigen::CwiseUnaryView<CustomViewOp, const Derived>
109  unaryViewExpr(const CustomViewOp& func = CustomViewOp()) const;
110 
111  // #include "Eigen/src/plugins/CommonCwiseBinaryOps.h"
112  template<typename OtherDerived>
113  struct BinaryOpTraits {
114  typedef Eigen::CwiseBinaryOp<Eigen::internal::scalar_difference_op<Scalar,
115  typename OtherDerived::Scalar>,
116  const Derived, const OtherDerived> DifferenceType;
117  typedef Eigen::CwiseBinaryOp<Eigen::internal::scalar_sum_op<Scalar,
118  typename OtherDerived::Scalar>,
119  const Derived, const OtherDerived> SumType;
120  };
121 
122  // EIGEN_MAKE_CWISE_BINARY_OP(operator-,difference)
123  template<typename OtherDerived>
124  const typename BinaryOpTraits<OtherDerived>::DifferenceType
125  operator-(const SpatialMotionBase<OtherDerived> &other) const;
126 
127  // EIGEN_MAKE_CWISE_BINARY_OP(operator+,sum)
128  template<typename OtherDerived>
129  inline const typename BinaryOpTraits<OtherDerived>::SumType
130  operator+(const SpatialMotionBase<OtherDerived> &other) const;
131 
132  template<typename T>
133  struct ScalarOpTraits {
134  typedef typename Eigen::ScalarBinaryOpTraits<Scalar, T,
135  Eigen::internal::scalar_product_op<Scalar, T>> RightProduct;
136  typedef typename Eigen::ScalarBinaryOpTraits<T, Scalar,
137  Eigen::internal::scalar_product_op<T, Scalar>> LeftProduct;
138  typedef typename Eigen::ScalarBinaryOpTraits<Scalar, T,
139  Eigen::internal::scalar_product_op<Scalar, T>> RightQuotient;
140  };
141 
142  // EIGEN_MAKE_SCALAR_BINARY_OP_ONTHERIGHT(operator*,product)
143  template<typename T>
144  Eigen::CwiseBinaryOp<
145  Eigen::internal::scalar_product_op<
146  Scalar,
147  typename Eigen::internal::promote_scalar_arg<Scalar, T,
148  Eigen::internal::has_ReturnType<typename ScalarOpTraits<T>::RightProduct>::value>::type>,
149  const Derived,
150  const typename Eigen::internal::plain_constant_type<
151  Derived,
152  typename Eigen::internal::promote_scalar_arg<Scalar, T,
153  Eigen::internal::has_ReturnType<typename ScalarOpTraits<T>::RightProduct>::value>::type>::type>
154  operator*(const T& scalar) const;
155 
156  // EIGEN_MAKE_SCALAR_BINARY_OP_ONTHELEFT(operator*,product)
157  // See defined function below
158 
159  // EIGEN_MAKE_SCALAR_BINARY_OP_ONTHERIGHT(operator/,quotient)
160  template<typename T>
161  Eigen::CwiseBinaryOp<
162  Eigen::internal::scalar_quotient_op<
163  Scalar,
164  typename Eigen::internal::promote_scalar_arg<Scalar, T,
165  Eigen::internal::has_ReturnType<typename ScalarOpTraits<T>::RightQuotient>::value>::type>,
166  const Derived,
167  const typename Eigen::internal::plain_constant_type<
168  Derived,
169  typename Eigen::internal::promote_scalar_arg<Scalar, T,
170  Eigen::internal::has_ReturnType<typename ScalarOpTraits<T>::RightQuotient>::value>::type>::type>
171  operator/(const T& scalar) const;
172 
173  template<typename CustomBinaryOp, typename OtherDerived>
174  inline const Eigen::CwiseBinaryOp<CustomBinaryOp, const Derived, const OtherDerived>
175  binaryExpr(const SpatialMotionBase<OtherDerived> &other,
176  const CustomBinaryOp& func = CustomBinaryOp()) const;
177 
178  Derived& operator=(const SpatialMotionBase& other);
179 
180  template <typename OtherDerived>
181  Derived& operator=(const Eigen::DenseBase<OtherDerived>& other);
182 
183  template <typename OtherDerived>
184  Derived& operator=(const Eigen::EigenBase<OtherDerived>& other);
185 
186  template<typename OtherDerived>
187  Derived& operator=(const Eigen::ReturnByValue<OtherDerived>& other);
188 
189  template<typename OtherDerived>
190  Derived& operator+=(const SpatialMotionBase<OtherDerived>& other);
191 
192  template<typename OtherDerived>
193  Derived& operator+=(const SpatialForceBase<OtherDerived>& other);
194 
195  template<typename OtherDerived>
196  Derived& operator-=(const SpatialMotionBase<OtherDerived>& other);
197 
198  template<typename OtherDerived>
199  Derived& operator-=(const SpatialForceBase<OtherDerived>& other);
200 
201  template<typename OtherDerived>
202  const Eigen::Product<Derived, OtherDerived>
203  operator*(const SpatialMotionBase<OtherDerived>& other) const;
204 
205  template<typename OtherDerived>
206  SpatialMotion<typename SpatialOpTraits<OtherDerived>::ScalarT, OtherDerived::ColsAtCompileTime>
207  operator*(const Eigen::MatrixBase<OtherDerived>& other) const;
208 
209  template<typename OtherDerived>
210  Derived& operator*=(const SpatialMotionBase<OtherDerived>& other);
211 
212  template<typename CustomNullaryOp>
213  static const Eigen::CwiseNullaryOp<CustomNullaryOp, PlainObject>
214  NullaryExpr(Eigen::Index rows, Eigen::Index cols, const CustomNullaryOp& func);
215 
216  template<typename CustomNullaryOp>
217  static const Eigen::CwiseNullaryOp<CustomNullaryOp, PlainObject>
218  NullaryExpr(Eigen::Index cols, const CustomNullaryOp& func);
219 
220  template<typename CustomNullaryOp>
221  static const Eigen::CwiseNullaryOp<CustomNullaryOp, PlainObject>
222  NullaryExpr(const CustomNullaryOp& func);
223 
224  static const ConstantReturnType Constant(Eigen::Index cols, const Scalar& value);
225  static const ConstantReturnType Constant(const Scalar& value);
226  static const ConstantReturnType Zero(Eigen::Index cols);
227  static const ConstantReturnType Zero();
228  static const ConstantReturnType Ones(Eigen::Index cols);
229  static const ConstantReturnType Ones();
230 
231  static const IdentityReturnType Identity();
232  static const IdentityReturnType Identity(Eigen::Index cols);
233  static const BasisReturnType Unit(Eigen::Index i);
234  static const BasisReturnType UnitLinX();
235  static const BasisReturnType UnitLinY();
236  static const BasisReturnType UnitLinZ();
237  static const BasisReturnType UnitAngX();
238  static const BasisReturnType UnitAngY();
239  static const BasisReturnType UnitAngZ();
240 
241  Derived& setIdentity();
242  Derived& setIdentity(Eigen::Index cols);
243  Derived& setUnit(Eigen::Index i);
244 
245  template<typename OtherDerived>
246  typename SpatialOpTraits<OtherDerived>::ScalarT
247  dot(const SpatialForceBase<OtherDerived>& other) const;
248 
249  template<typename OtherDerived>
250  typename SpatialOpTraits<OtherDerived>::ScalarT
251  dot(const SpatialMotionBase<OtherDerived>& other) const;
252 
253  template<typename OtherDerived>
254  typename SpatialOpTraits<OtherDerived>::MotionReturnType
255  cross(const SpatialMotionBase<OtherDerived>& other) const;
256 
257  template<typename OtherDerived>
258  typename SpatialOpTraits<OtherDerived>::ForceReturnType
259  cross(const SpatialForceBase<OtherDerived>& other) const;
260 
261  template<typename OtherDerived>
262  bool operator==(const SpatialMotionBase<OtherDerived>& other) const;
263 
264  template<typename OtherDerived>
265  bool operator!=(const SpatialMotionBase<OtherDerived>& other) const;
266 
267  Eigen::MatrixWrapper<Derived> matrix();
268  const Eigen::MatrixWrapper<const Derived> matrix() const;
269 
270  Eigen::ArrayWrapper<Derived> array();
271  const Eigen::ArrayWrapper<const Derived> array() const;
272 
273  Eigen::Transpose<Eigen::MatrixWrapper<Derived>> transpose();
274  const Eigen::Transpose<Eigen::MatrixWrapper<const Derived>> transpose() const;
275 
276  CartesianBlockType linear();
277  ConstCartesianBlockType linear() const;
278 
279  CartesianBlockType angular();
280  ConstCartesianBlockType angular() const;
281 
282  protected:
283  SpatialMotionBase() : Base() {}
284 
285  private:
286  // TODO: Implement?
287  template<typename OtherDerived>
288  explicit SpatialMotionBase(const SpatialMotionBase<OtherDerived>&);
289 
290 };
291 
292 // Transform operations
293 template<typename Scalar, int Dim, typename Derived>
294 inline typename Derived::PlainObject
295 operator*(const Eigen::Translation<Scalar, Dim>& T, const SpatialMotionBase<Derived>& m) {
296  static_assert(Dim == 3, "YOU_CAN_ONLY_APPLY_3D_TRANSFORMS_TO_SPATIAL_MOTIONS");
297  typename Derived::PlainObject result = m; // Evaluate expr
298  result.template topRows<3>() -= m.matrix().template bottomRows<3>().colwise().cross(T.translation());
299  return result;
300 }
301 
302 template<typename Scalar, int Dim, int Mode, int Options, typename Derived>
303 inline typename Derived::PlainObject
304 operator*(const Eigen::Transform<Scalar, Dim, Mode, Options>& T, const SpatialMotionBase<Derived>& m) {
305  static_assert(Dim == 3, "YOU_CAN_ONLY_APPLY_3D_TRANSFORMS_TO_SPATIAL_MOTIONS");
306  static_assert(Mode == int(Eigen::Isometry), "YOU_CAN_ONLY_APPLY_ISOMETRY_TRANSFORMS_TO_SPATIAL_MOTIONS");
307 
308  typename Derived::PlainObject result = m; // Evaluate expr
309  result.template bottomRows<3>() = T.linear() * result.matrix().template bottomRows<3>();
310  result.template topRows<3>() = T.linear() * result.matrix().template topRows<3>() -
311  result.matrix().template bottomRows<3>().colwise().cross(T.translation());
312  return result;
313 }
314 
315 // from src/plugins/CommonCwiseUnaryOps.h
316 template<typename Derived>
317 inline const typename SpatialMotionBase<Derived>::NegativeReturnType
318 SpatialMotionBase<Derived>::operator-() const {
319  return NegativeReturnType(derived());
320 }
321 
322 template<typename Derived>
323 template<typename NewType>
324 inline typename SpatialMotionBase<Derived>::template CastXpr<NewType>::Type
325 SpatialMotionBase<Derived>::cast() const {
326  return typename CastXpr<NewType>::Type(derived());
327 }
328 
329 template<typename Derived>
330 template<typename CustomUnaryOp>
331 inline const Eigen::CwiseUnaryOp<CustomUnaryOp, const Derived>
332 SpatialMotionBase<Derived>::unaryExpr(const CustomUnaryOp& func) const {
333  return Eigen::CwiseUnaryOp<CustomUnaryOp, const Derived>(derived(), func);
334 }
335 
336 template<typename Derived>
337 template<typename CustomViewOp>
338 inline const Eigen::CwiseUnaryView<CustomViewOp, const Derived>
339 SpatialMotionBase<Derived>::unaryViewExpr(const CustomViewOp& func) const {
340  return Eigen::CwiseUnaryView<CustomViewOp, const Derived>(derived(), func);
341 }
342 
343 // from src/plugins/CommonCwiseBinaryOps.h
344 template<typename Derived>
345 template<typename OtherDerived>
346 inline const typename SpatialMotionBase<Derived>::template BinaryOpTraits<OtherDerived>::DifferenceType
347 SpatialMotionBase<Derived>::operator-(const SpatialMotionBase<OtherDerived> &other) const {
348  return typename BinaryOpTraits<OtherDerived>::DifferenceType(derived(), other.derived());
349 }
350 
351 template<typename Derived>
352 template<typename OtherDerived>
353 inline const typename SpatialMotionBase<Derived>::template BinaryOpTraits<OtherDerived>::SumType
354 SpatialMotionBase<Derived>::operator+(const SpatialMotionBase<OtherDerived> &other) const {
355  return typename BinaryOpTraits<OtherDerived>::SumType(derived(), other.derived());
356 }
357 
358 template<typename Derived>
359 template<typename T>
360 inline Eigen::CwiseBinaryOp<
361  Eigen::internal::scalar_product_op<
362  typename SpatialMotionBase<Derived>::Scalar,
363  typename Eigen::internal::promote_scalar_arg<typename SpatialMotionBase<Derived>::Scalar, T,
364  Eigen::internal::has_ReturnType<
365  typename SpatialMotionBase<Derived>::template ScalarOpTraits<T>::RightProduct>::value>::type>,
366  const Derived,
367  const typename Eigen::internal::plain_constant_type<
368  Derived,
369  typename Eigen::internal::promote_scalar_arg<typename SpatialMotionBase<Derived>::Scalar, T,
370  Eigen::internal::has_ReturnType<
371  typename SpatialMotionBase<Derived>::template ScalarOpTraits<T>::RightProduct>::value>::type>::type>
372 SpatialMotionBase<Derived>::operator*(const T& scalar) const {
373  typedef typename Eigen::internal::promote_scalar_arg<Scalar, T,
374  Eigen::internal::has_ReturnType<typename ScalarOpTraits<T>::RightProduct>::value>::type PromotedT;
375  typedef typename Eigen::internal::plain_constant_type<Derived, PromotedT>::type PlainType;
376  typedef typename Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<Scalar, PromotedT>,
377  const Derived, const PlainType> ReturnType;
378  typedef typename Eigen::internal::scalar_constant_op<PromotedT> ScalarOp;
379  return ReturnType(derived(), PlainType(derived().rows(), derived().cols(), ScalarOp(scalar)));
380 }
381 
382 template<typename T, typename Derived>
383 inline Eigen::CwiseBinaryOp<
384  Eigen::internal::scalar_product_op<
385  typename Eigen::internal::promote_scalar_arg<typename SpatialMotionBase<Derived>::Scalar, T,
386  Eigen::internal::has_ReturnType<
387  typename SpatialMotionBase<Derived>::template ScalarOpTraits<T>::LeftProduct>::value>::type,
388  typename SpatialMotionBase<Derived>::Scalar>,
389  const typename Eigen::internal::plain_constant_type<
390  Derived,
391  typename Eigen::internal::promote_scalar_arg<typename SpatialMotionBase<Derived>::Scalar, T,
392  Eigen::internal::has_ReturnType<
393  typename SpatialMotionBase<Derived>::template ScalarOpTraits<T>::LeftProduct>::value>::type>::type,
394  const Derived>
395 operator*(const T& scalar, const SpatialMotionBase<Derived>& m) {
396  typedef typename SpatialMotionBase<Derived>::template ScalarOpTraits<T> ScalarOpTraits;
397  typedef typename SpatialMotionBase<Derived>::Scalar Scalar;
398 
399  typedef typename Eigen::internal::promote_scalar_arg<Scalar, T,
400  Eigen::internal::has_ReturnType<typename ScalarOpTraits::LeftProduct>::value>::type PromotedT;
401  typedef typename Eigen::internal::plain_constant_type<Derived, PromotedT>::type PlainType;
402  typedef typename Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<PromotedT, Scalar>,
403  const PlainType, const Derived> ReturnType;
404  typedef typename Eigen::internal::scalar_constant_op<PromotedT> ScalarOp;
405  return ReturnType(PlainType(m.derived().rows(), m.derived().cols(), ScalarOp(scalar)), m.derived());
406 }
407 
408 template<typename Derived>
409 template<typename T>
410 inline Eigen::CwiseBinaryOp<
411  Eigen::internal::scalar_quotient_op<
412  typename SpatialMotionBase<Derived>::Scalar,
413  typename Eigen::internal::promote_scalar_arg<typename SpatialMotionBase<Derived>::Scalar, T,
414  Eigen::internal::has_ReturnType<
415  typename SpatialMotionBase<Derived>::template ScalarOpTraits<T>::RightQuotient>::value>::type>,
416  const Derived,
417  const typename Eigen::internal::plain_constant_type<
418  Derived,
419  typename Eigen::internal::promote_scalar_arg<typename SpatialMotionBase<Derived>::Scalar, T,
420  Eigen::internal::has_ReturnType<
421  typename SpatialMotionBase<Derived>::template ScalarOpTraits<T>::RightQuotient>::value>::type>::type>
422 SpatialMotionBase<Derived>::operator/(const T& scalar) const {
423  typedef typename Eigen::internal::promote_scalar_arg<Scalar, T,
424  Eigen::internal::has_ReturnType<typename ScalarOpTraits<T>::RightQuotient>::value>::type PromotedT;
425  typedef typename Eigen::internal::plain_constant_type<Derived, PromotedT>::type PlainType;
426  typedef typename Eigen::CwiseBinaryOp<Eigen::internal::scalar_quotient_op<Scalar, PromotedT>,
427  const Derived, const PlainType> ReturnType;
428  typedef typename Eigen::internal::scalar_constant_op<PromotedT> ScalarOp;
429  return ReturnType(derived(), PlainType(derived().rows(), derived().cols(), ScalarOp(scalar)));
430 }
431 
432 template<typename Derived>
433 template<typename CustomBinaryOp, typename OtherDerived>
434 inline const Eigen::CwiseBinaryOp<CustomBinaryOp, const Derived, const OtherDerived>
435 SpatialMotionBase<Derived>::binaryExpr(const SpatialMotionBase<OtherDerived> &other,
436  const CustomBinaryOp& func) const {
437  return Eigen::CwiseBinaryOp<CustomBinaryOp, const Derived,
438  const OtherDerived>(derived(), other.derived(), func);
439 }
440 
441 // from Assign.h
442 template<typename Derived>
443 inline Derived& SpatialMotionBase<Derived>::operator=(const SpatialMotionBase& other) {
444  Eigen::internal::call_assignment(derived(), other.derived());
445  return derived();
446 }
447 
448 template<typename Derived>
449 template<typename OtherDerived>
450 inline Derived& SpatialMotionBase<Derived>::operator=(const Eigen::DenseBase<OtherDerived>& other) {
451  Eigen::internal::call_assignment(derived(), other.derived());
452  return derived();
453 }
454 
455 template<typename Derived>
456 template<typename OtherDerived>
457 inline Derived& SpatialMotionBase<Derived>::operator=(const Eigen::EigenBase<OtherDerived>& other) {
458  Eigen::internal::call_assignment(derived(), other.derived());
459  return derived();
460 }
461 
462 template<typename Derived>
463 template<typename OtherDerived>
464 inline Derived& SpatialMotionBase<Derived>::operator=(const Eigen::ReturnByValue<OtherDerived>& other) {
465  other.derived().evalTo(derived());
466  return derived();
467 }
468 
469 // from CwiseBinaryOp.h
470 template<typename Derived>
471 template<typename OtherDerived>
472 inline Derived& SpatialMotionBase<Derived>::operator+=(const SpatialMotionBase<OtherDerived>& other) {
473  Eigen::internal::call_assignment(derived(), other.derived(),
474  Eigen::internal::add_assign_op<Scalar, typename OtherDerived::Scalar>());
475  return derived();
476 }
477 
478 template<typename Derived>
479 template<typename OtherDerived>
480 inline Derived& SpatialMotionBase<Derived>::operator+=(const SpatialForceBase<OtherDerived>&) {
481  static_assert(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar)) == -1,
482  "YOU_CANNOT_ADD_SPATIAL_MOTION_AND_SPATIAL_FORCE_VECTORS");
483  return *this;
484 };
485 
486 template<typename Derived>
487 template<typename OtherDerived>
488 inline Derived& SpatialMotionBase<Derived>::operator-=(const SpatialMotionBase<OtherDerived>& other) {
489  Eigen::internal::call_assignment(derived(), other.derived(),
490  Eigen::internal::sub_assign_op<Scalar,typename OtherDerived::Scalar>());
491  return derived();
492 }
493 
494 template<typename Derived>
495 template<typename OtherDerived>
496 inline Derived& SpatialMotionBase<Derived>::operator-=(const SpatialForceBase<OtherDerived>&) {
497  static_assert(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar)) == -1,
498  "YOU_CANNOT_ADD_SPATIAL_MOTION_AND_SPATIAL_FORCE_VECTORS");
499  return *this;
500 }
501 
502 template<typename Derived>
503 template<typename OtherDerived>
504 inline const Eigen::Product<Derived, OtherDerived>
505 SpatialMotionBase<Derived>::operator*(const SpatialMotionBase<OtherDerived>&) const {
506  static_assert(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar)) == -1,
507  "YOU_CANNOT_MULTIPLY_TWO_SPATIAL_MOTION_VECTORS");
508 }
509 
510 template<typename Derived>
511 template<typename OtherDerived>
512 inline SpatialMotion<typename SpatialMotionBase<Derived>::template SpatialOpTraits<OtherDerived>::ScalarT,
513  OtherDerived::ColsAtCompileTime>
514 SpatialMotionBase<Derived>::operator*(const Eigen::MatrixBase<OtherDerived>& other) const {
515  static_assert(ColsAtCompileTime == Eigen::Dynamic ||
516  OtherDerived::RowsAtCompileTime == Eigen::Dynamic ||
517  int(ColsAtCompileTime) == int(OtherDerived::RowsAtCompileTime),
518  "INVALID_MATRIX_PRODUCT");
519  // TODO: Make not lazy
520  return Eigen::Product<Derived, OtherDerived, Eigen::LazyProduct>(derived(), other.derived());
521 }
522 
523 
524 template<typename Derived>
525 template<typename OtherDerived>
526 inline Derived& SpatialMotionBase<Derived>::operator*=(const SpatialMotionBase<OtherDerived>&) {
527  static_assert(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar)) == -1,
528  "YOU_CANNOT_MULTIPLY_TWO_SPATIAL_MOTION_VECTORS");
529 }
530 
531 // from CwiseNullaryOp.h
532 template<typename Derived>
533 template<typename CustomNullaryOp>
534 inline const Eigen::CwiseNullaryOp<CustomNullaryOp,
535  typename SpatialMotionBase<Derived>::PlainObject>
536 SpatialMotionBase<Derived>::NullaryExpr(Eigen::Index rows, Eigen::Index cols,
537  const CustomNullaryOp& func) {
538  return Eigen::CwiseNullaryOp<CustomNullaryOp, PlainObject>(6, cols, func);
539 }
540 
541 template<typename Derived>
542 template<typename CustomNullaryOp>
543 inline const Eigen::CwiseNullaryOp<CustomNullaryOp,
544  typename SpatialMotionBase<Derived>::PlainObject>
545 SpatialMotionBase<Derived>::NullaryExpr(Eigen::Index cols, const CustomNullaryOp& func) {
546  return Eigen::CwiseNullaryOp<CustomNullaryOp, PlainObject>(6, cols, func);
547 }
548 
549 template<typename Derived>
550 template<typename CustomNullaryOp>
551 inline const Eigen::CwiseNullaryOp<CustomNullaryOp,
552  typename SpatialMotionBase<Derived>::PlainObject>
553 SpatialMotionBase<Derived>::NullaryExpr(const CustomNullaryOp& func) {
554  return Eigen::CwiseNullaryOp<CustomNullaryOp, PlainObject>(6, ColsAtCompileTime, func);
555 }
556 
557 template<typename Derived>
558 inline const typename SpatialMotionBase<Derived>::ConstantReturnType
559 SpatialMotionBase<Derived>::Constant(Eigen::Index cols, const Scalar& value) {
560  return NullaryExpr(cols, Eigen::internal::scalar_constant_op<Scalar>(value));
561 };
562 
563 template<typename Derived>
564 inline const typename SpatialMotionBase<Derived>::ConstantReturnType
565 SpatialMotionBase<Derived>::Constant(const Scalar& value) {
566  static_assert(ColsAtCompileTime != Eigen::Dynamic, "MATRIX_MUST_BE_OF_FIXED_SIZE");
567  return NullaryExpr(ColsAtCompileTime, Eigen::internal::scalar_constant_op<Scalar>(value));
568 };
569 
570 template<typename Derived>
571 inline const typename SpatialMotionBase<Derived>::ConstantReturnType
572 SpatialMotionBase<Derived>::Zero(Eigen::Index cols) {
573  return Constant(cols, Scalar(0));
574 };
575 
576 template<typename Derived>
577 inline const typename SpatialMotionBase<Derived>::ConstantReturnType
578 SpatialMotionBase<Derived>::Zero() {
579  return Constant(Scalar(0));
580 };
581 
582 template<typename Derived>
583 inline const typename SpatialMotionBase<Derived>::ConstantReturnType
584 SpatialMotionBase<Derived>::Ones(Eigen::Index cols) {
585  return Constant(cols, Scalar(1));
586 };
587 
588 template<typename Derived>
589 inline const typename SpatialMotionBase<Derived>::ConstantReturnType
590 SpatialMotionBase<Derived>::Ones() {
591  return Constant(Scalar(1));
592 };
593 
594 template<typename Derived>
595 inline const typename SpatialMotionBase<Derived>::IdentityReturnType
596 SpatialMotionBase<Derived>::Identity() {
597  static_assert(ColsAtCompileTime != Eigen::Dynamic, "MATRIX_MUST_BE_OF_FIXED_SIZE");
598  return NullaryExpr(ColsAtCompileTime, Eigen::internal::scalar_identity_op<Scalar>());
599 }
600 
601 template<typename Derived>
602 inline const typename SpatialMotionBase<Derived>::IdentityReturnType
603 SpatialMotionBase<Derived>::Identity(Eigen::Index cols) {
604  return NullaryExpr(cols, Eigen::internal::scalar_identity_op<Scalar>());
605 }
606 
607 template<typename Derived>
608 inline const typename SpatialMotionBase<Derived>::BasisReturnType
609 SpatialMotionBase<Derived>::Unit(Eigen::Index i) {
610  static_assert(ColsAtCompileTime == 1, "MATRIX_MUST_BE_A_VECTOR");
611  return BasisReturnType(SquareMatrixType::Identity(), i);
612 }
613 
614 template<typename Derived>
615 inline const typename SpatialMotionBase<Derived>::BasisReturnType
616 SpatialMotionBase<Derived>::UnitLinX() {
617  return Derived::Unit(0);
618 }
619 
620 template<typename Derived>
621 inline const typename SpatialMotionBase<Derived>::BasisReturnType
622 SpatialMotionBase<Derived>::UnitLinY() {
623  return Derived::Unit(1);
624 }
625 
626 template<typename Derived>
627 inline const typename SpatialMotionBase<Derived>::BasisReturnType
628 SpatialMotionBase<Derived>::UnitLinZ() {
629  return Derived::Unit(2);
630 }
631 
632 template<typename Derived>
633 inline const typename SpatialMotionBase<Derived>::BasisReturnType
634 SpatialMotionBase<Derived>::UnitAngX() {
635  return Derived::Unit(3);
636 }
637 
638 template<typename Derived>
639 inline const typename SpatialMotionBase<Derived>::BasisReturnType
640 SpatialMotionBase<Derived>::UnitAngY() {
641  return Derived::Unit(4);
642 }
643 
644 template<typename Derived>
645 inline const typename SpatialMotionBase<Derived>::BasisReturnType
646 SpatialMotionBase<Derived>::UnitAngZ() {
647  return Derived::Unit(5);
648 }
649 
650 template<typename Derived>
651 inline Derived& SpatialMotionBase<Derived>::setIdentity() {
652  return Eigen::internal::setIdentity_impl<Derived>::run(derived());
653 }
654 
655 template<typename Derived>
656 inline Derived& SpatialMotionBase<Derived>::setIdentity(Eigen::Index cols) {
657  derived().resize(Eigen::NoChange, cols);
658  return setIdentity();
659 }
660 
661 template<typename Derived>
662 inline Derived& SpatialMotionBase<Derived>::setUnit(Eigen::Index i) {
663  static_assert(ColsAtCompileTime == 1, "MATRIX_MUST_BE_A_VECTOR");
664  eigen_assert(i < size());
665  derived().setZero();
666  derived().coeffRef(i) = Scalar(1);
667  return derived();
668 }
669 
670 // from Dot.h
671 template<typename Derived>
672 template<typename OtherDerived>
673 inline typename SpatialMotionBase<Derived>::template SpatialOpTraits<OtherDerived>::ScalarT
674 SpatialMotionBase<Derived>::dot(const SpatialMotionBase<OtherDerived>&) const {
675  static_assert(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar)) == -1,
676  "YOU_CANNOT_DOT_TWO_SPATIAL_MOTION_VECTORS");
677  return 0;
678 }
679 
680 template<typename Derived>
681 template<typename OtherDerived>
682 inline typename SpatialMotionBase<Derived>::template SpatialOpTraits<OtherDerived>::ScalarT
683 SpatialMotionBase<Derived>::dot(const SpatialForceBase<OtherDerived>& other) const {
684  static_assert(ColsAtCompileTime == 1, "MATRIX_MUST_BE_A_VECTOR");
685  static_assert(OtherDerived::ColsAtCompileTime == 1, "MATRIX_MUST_BE_A_VECTOR");
686  typedef Eigen::internal::scalar_conj_product_op<Scalar, typename OtherDerived::Scalar> conj_prod;
687  return Eigen::CwiseBinaryOp<conj_prod, const Derived, const OtherDerived>(
688  derived(), other.derived(), conj_prod()).sum();
689 }
690 
691 // from Geometry/OrthoMethods.h
692 template<typename Derived>
693 template<typename OtherDerived>
694 inline typename SpatialMotionBase<Derived>::template SpatialOpTraits<OtherDerived>::MotionReturnType
695 SpatialMotionBase<Derived>::cross(const SpatialMotionBase<OtherDerived>& other) const {
696  static_assert(ColsAtCompileTime == 1, "MATRIX_MUST_BE_A_VECTOR");
697  static_assert(OtherDerived::ColsAtCompileTime == 1, "MATRIX_MUST_BE_A_VECTOR");
698  typename Eigen::internal::nested_eval<Derived,4>::type lhs(derived());
699  typename Eigen::internal::nested_eval<OtherDerived,4>::type rhs(other.derived());
700  return typename SpatialOpTraits<OtherDerived>::MotionReturnType(
701  lhs.coeff(1) * rhs.coeff(5) - lhs.coeff(2) * rhs.coeff(4) + lhs.coeff(4) * rhs.coeff(2) - lhs.coeff(5) * rhs.coeff(1),
702  lhs.coeff(2) * rhs.coeff(3) - lhs.coeff(0) * rhs.coeff(5) + lhs.coeff(5) * rhs.coeff(0) - lhs.coeff(3) * rhs.coeff(2),
703  lhs.coeff(0) * rhs.coeff(4) - lhs.coeff(1) * rhs.coeff(3) + lhs.coeff(3) * rhs.coeff(1) - lhs.coeff(4) * rhs.coeff(0),
704  lhs.coeff(4) * rhs.coeff(5) - lhs.coeff(5) * rhs.coeff(4),
705  lhs.coeff(5) * rhs.coeff(3) - lhs.coeff(3) * rhs.coeff(5),
706  lhs.coeff(3) * rhs.coeff(4) - lhs.coeff(4) * rhs.coeff(3)
707  );
708 }
709 
710 template<typename Derived>
711 template<typename OtherDerived>
712 inline typename SpatialMotionBase<Derived>::template SpatialOpTraits<OtherDerived>::ForceReturnType
713 SpatialMotionBase<Derived>::cross(const SpatialForceBase<OtherDerived>& other) const {
714  static_assert(ColsAtCompileTime == 1, "MATRIX_MUST_BE_A_VECTOR");
715  static_assert(OtherDerived::ColsAtCompileTime == 1, "MATRIX_MUST_BE_A_VECTOR");
716  typename Eigen::internal::nested_eval<Derived,4>::type lhs(derived());
717  typename Eigen::internal::nested_eval<OtherDerived,4>::type rhs(other.derived());
718  return typename SpatialOpTraits<OtherDerived>::ForceReturnType(
719  lhs.coeff(4) * rhs.coeff(2) - lhs.coeff(5) * rhs.coeff(1),
720  lhs.coeff(5) * rhs.coeff(0) - lhs.coeff(3) * rhs.coeff(2),
721  lhs.coeff(3) * rhs.coeff(1) - lhs.coeff(4) * rhs.coeff(0),
722  lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1) + lhs.coeff(4) * rhs.coeff(5) - lhs.coeff(5) * rhs.coeff(4),
723  lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2) + lhs.coeff(5) * rhs.coeff(3) - lhs.coeff(3) * rhs.coeff(5),
724  lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0) + lhs.coeff(3) * rhs.coeff(4) - lhs.coeff(4) * rhs.coeff(3)
725  );
726 }
727 
728 template<typename Derived>
729 template<typename OtherDerived>
730 inline bool SpatialMotionBase<Derived>::operator==(const SpatialMotionBase<OtherDerived>& other) const {
731  return cwiseEqual(other).all();
732 }
733 
734 template<typename Derived>
735 template<typename OtherDerived>
736 inline bool SpatialMotionBase<Derived>::operator!=(const SpatialMotionBase<OtherDerived>& other) const {
737  return cwiseNotEqual(other).any();
738 }
739 
740 template<typename Derived>
741 inline Eigen::MatrixWrapper<Derived> SpatialMotionBase<Derived>::matrix() {
742  return Eigen::MatrixWrapper<Derived>(derived());
743 }
744 
745 template<typename Derived>
746 inline const Eigen::MatrixWrapper<const Derived> SpatialMotionBase<Derived>::matrix() const {
747  return Eigen::MatrixWrapper<const Derived>(derived());
748 }
749 
750 template<typename Derived>
751 inline Eigen::ArrayWrapper<Derived> SpatialMotionBase<Derived>::array() {
752  return Eigen::ArrayWrapper<Derived>(derived());
753 }
754 
755 template<typename Derived>
756 inline const Eigen::ArrayWrapper<const Derived> SpatialMotionBase<Derived>::array() const {
757  return Eigen::ArrayWrapper<const Derived>(derived());
758 }
759 
760 template<typename Derived>
761 inline Eigen::Transpose<Eigen::MatrixWrapper<Derived>>
762 SpatialMotionBase<Derived>::transpose() {
763  return Eigen::MatrixWrapper<Derived>(derived()).transpose();
764 }
765 
766 template<typename Derived>
767 inline const Eigen::Transpose<Eigen::MatrixWrapper<const Derived>>
768 SpatialMotionBase<Derived>::transpose() const {
769  return Eigen::MatrixWrapper<const Derived>(derived()).transpose();
770 }
771 
772 template<typename Derived>
773 inline typename SpatialMotionBase<Derived>::CartesianBlockType
774 SpatialMotionBase<Derived>::linear() {
775  return this->matrix().template topRows<3>();
776 }
777 
778 template<typename Derived>
779 inline typename SpatialMotionBase<Derived>::ConstCartesianBlockType
780 SpatialMotionBase<Derived>::linear() const {
781  return this->matrix().template topRows<3>();
782 }
783 
784 template<typename Derived>
785 inline typename SpatialMotionBase<Derived>::CartesianBlockType
786 SpatialMotionBase<Derived>::angular() {
787  return this->matrix().template bottomRows<3>();
788 }
789 
790 template<typename Derived>
791 inline typename SpatialMotionBase<Derived>::ConstCartesianBlockType
792 SpatialMotionBase<Derived>::angular() const {
793  return this->matrix().template bottomRows<3>();
794 }
795 
796 } // namespace Eigen
797 
798 #endif // EIGEN_SPATIAL_MOTION_BASE_H_
spatial_dyn::SpatialForceBase
Definition: spatial_force_base.h:18
spatial_dyn::SpatialForce
Definition: spatial_force.h:18
spatial_dyn::SpatialMotionBase
Definition: spatial_motion_base.h:18
spatial_dyn::SpatialMotion
Definition: spatial_motion.h:18
spatial_dyn
Definition: discrete_dynamics.cc:21
spatial_dyn::SpatialMotionBase::BinaryOpTraits
Definition: spatial_motion_base.h:113
spatial_dyn::SpatialMotionBase::CastXpr
Definition: spatial_motion_base.h:94
spatial_dyn::SpatialMotionBase::ScalarOpTraits
Definition: spatial_motion_base.h:133
spatial_dyn::SpatialMotionBase::SpatialOpTraits
Definition: spatial_motion_base.h:80
Generated by doxygen 1.9.1