pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
se3-base.hpp
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
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 {
29  template<class Derived>
30  struct SE3Base
31  {
32  PINOCCHIO_SE3_TYPEDEF_TPL(Derived);
33 
34  Derived & derived() { return *static_cast<Derived*>(this); }
35  const Derived& derived() const { return *static_cast<const Derived*>(this); }
36 
37  ConstAngularRef rotation() const { return derived().rotation_impl(); }
38  ConstLinearRef translation() const { return derived().translation_impl(); }
39  AngularRef rotation() { return derived().rotation_impl(); }
40  LinearRef translation() { return derived().translation_impl(); }
41  void rotation(const AngularType & R) { derived().rotation_impl(R); }
42  void translation(const LinearType & t) { derived().translation_impl(t); }
43 
44  HomogeneousMatrixType toHomogeneousMatrix() const
45  {
46  return derived().toHomogeneousMatrix_impl();
47  }
48  operator HomogeneousMatrixType() const { return toHomogeneousMatrix(); }
49 
60  ActionMatrixType toActionMatrix() const
61  {
62  return derived().toActionMatrix_impl();
63  }
64  operator ActionMatrixType() const { return toActionMatrix(); }
65 
70  ActionMatrixType toActionMatrixInverse() const
71  {
72  return derived().toActionMatrixInverse_impl();
73  }
74 
75  ActionMatrixType toDualActionMatrix() const
76  { return derived().toDualActionMatrix_impl(); }
77 
78  void disp(std::ostream & os) const
79  {
80  static_cast<const Derived*>(this)->disp_impl(os);
81  }
82 
84  operator*(const Derived & m2) const
85  { return derived().__mult__(m2); }
86 
88  template<typename D>
90  act(const D & d) const
91  {
92  return derived().act_impl(d);
93  }
94 
96  template<typename D> typename SE3GroupAction<D>::ReturnType
97  actInv(const D & d) const
98  {
99  return derived().actInv_impl(d);
100  }
101 
102  bool operator==(const Derived & other) const
103  { return derived().isEqual(other); }
104 
105  bool operator!=(const Derived & other) const
106  { return !(*this == other); }
107 
108  bool isApprox(const Derived & other, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
109  {
110  return derived().isApprox_impl(other, prec);
111  }
112 
113  friend std::ostream & operator <<(std::ostream & os,const SE3Base<Derived> & X)
114  {
115  X.disp(os);
116  return os;
117  }
118 
122  bool isIdentity(const typename traits<Derived>::Scalar & prec = Eigen::NumTraits<typename traits<Derived>::Scalar>::dummy_precision()) const
123  {
124  return derived().isIdentity(prec);
125  }
126 
130  bool isNormalized(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
131  {
132  return derived().isNormalized(prec);
133  }
134 
138  void normalize()
139  {
140  derived().normalize();
141  }
142 
146  PlainType normalized() const
147  {
148  derived().normalized();
149  }
150 
151  }; // struct SE3Base
152 
153 } // namespace pinocchio
154 
155 #endif // ifndef __pinocchio_se3_base_hpp__
ActionMatrixType toActionMatrixInverse() const
The action matrix of .
Definition: se3-base.hpp:70
bool isNormalized(const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition: se3-base.hpp:130
PlainType normalized() const
Definition: se3-base.hpp:146
bool isIdentity(const typename traits< Derived >::Scalar &prec=Eigen::NumTraits< typename traits< Derived >::Scalar >::dummy_precision()) const
Definition: se3-base.hpp:122
ActionMatrixType toActionMatrix() const
The action matrix of .
Definition: se3-base.hpp:60
Main pinocchio namespace.
Definition: treeview.dox:24
Base class for rigid transformation.
Definition: se3-base.hpp:30
void normalize()
Normalize *this in such a way the rotation part of *this lies on SO(3).
Definition: se3-base.hpp:138
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:35
SE3GroupAction< D >::ReturnType actInv(const D &d) const
by = aXb.actInv(ay)
Definition: se3-base.hpp:97
SE3GroupAction< D >::ReturnType act(const D &d) const
ay = aXb.act(by)
Definition: se3-base.hpp:90