motion-zero.hpp 3.58 KB
Newer Older
1
//
2
// Copyright (c) 2015-2019 CNRS, INRIA
3
4
5
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

6
7
#ifndef __pinocchio_motion_zero_hpp__
#define __pinocchio_motion_zero_hpp__
8

9
namespace pinocchio
10
{
11
  
12
  template<typename Scalar, int Options>
13
  struct SE3GroupAction< MotionZeroTpl<Scalar,Options> >
14
  {
15
    typedef MotionZeroTpl<Scalar,Options> ReturnType;
16
17
18
  };
  
  template<typename Scalar, int Options, typename MotionDerived>
19
  struct MotionAlgebraAction< MotionZeroTpl<Scalar,Options>, MotionDerived>
20
  {
21
    typedef MotionZeroTpl<Scalar,Options> ReturnType;
22
  };
23

24
  template<typename _Scalar, int _Options>
25
  struct traits< MotionZeroTpl<_Scalar,_Options> >
26
  {
27
28
29
30
31
32
33
34
35
36
    enum {
      Options = _Options,
      LINEAR = 0,
      ANGULAR = 3
    };
    typedef _Scalar Scalar;
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
    typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
gabrielebndn's avatar
gabrielebndn committed
37
38
    typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
    typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
39
40
41
42
43
44
    typedef Matrix6 ActionMatrixType;
    typedef Vector3 AngularType;
    typedef const Vector3 ConstAngularType;
    typedef Vector3 LinearType;
    typedef const Vector3 ConstLinearType;
    typedef Motion MotionPlain;
45
    typedef MotionPlain PlainReturnType;
46
    
47
  }; // traits MotionZeroTpl
48
  
49
  template<typename Scalar, int Options>
50
51
  struct MotionZeroTpl
  : public MotionBase< MotionZeroTpl<Scalar,Options> >
52
  {
53
54
    typedef typename traits<MotionZeroTpl>::MotionPlain MotionPlain;
    typedef typename traits<MotionZeroTpl>::PlainReturnType PlainReturnType;
55
    
56
    static PlainReturnType plain() { return MotionPlain::Zero(); }
57
58
59
60
61
    
    template<typename D2>
    static bool isEqual_impl(const MotionDense<D2> & other)
    { return other.linear().isZero() && other.angular().isZero(); }
    
62
    template<typename D2>
63
64
65
66
67
68
69
    static void addTo(const MotionBase<D2> &) {}
    
    template<typename D2>
    static void setTo(MotionBase<D2> & other)
    {
      other.setZero();
    }
70
    
71
    template<typename M1>
72
    MotionZeroTpl motionAction(const MotionBase<M1> &) const
73
    {
74
      return MotionZeroTpl();
75
76
77
78
79
80
81
82
83
    }
    
    template<typename S2, int O2, typename D2>
    void se3Action_impl(const SE3Tpl<S2,O2> &, MotionDense<D2> & v) const
    {
      v.setZero();
    }
    
    template<typename S2, int O2>
84
    MotionZeroTpl se3Action_impl(const SE3Tpl<S2,O2> &) const
85
    {
86
      return MotionZeroTpl();
87
88
89
90
91
92
93
94
95
    }
    
    template<typename S2, int O2, typename D2>
    void se3ActionInverse_impl(const SE3Tpl<S2,O2> &, MotionDense<D2> & v) const
    {
      v.setZero();
    }
    
    template<typename S2, int O2>
96
    MotionZeroTpl se3ActionInverse_impl(const SE3Tpl<S2,O2> &) const
97
    {
98
      return MotionZeroTpl();
99
100
    }
    
101
  }; // struct MotionZeroTpl
102
  
103
  template<typename M1, typename Scalar, int Options>
104
  inline const M1 & operator+(const MotionBase<M1> & v,
105
                              const MotionZeroTpl<Scalar,Options> &)
106
107
  { return v.derived(); }
  
108
  template<typename Scalar, int Options, typename M1>
109
  inline const M1 & operator+(const MotionZeroTpl<Scalar,Options> &,
110
111
                              const MotionBase<M1> & v)
  { return v.derived(); }
112
113
114
115
116

  /// \brief BiasZeroTpl has been replaced by MotionZeroTpl. Please use this naming instead.
  template<typename Scalar, int Options>
  struct PINOCCHIO_DEPRECATED BiasZeroTpl : MotionZeroTpl<Scalar,Options>
  {};
117
  
118
} // namespace pinocchio
119

120
#endif // ifndef __pinocchio_motion_zero_hpp__