pinocchio  2.2.1-dirty
se3-base.hpp
1 //
2 // Copyright (c) 2015-2018 CNRS
3 // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #ifndef __pinocchio_se3_base_hpp__
7 #define __pinocchio_se3_base_hpp__
8 
9 namespace pinocchio
10 {
25  template<class Derived>
26  struct SE3Base
27  {
28  PINOCCHIO_SE3_TYPEDEF_TPL(Derived);
29 
30  Derived & derived() { return *static_cast<Derived*>(this); }
31  const Derived& derived() const { return *static_cast<const Derived*>(this); }
32 
33  ConstAngularRef rotation() const { return derived().rotation_impl(); }
34  ConstLinearRef translation() const { return derived().translation_impl(); }
35  AngularRef rotation() { return derived().rotation_impl(); }
36  LinearRef translation() { return derived().translation_impl(); }
37  void rotation(const AngularType & R) { derived().rotation_impl(R); }
38  void translation(const LinearType & t) { derived().translation_impl(t); }
39 
40  HomogeneousMatrixType toHomogeneousMatrix() const
41  {
42  return derived().toHomogeneousMatrix_impl();
43  }
44  operator HomogeneousMatrixType() const { return toHomogeneousMatrix(); }
45 
56  ActionMatrixType toActionMatrix() const
57  {
58  return derived().toActionMatrix_impl();
59  }
60  operator ActionMatrixType() const { return toActionMatrix(); }
61 
66  ActionMatrixType toActionMatrixInverse() const
67  {
68  return derived().toActionMatrixInverse_impl();
69  }
70 
71  ActionMatrixType toDualActionMatrix() const
72  { return derived().toDualActionMatrix_impl(); }
73 
74  void disp(std::ostream & os) const
75  {
76  static_cast<const Derived*>(this)->disp_impl(os);
77  }
78 
80  operator*(const Derived & m2) const
81  { return derived().__mult__(m2); }
82 
84  template<typename D>
86  act(const D & d) const
87  {
88  return derived().act_impl(d);
89  }
90 
92  template<typename D> typename SE3GroupAction<D>::ReturnType
93  actInv(const D & d) const
94  {
95  return derived().actInv_impl(d);
96  }
97 
98  bool operator==(const Derived & other) const
99  { return derived().__equal__(other); }
100 
101  bool operator!=(const Derived & other) const
102  { return !(*this == other); }
103 
104  bool isApprox(const Derived & other, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
105  {
106  return derived().isApprox_impl(other, prec);
107  }
108 
109  friend std::ostream & operator <<(std::ostream & os,const SE3Base<Derived> & X)
110  {
111  X.disp(os);
112  return os;
113  }
114 
118  bool isIdentity(const typename traits<Derived>::Scalar & prec = Eigen::NumTraits<typename traits<Derived>::Scalar>::dummy_precision()) const
119  {
120  return derived().isIdentity(prec);
121  }
122 
123  }; // struct SE3Base
124 
125 } // namespace pinocchio
126 
127 #endif // ifndef __pinocchio_se3_base_hpp__
SE3GroupAction< D >::ReturnType actInv(const D &d) const
by = aXb.actInv(ay)
Definition: se3-base.hpp:93
SE3GroupAction< D >::ReturnType act(const D &d) const
ay = aXb.act(by)
Definition: se3-base.hpp:86
Main pinocchio namespace.
Definition: treeview.dox:24
ActionMatrixType toActionMatrixInverse() const
The action matrix of .
Definition: se3-base.hpp:66
ActionMatrixType toActionMatrix() const
The action matrix of .
Definition: se3-base.hpp:56
Common traits structure to fully define base classes for CRTP.
Definition: spatial/fwd.hpp:32
bool isIdentity(const typename traits< Derived >::Scalar &prec=Eigen::NumTraits< typename traits< Derived >::Scalar >::dummy_precision()) const
Definition: se3-base.hpp:118