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