pinocchio  UNKNOWN
motion-base.hpp
1 //
2 // Copyright (c) 2015-2018 CNRS
3 // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 // This file is part of Pinocchio
6 // Pinocchio is free software: you can redistribute it
7 // and/or modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation, either version
9 // 3 of the License, or (at your option) any later version.
10 //
11 // Pinocchio is distributed in the hope that it will be
12 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 // General Lesser Public License for more details. You should have
15 // received a copy of the GNU Lesser General Public License along with
16 // Pinocchio If not, see
17 // <http://www.gnu.org/licenses/>.
18 
19 #ifndef __se3_motion_base_hpp__
20 #define __se3_motion_base_hpp__
21 
22 namespace se3
23 {
24 
25  template<class Derived>
26  class MotionBase
27  {
28  public:
29  MOTION_TYPEDEF_TPL(Derived);
30 
31  Derived & derived() { return *static_cast<Derived*>(this); }
32  const Derived& derived() const { return *static_cast<const Derived*>(this); }
33 
34  ConstAngularType angular() const { return derived().angular_impl(); }
35  ConstLinearType linear() const { return derived().linear_impl(); }
36  AngularType angular() { return derived().angular_impl(); }
37  LinearType linear() { return derived().linear_impl(); }
38 
39  template<typename V3Like>
40  void angular(const Eigen::MatrixBase<V3Like> & w)
41  { derived().angular_impl(w.derived()); }
42 
43  template<typename V3Like>
44  void linear(const Eigen::MatrixBase<V3Like> & v)
45  { derived().linear_impl(v.derived()); }
46 
47  ToVectorConstReturnType toVector() const { return derived().toVector_impl(); }
48  ToVectorReturnType toVector() { return derived().toVector_impl(); }
49  operator Vector6() const { return toVector(); }
50 
51  ActionMatrixType toActionMatrix() const { return derived().toActionMatrix_impl(); }
52  ActionMatrixType toDualActionMatrix() const { return derived().toDualActionMatrix_impl(); }
53  operator Matrix6() const { return toActionMatrix(); }
54 
55  template<typename M2>
56  bool operator==(const MotionBase<M2> & other) const
57  { return derived().isEqual_impl(other.derived()); }
58 
59  template<typename M2>
60  bool operator!=(const MotionBase<M2> & other) const
61  { return !(derived() == other.derived()); }
62 
63  MotionPlain operator-() const { return derived().__opposite__(); }
64  MotionPlain operator+(const Derived & v) const { return derived().__plus__(v); }
65  MotionPlain operator-(const Derived & v) const { return derived().__minus__(v); }
66  Derived & operator+=(const Derived & v) { return derived().__pequ__(v); }
67  Derived & operator-=(const Derived & v) { return derived().__mequ__(v); }
68 
69  template<typename OtherScalar>
70  MotionPlain operator*(const OtherScalar & alpha) const
71  { return derived().__mult__(alpha); }
72  template<typename OtherScalar>
73  MotionPlain operator/(const OtherScalar & alpha) const
74  { return derived().__div__(alpha); }
75 
76  template<typename OtherSpatialType>
77  typename internal::MotionAlgebraAction<OtherSpatialType,Derived>::ReturnType
78  cross(const OtherSpatialType & d) const
79  {
80  return derived().cross_impl(d);
81  }
82 
83  bool isApprox(const Derived & other, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
84  { return derived().isApprox_impl(other, prec);}
85 
86  template<typename S2, int O2>
87  MotionPlain se3Action(const SE3Tpl<S2,O2> & m) const
88  { return derived().se3Action_impl(m); }
89 
90  template<typename S2, int O2>
91  MotionPlain se3ActionInverse(const SE3Tpl<S2,O2> & m) const
92  { return derived().se3ActionInverse_impl(m); }
93 
94  Scalar dot(const Force & f) const { return derived().dot(f); }
95 
96  void disp(std::ostream & os) const { derived().disp_impl(os); }
97  friend std::ostream & operator << (std::ostream & os, const MotionBase<Derived> & v)
98  {
99  v.disp(os);
100  return os;
101  }
102 
103  }; // class MotionBase
104 
105 } // namespace se3
106 
107 #endif // ifndef __se3_motion_base_hpp__