Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2015-2020 CNRS INRIA |
3 |
|
|
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. |
4 |
|
|
// |
5 |
|
|
|
6 |
|
|
#ifndef __pinocchio_spatial_motion_base_hpp__ |
7 |
|
|
#define __pinocchio_spatial_motion_base_hpp__ |
8 |
|
|
|
9 |
|
|
namespace pinocchio |
10 |
|
|
{ |
11 |
|
|
|
12 |
|
|
template<class Derived> |
13 |
|
|
class MotionBase : NumericalBase<Derived> |
14 |
|
|
{ |
15 |
|
|
public: |
16 |
|
|
MOTION_TYPEDEF_TPL(Derived); |
17 |
|
|
|
18 |
|
1929512 |
Derived & derived() |
19 |
|
|
{ |
20 |
|
1929512 |
return *static_cast<Derived *>(this); |
21 |
|
|
} |
22 |
|
2690228 |
const Derived & derived() const |
23 |
|
|
{ |
24 |
|
5368384 |
return *static_cast<const Derived *>(this); |
25 |
|
|
} |
26 |
|
|
|
27 |
|
|
Derived & const_cast_derived() const |
28 |
|
|
{ |
29 |
|
|
return *const_cast<Derived *>(&derived()); |
30 |
|
|
} |
31 |
|
|
|
32 |
|
1851775 |
ConstAngularType angular() const |
33 |
|
|
{ |
34 |
|
1851775 |
return derived().angular_impl(); |
35 |
|
|
} |
36 |
|
514949 |
ConstLinearType linear() const |
37 |
|
|
{ |
38 |
|
1028373 |
return derived().linear_impl(); |
39 |
|
|
} |
40 |
|
400677 |
AngularType angular() |
41 |
|
|
{ |
42 |
|
800026 |
return derived().angular_impl(); |
43 |
|
|
} |
44 |
|
323161 |
LinearType linear() |
45 |
|
|
{ |
46 |
|
645438 |
return derived().linear_impl(); |
47 |
|
|
} |
48 |
|
|
|
49 |
|
|
template<typename V3Like> |
50 |
|
✗ |
void angular(const Eigen::MatrixBase<V3Like> & w) |
51 |
|
|
{ |
52 |
|
✗ |
derived().angular_impl(w.derived()); |
53 |
|
|
} |
54 |
|
|
|
55 |
|
|
template<typename V3Like> |
56 |
|
✗ |
void linear(const Eigen::MatrixBase<V3Like> & v) |
57 |
|
|
{ |
58 |
|
✗ |
derived().linear_impl(v.derived()); |
59 |
|
|
} |
60 |
|
|
|
61 |
|
34702 |
operator PlainReturnType() const |
62 |
|
|
{ |
63 |
0/2
✗ Branch 3 not taken.
✗ Branch 4 not taken.
|
34702 |
return derived().plain(); |
64 |
|
|
} |
65 |
|
|
PlainReturnType plain() const |
66 |
|
|
{ |
67 |
|
|
return derived().plain(); |
68 |
|
|
} |
69 |
|
|
|
70 |
|
643346 |
ToVectorConstReturnType toVector() const |
71 |
|
|
{ |
72 |
|
643346 |
return derived().toVector_impl(); |
73 |
|
|
} |
74 |
|
26115 |
ToVectorReturnType toVector() |
75 |
|
|
{ |
76 |
|
30741 |
return derived().toVector_impl(); |
77 |
|
|
} |
78 |
|
✗ |
operator Vector6() const |
79 |
|
|
{ |
80 |
|
✗ |
return toVector(); |
81 |
|
|
} |
82 |
|
|
|
83 |
|
2 |
ActionMatrixType toActionMatrix() const |
84 |
|
|
{ |
85 |
|
2 |
return derived().toActionMatrix_impl(); |
86 |
|
|
} |
87 |
|
✗ |
ActionMatrixType toDualActionMatrix() const |
88 |
|
|
{ |
89 |
|
✗ |
return derived().toDualActionMatrix_impl(); |
90 |
|
|
} |
91 |
|
|
operator Matrix6() const |
92 |
|
|
{ |
93 |
|
|
return toActionMatrix(); |
94 |
|
|
} |
95 |
|
|
|
96 |
|
|
/** |
97 |
|
|
* @brief The homogeneous representation of the motion vector \f$ \xi \f$. |
98 |
|
|
* |
99 |
|
|
* With \f$ \hat{\xi} = \left( \begin{array}{cc} \omega & v \\ 0 & 0 \\ \end{array} \right) \f$, |
100 |
|
|
* \f[ |
101 |
|
|
* {}^a\dot{M}_b = \hat{\xi} {}^aM_b |
102 |
|
|
* \f] |
103 |
|
|
* |
104 |
|
|
* @note This function is provided for completeness, but it is not the best |
105 |
|
|
* way to use Motion quantities in terms of sparsity exploitation and |
106 |
|
|
* general efficiency. For integration, the recommended way is to use |
107 |
|
|
* Motion vectors along with the \ref integrate function. |
108 |
|
|
*/ |
109 |
|
✗ |
HomogeneousMatrixType toHomogeneousMatrix() const |
110 |
|
|
{ |
111 |
|
✗ |
return derived().toHomogeneousMatrix_impl(); |
112 |
|
|
} |
113 |
|
|
|
114 |
|
39212 |
void setZero() |
115 |
|
|
{ |
116 |
|
39212 |
derived().setZero(); |
117 |
|
39212 |
} |
118 |
|
|
|
119 |
|
|
template<typename M2> |
120 |
|
1 |
bool operator==(const MotionBase<M2> & other) const |
121 |
|
|
{ |
122 |
|
1 |
return derived().isEqual_impl(other.derived()); |
123 |
|
|
} |
124 |
|
|
|
125 |
|
|
template<typename M2> |
126 |
|
✗ |
bool operator!=(const MotionBase<M2> & other) const |
127 |
|
|
{ |
128 |
|
✗ |
return !(derived() == other.derived()); |
129 |
|
|
} |
130 |
|
|
|
131 |
|
|
Derived operator-() const |
132 |
|
|
{ |
133 |
|
|
return derived().__opposite__(); |
134 |
|
|
} |
135 |
|
✗ |
Derived operator+(const MotionBase<Derived> & v) const |
136 |
|
|
{ |
137 |
|
✗ |
return derived().__plus__(v.derived()); |
138 |
|
|
} |
139 |
|
|
Derived operator-(const MotionBase<Derived> & v) const |
140 |
|
|
{ |
141 |
|
|
return derived().__minus__(v.derived()); |
142 |
|
|
} |
143 |
|
|
Derived & operator+=(const MotionBase<Derived> & v) |
144 |
|
|
{ |
145 |
|
|
return derived().__pequ__(v.derived()); |
146 |
|
|
} |
147 |
|
|
Derived & operator-=(const MotionBase<Derived> & v) |
148 |
|
|
{ |
149 |
|
|
return derived().__mequ__(v.derived()); |
150 |
|
|
} |
151 |
|
|
|
152 |
|
|
template<typename OtherScalar> |
153 |
|
|
typename internal::RHSScalarMultiplication<Derived, OtherScalar>::ReturnType |
154 |
|
20995 |
operator*(const OtherScalar & alpha) const |
155 |
|
|
{ |
156 |
|
20995 |
return derived().__mult__(alpha); |
157 |
|
|
} |
158 |
|
|
|
159 |
|
|
template<typename OtherScalar> |
160 |
|
3 |
Derived operator/(const OtherScalar & alpha) const |
161 |
|
|
{ |
162 |
|
3 |
return derived().__div__(alpha); |
163 |
|
|
} |
164 |
|
|
|
165 |
|
|
template<typename OtherSpatialType> |
166 |
|
|
typename MotionAlgebraAction<OtherSpatialType, Derived>::ReturnType |
167 |
|
234494 |
cross(const OtherSpatialType & d) const |
168 |
|
|
{ |
169 |
|
234494 |
return derived().cross_impl(d); |
170 |
|
|
} |
171 |
|
|
|
172 |
|
371 |
bool isApprox( |
173 |
|
|
const Derived & other, |
174 |
|
|
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const |
175 |
|
|
{ |
176 |
|
371 |
return derived().isApprox_impl(other, prec); |
177 |
|
|
} |
178 |
|
|
|
179 |
|
91 |
bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const |
180 |
|
|
{ |
181 |
|
91 |
return derived().isZero_impl(prec); |
182 |
|
|
} |
183 |
|
|
|
184 |
|
|
template<typename S2, int O2> |
185 |
|
193658 |
typename SE3GroupAction<Derived>::ReturnType se3Action(const SE3Tpl<S2, O2> & m) const |
186 |
|
|
{ |
187 |
|
193658 |
return derived().se3Action_impl(m); |
188 |
|
|
} |
189 |
|
|
|
190 |
|
|
template<typename S2, int O2> |
191 |
|
120881 |
typename SE3GroupAction<Derived>::ReturnType se3ActionInverse(const SE3Tpl<S2, O2> & m) const |
192 |
|
|
{ |
193 |
|
120881 |
return derived().se3ActionInverse_impl(m); |
194 |
|
|
} |
195 |
|
|
|
196 |
|
|
template<typename ForceDerived> |
197 |
|
|
Scalar dot(const ForceDense<ForceDerived> & f) const |
198 |
|
|
{ |
199 |
|
|
return derived().dot(f.derived()); |
200 |
|
|
} |
201 |
|
|
|
202 |
|
12 |
void disp(std::ostream & os) const |
203 |
|
|
{ |
204 |
|
12 |
derived().disp_impl(os); |
205 |
|
12 |
} |
206 |
|
12 |
friend std::ostream & operator<<(std::ostream & os, const MotionBase<Derived> & v) |
207 |
|
|
{ |
208 |
|
12 |
v.disp(os); |
209 |
|
12 |
return os; |
210 |
|
|
} |
211 |
|
|
|
212 |
|
|
}; // class MotionBase |
213 |
|
|
|
214 |
|
|
template<typename MotionDerived> |
215 |
|
|
typename internal::RHSScalarMultiplication< |
216 |
|
|
MotionDerived, |
217 |
|
|
typename MotionDerived::Scalar>::ReturnType |
218 |
|
|
operator*(const typename MotionDerived::Scalar & alpha, const MotionBase<MotionDerived> & motion) |
219 |
|
|
{ |
220 |
|
|
return motion * alpha; |
221 |
|
|
} |
222 |
|
|
|
223 |
|
|
} // namespace pinocchio |
224 |
|
|
|
225 |
|
|
#endif // ifndef __pinocchio_spatial_motion_base_hpp__ |
226 |
|
|
|