pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
motion-base.hpp
1 //
2 // Copyright (c) 2015-2019 CNRS INRIA
3 // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #ifndef __pinocchio_motion_base_hpp__
7 #define __pinocchio_motion_base_hpp__
8 
9 namespace pinocchio
10 {
11 
12  template<class Derived>
13  class MotionBase
14  {
15  public:
16  MOTION_TYPEDEF_TPL(Derived);
17 
18  Derived & derived() { return *static_cast<Derived*>(this); }
19  const Derived & derived() const { return *static_cast<const Derived*>(this); }
20 
21  ConstAngularType angular() const { return derived().angular_impl(); }
22  ConstLinearType linear() const { return derived().linear_impl(); }
23  AngularType angular() { return derived().angular_impl(); }
24  LinearType linear() { return derived().linear_impl(); }
25 
26  template<typename V3Like>
27  void angular(const Eigen::MatrixBase<V3Like> & w)
28  { derived().angular_impl(w.derived()); }
29 
30  template<typename V3Like>
31  void linear(const Eigen::MatrixBase<V3Like> & v)
32  { derived().linear_impl(v.derived()); }
33 
34  operator PlainReturnType() const { return derived().plain(); }
35  PlainReturnType plain() const { return derived().plain(); }
36 
37  ToVectorConstReturnType toVector() const { return derived().toVector_impl(); }
38  ToVectorReturnType toVector() { return derived().toVector_impl(); }
39  operator Vector6() const { return toVector(); }
40 
41  ActionMatrixType toActionMatrix() const { return derived().toActionMatrix_impl(); }
42  ActionMatrixType toDualActionMatrix() const { return derived().toDualActionMatrix_impl(); }
43  operator Matrix6() const { return toActionMatrix(); }
44 
45  void setZero() { derived().setZero(); }
46 
47  template<typename M2>
48  bool operator==(const MotionBase<M2> & other) const
49  { return derived().isEqual_impl(other.derived()); }
50 
51  template<typename M2>
52  bool operator!=(const MotionBase<M2> & other) const
53  { return !(derived() == other.derived()); }
54 
55  Derived operator-() const { return derived().__opposite__(); }
56  Derived operator+(const MotionBase<Derived> & v) const { return derived().__plus__(v.derived()); }
57  Derived operator-(const MotionBase<Derived> & v) const { return derived().__minus__(v.derived()); }
58  Derived & operator+=(const MotionBase<Derived> & v) { return derived().__pequ__(v.derived()); }
59  Derived & operator-=(const MotionBase<Derived> & v) { return derived().__mequ__(v.derived()); }
60 
61  template<typename OtherScalar>
62  typename internal::RHSScalarMultiplication<Derived,OtherScalar>::ReturnType
63  operator*(const OtherScalar & alpha) const
64  { return derived().__mult__(alpha); }
65 
66  template<typename OtherScalar>
67  Derived operator/(const OtherScalar & alpha) const
68  { return derived().__div__(alpha); }
69 
70  template<typename OtherSpatialType>
71  typename MotionAlgebraAction<OtherSpatialType,Derived>::ReturnType
72  cross(const OtherSpatialType & d) const
73  {
74  return derived().cross_impl(d);
75  }
76 
77  bool isApprox(const Derived & other, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
78  { return derived().isApprox_impl(other, prec);}
79 
80  bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
81  { return derived().isZero_impl(prec);}
82 
83  template<typename S2, int O2>
84  typename SE3GroupAction<Derived>::ReturnType
85  se3Action(const SE3Tpl<S2,O2> & m) const
86  { return derived().se3Action_impl(m); }
87 
88  template<typename S2, int O2>
89  typename SE3GroupAction<Derived>::ReturnType
90  se3ActionInverse(const SE3Tpl<S2,O2> & m) const
91  { return derived().se3ActionInverse_impl(m); }
92 
93  template<typename ForceDerived>
94  Scalar dot(const ForceDense<ForceDerived> & f) const { return derived().dot(f.derived()); }
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  template<typename MotionDerived>
106  typename internal::RHSScalarMultiplication<MotionDerived,typename MotionDerived::Scalar>::ReturnType
107  operator*(const typename MotionDerived::Scalar & alpha,
108  const MotionBase<MotionDerived> & motion)
109  {
110  return motion*alpha;
111  }
112 
113 } // namespace pinocchio
114 
115 #endif // ifndef __pinocchio_motion_base_hpp__
MotionTpl< Scalar, Options > motion(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointMotionVisitor to get the joint internal motion as a dense motion...
Main pinocchio namespace.
Definition: treeview.dox:24