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 |
|
2107556 |
Derived & derived() { return *static_cast<Derived*>(this); } |
19 |
|
2189664 |
const Derived & derived() const { return *static_cast<const Derived*>(this); } |
20 |
|
|
|
21 |
|
1623744 |
ConstAngularType angular() const { return derived().angular_impl(); } |
22 |
|
500363 |
ConstLinearType linear() const { return derived().linear_impl(); } |
23 |
|
463200 |
AngularType angular() { return derived().angular_impl(); } |
24 |
|
300711 |
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 |
|
5 |
void linear(const Eigen::MatrixBase<V3Like> & v) |
32 |
|
5 |
{ derived().linear_impl(v.derived()); } |
33 |
|
|
|
34 |
|
49571 |
operator PlainReturnType() const { return derived().plain(); } |
35 |
|
|
PlainReturnType plain() const { return derived().plain(); } |
36 |
|
|
|
37 |
|
518942 |
ToVectorConstReturnType toVector() const { return derived().toVector_impl(); } |
38 |
|
85948 |
ToVectorReturnType toVector() { return derived().toVector_impl(); } |
39 |
|
4 |
operator Vector6() const { return toVector(); } |
40 |
|
|
|
41 |
|
158 |
ActionMatrixType toActionMatrix() const { return derived().toActionMatrix_impl(); } |
42 |
|
6 |
ActionMatrixType toDualActionMatrix() const { return derived().toDualActionMatrix_impl(); } |
43 |
|
|
operator Matrix6() const { return toActionMatrix(); } |
44 |
|
|
|
45 |
|
|
/** |
46 |
|
|
* @brief The homogeneous representation of the motion vector \f$ \xi \f$. |
47 |
|
|
* |
48 |
|
|
* With \f$ \hat{\xi} = \left( \begin{array}{cc} \omega & v \\ 0 & 0 \\ \end{array} \right) \f$, |
49 |
|
|
* \f[ |
50 |
|
|
* {}^a\dot{M}_b = \hat{\xi} {}^aM_b |
51 |
|
|
* \f] |
52 |
|
|
* |
53 |
|
|
* @note This function is provided for completeness, but it is not the best |
54 |
|
|
* way to use Motion quantities in terms of sparsity exploitation and |
55 |
|
|
* general efficiency. For integration, the recommended way is to use |
56 |
|
|
* Motion vectors along with the \ref integrate function. |
57 |
|
|
*/ |
58 |
|
|
HomogeneousMatrixType toHomogeneousMatrix() const { return derived().toHomogeneousMatrix_impl(); } |
59 |
|
|
|
60 |
|
40 |
void setZero() { derived().setZero(); } |
61 |
|
|
|
62 |
|
|
template<typename M2> |
63 |
|
7538 |
bool operator==(const MotionBase<M2> & other) const |
64 |
|
7538 |
{ return derived().isEqual_impl(other.derived()); } |
65 |
|
|
|
66 |
|
|
template<typename M2> |
67 |
|
1 |
bool operator!=(const MotionBase<M2> & other) const |
68 |
|
1 |
{ return !(derived() == other.derived()); } |
69 |
|
|
|
70 |
|
|
Derived operator-() const { return derived().__opposite__(); } |
71 |
|
1 |
Derived operator+(const MotionBase<Derived> & v) const { return derived().__plus__(v.derived()); } |
72 |
|
|
Derived operator-(const MotionBase<Derived> & v) const { return derived().__minus__(v.derived()); } |
73 |
|
|
Derived & operator+=(const MotionBase<Derived> & v) { return derived().__pequ__(v.derived()); } |
74 |
|
|
Derived & operator-=(const MotionBase<Derived> & v) { return derived().__mequ__(v.derived()); } |
75 |
|
|
|
76 |
|
|
template<typename OtherScalar> |
77 |
|
|
typename internal::RHSScalarMultiplication<Derived,OtherScalar>::ReturnType |
78 |
|
36748 |
operator*(const OtherScalar & alpha) const |
79 |
|
36748 |
{ return derived().__mult__(alpha); } |
80 |
|
|
|
81 |
|
|
template<typename OtherScalar> |
82 |
|
1 |
Derived operator/(const OtherScalar & alpha) const |
83 |
|
1 |
{ return derived().__div__(alpha); } |
84 |
|
|
|
85 |
|
|
template<typename OtherSpatialType> |
86 |
|
|
typename MotionAlgebraAction<OtherSpatialType,Derived>::ReturnType |
87 |
|
78692 |
cross(const OtherSpatialType & d) const |
88 |
|
|
{ |
89 |
|
78692 |
return derived().cross_impl(d); |
90 |
|
|
} |
91 |
|
|
|
92 |
|
1231 |
bool isApprox(const Derived & other, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const |
93 |
|
1231 |
{ return derived().isApprox_impl(other, prec);} |
94 |
|
|
|
95 |
|
10 |
bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const |
96 |
|
10 |
{ return derived().isZero_impl(prec);} |
97 |
|
|
|
98 |
|
|
template<typename S2, int O2> |
99 |
|
|
typename SE3GroupAction<Derived>::ReturnType |
100 |
|
65425 |
se3Action(const SE3Tpl<S2,O2> & m) const |
101 |
|
65425 |
{ return derived().se3Action_impl(m); } |
102 |
|
|
|
103 |
|
|
template<typename S2, int O2> |
104 |
|
|
typename SE3GroupAction<Derived>::ReturnType |
105 |
|
184359 |
se3ActionInverse(const SE3Tpl<S2,O2> & m) const |
106 |
|
184359 |
{ return derived().se3ActionInverse_impl(m); } |
107 |
|
|
|
108 |
|
|
template<typename ForceDerived> |
109 |
|
|
Scalar dot(const ForceDense<ForceDerived> & f) const { return derived().dot(f.derived()); } |
110 |
|
|
|
111 |
|
1 |
void disp(std::ostream & os) const { derived().disp_impl(os); } |
112 |
|
1 |
friend std::ostream & operator << (std::ostream & os, const MotionBase<Derived> & v) |
113 |
|
|
{ |
114 |
|
1 |
v.disp(os); |
115 |
|
1 |
return os; |
116 |
|
|
} |
117 |
|
|
|
118 |
|
|
}; // class MotionBase |
119 |
|
|
|
120 |
|
|
template<typename MotionDerived> |
121 |
|
|
typename internal::RHSScalarMultiplication<MotionDerived,typename MotionDerived::Scalar>::ReturnType |
122 |
|
|
operator*(const typename MotionDerived::Scalar & alpha, |
123 |
|
|
const MotionBase<MotionDerived> & motion) |
124 |
|
|
{ |
125 |
|
|
return motion*alpha; |
126 |
|
|
} |
127 |
|
|
|
128 |
|
|
} // namespace pinocchio |
129 |
|
|
|
130 |
|
|
#endif // ifndef __pinocchio_motion_base_hpp__ |