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_zero_hpp__ |
7 |
|
|
#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 |
|
|
enum { |
28 |
|
|
Options = _Options, |
29 |
|
|
LINEAR = 0, |
30 |
|
|
ANGULAR = 3 |
31 |
|
|
}; |
32 |
|
|
typedef _Scalar Scalar; |
33 |
|
|
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3; |
34 |
|
|
typedef Eigen::Matrix<Scalar,6,1,Options> Vector6; |
35 |
|
|
typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3; |
36 |
|
|
typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4; |
37 |
|
|
typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6; |
38 |
|
|
typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType; |
39 |
|
|
typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType; |
40 |
|
|
typedef Matrix6 ActionMatrixType; |
41 |
|
|
typedef Matrix4 HomogeneousMatrixType; |
42 |
|
|
typedef Vector3 AngularType; |
43 |
|
|
typedef const Vector3 ConstAngularType; |
44 |
|
|
typedef Vector3 LinearType; |
45 |
|
|
typedef const Vector3 ConstLinearType; |
46 |
|
|
typedef Motion MotionPlain; |
47 |
|
|
typedef MotionPlain PlainReturnType; |
48 |
|
|
|
49 |
|
|
}; // traits MotionZeroTpl |
50 |
|
|
|
51 |
|
|
template<typename Scalar, int Options> |
52 |
|
|
struct MotionZeroTpl |
53 |
|
|
: public MotionBase< MotionZeroTpl<Scalar,Options> > |
54 |
|
|
{ |
55 |
|
|
typedef typename traits<MotionZeroTpl>::MotionPlain MotionPlain; |
56 |
|
|
typedef typename traits<MotionZeroTpl>::PlainReturnType PlainReturnType; |
57 |
|
|
|
58 |
|
|
static PlainReturnType plain() { return MotionPlain::Zero(); } |
59 |
|
|
|
60 |
|
|
template<typename D2> |
61 |
|
23 |
static bool isEqual_impl(const MotionDense<D2> & other) |
62 |
|
|
{ |
63 |
✓✗✓✗
✓✗✓✗
✓✗✓✗
|
23 |
return other.linear().isZero(0) && other.angular().isZero(0); |
64 |
|
|
} |
65 |
|
|
|
66 |
|
627 |
static bool isEqual_impl(const MotionZeroTpl &) |
67 |
|
|
{ |
68 |
|
627 |
return true; |
69 |
|
|
} |
70 |
|
|
|
71 |
|
|
template<typename D2> |
72 |
|
32 |
static void addTo(const MotionBase<D2> &) {} |
73 |
|
|
|
74 |
|
|
template<typename D2> |
75 |
|
670 |
static void setTo(MotionBase<D2> & other) |
76 |
|
|
{ |
77 |
|
670 |
other.setZero(); |
78 |
|
670 |
} |
79 |
|
|
|
80 |
|
|
template<typename M1> |
81 |
|
1 |
MotionZeroTpl motionAction(const MotionBase<M1> &) const |
82 |
|
|
{ |
83 |
|
1 |
return MotionZeroTpl(); |
84 |
|
|
} |
85 |
|
|
|
86 |
|
|
template<typename S2, int O2, typename D2> |
87 |
|
|
void se3Action_impl(const SE3Tpl<S2,O2> &, MotionDense<D2> & v) const |
88 |
|
|
{ |
89 |
|
|
v.setZero(); |
90 |
|
|
} |
91 |
|
|
|
92 |
|
|
template<typename S2, int O2> |
93 |
|
1 |
MotionZeroTpl se3Action_impl(const SE3Tpl<S2,O2> &) const |
94 |
|
|
{ |
95 |
|
1 |
return MotionZeroTpl(); |
96 |
|
|
} |
97 |
|
|
|
98 |
|
|
template<typename S2, int O2, typename D2> |
99 |
|
|
void se3ActionInverse_impl(const SE3Tpl<S2,O2> &, MotionDense<D2> & v) const |
100 |
|
|
{ |
101 |
|
|
v.setZero(); |
102 |
|
|
} |
103 |
|
|
|
104 |
|
|
template<typename S2, int O2> |
105 |
|
33 |
MotionZeroTpl se3ActionInverse_impl(const SE3Tpl<S2,O2> &) const |
106 |
|
|
{ |
107 |
|
33 |
return MotionZeroTpl(); |
108 |
|
|
} |
109 |
|
|
|
110 |
|
|
}; // struct MotionZeroTpl |
111 |
|
|
|
112 |
|
|
template<typename M1, typename Scalar, int Options> |
113 |
|
33648 |
inline const M1 & operator+(const MotionBase<M1> & v, |
114 |
|
|
const MotionZeroTpl<Scalar,Options> &) |
115 |
|
33648 |
{ return v.derived(); } |
116 |
|
|
|
117 |
|
|
template<typename Scalar, int Options, typename M1> |
118 |
|
21792 |
inline const M1 & operator+(const MotionZeroTpl<Scalar,Options> &, |
119 |
|
|
const MotionBase<M1> & v) |
120 |
|
21792 |
{ return v.derived(); } |
121 |
|
|
|
122 |
|
|
/// \brief BiasZeroTpl has been replaced by MotionZeroTpl. Please use this naming instead. |
123 |
|
|
template<typename Scalar, int Options> |
124 |
|
|
struct PINOCCHIO_DEPRECATED BiasZeroTpl : MotionZeroTpl<Scalar,Options> |
125 |
|
|
{ |
126 |
|
|
typedef MotionZeroTpl<Scalar,Options> Base; |
127 |
|
|
BiasZeroTpl(const Base &) {} |
128 |
|
|
}; |
129 |
|
|
|
130 |
|
|
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH |
131 |
|
|
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS |
132 |
|
|
template<typename Scalar, int Options> |
133 |
|
|
struct SE3GroupAction< BiasZeroTpl<Scalar,Options> > |
134 |
|
|
{ |
135 |
|
|
typedef BiasZeroTpl<Scalar,Options> ReturnType; |
136 |
|
|
}; |
137 |
|
|
|
138 |
|
|
template<typename Scalar, int Options, typename MotionDerived> |
139 |
|
|
struct MotionAlgebraAction< BiasZeroTpl<Scalar,Options>, MotionDerived> |
140 |
|
|
{ |
141 |
|
|
typedef BiasZeroTpl<Scalar,Options> ReturnType; |
142 |
|
|
}; |
143 |
|
|
PINOCCHIO_COMPILER_DIAGNOSTIC_POP |
144 |
|
|
|
145 |
|
|
} // namespace pinocchio |
146 |
|
|
|
147 |
|
|
#endif // ifndef __pinocchio_motion_zero_hpp__ |