pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
motion-dense.hpp
1//
2// Copyright (c) 2017-2020 CNRS INRIA
3//
4
5#ifndef __pinocchio_spatial_motion_dense_hpp__
6#define __pinocchio_spatial_motion_dense_hpp__
7
8#include "pinocchio/spatial/skew.hpp"
9
10namespace pinocchio
11{
12
13 template<typename Derived>
14 struct SE3GroupAction<MotionDense<Derived>>
15 {
16 typedef typename SE3GroupAction<Derived>::ReturnType ReturnType;
17 };
18
19 template<typename Derived, typename MotionDerived>
21 {
22 typedef typename MotionAlgebraAction<Derived, MotionDerived>::ReturnType ReturnType;
23 };
24
25 template<typename Derived>
26 class MotionDense : public MotionBase<Derived>
27 {
28 public:
30 MOTION_TYPEDEF_TPL(Derived);
31 typedef typename traits<Derived>::MotionRefType MotionRefType;
32
33 using Base::angular;
34 using Base::derived;
35 using Base::isApprox;
36 using Base::isZero;
37 using Base::linear;
38
39 Derived & setZero()
40 {
41 linear().setZero();
42 angular().setZero();
43 return derived();
44 }
45 Derived & setRandom()
46 {
47 linear().setRandom();
48 angular().setRandom();
49 return derived();
50 }
51
52 ActionMatrixType toActionMatrix_impl() const
53 {
54 ActionMatrixType X;
55 X.template block<3, 3>(ANGULAR, ANGULAR) = X.template block<3, 3>(LINEAR, LINEAR) =
56 skew(angular());
57 X.template block<3, 3>(LINEAR, ANGULAR) = skew(linear());
58 X.template block<3, 3>(ANGULAR, LINEAR).setZero();
59
60 return X;
61 }
62
63 ActionMatrixType toDualActionMatrix_impl() const
64 {
65 ActionMatrixType X;
66 X.template block<3, 3>(ANGULAR, ANGULAR) = X.template block<3, 3>(LINEAR, LINEAR) =
67 skew(angular());
68 X.template block<3, 3>(ANGULAR, LINEAR) = skew(linear());
69 X.template block<3, 3>(LINEAR, ANGULAR).setZero();
70
71 return X;
72 }
73
74 HomogeneousMatrixType toHomogeneousMatrix_impl() const
75 {
76 HomogeneousMatrixType M;
77 M.template block<3, 3>(0, 0) = skew(angular());
78 M.template block<3, 1>(0, 3) = linear();
79 M.template block<1, 4>(3, 0).setZero();
80 return M;
81 }
82
83 template<typename D2>
84 bool isEqual_impl(const MotionDense<D2> & other) const
85 {
86 return linear() == other.linear() && angular() == other.angular();
87 }
88
89 template<typename D2>
90 bool isEqual_impl(const MotionBase<D2> & other) const
91 {
92 return other.derived() == derived();
93 }
94
95 // Arithmetic operators
96 template<typename D2>
97 Derived & operator=(const MotionDense<D2> & other)
98 {
99 return derived().set(other.derived());
100 }
101
102 Derived & operator=(const MotionDense & other)
103 {
104 return derived().set(other.derived());
105 }
106
107 template<typename D2>
108 Derived & set(const MotionDense<D2> & other)
109 {
110 linear() = other.linear();
111 angular() = other.angular();
112 return derived();
113 }
114
115 template<typename D2>
116 Derived & operator=(const MotionBase<D2> & other)
117 {
118 other.derived().setTo(derived());
119 return derived();
120 }
121
122 template<typename V6>
123 Derived & operator=(const Eigen::MatrixBase<V6> & v)
124 {
126 assert(v.size() == 6);
127 linear() = v.template segment<3>(LINEAR);
128 angular() = v.template segment<3>(ANGULAR);
129 return derived();
130 }
131
132 MotionPlain operator-() const
133 {
134 return derived().__opposite__();
135 }
136 template<typename M1>
137 MotionPlain operator+(const MotionDense<M1> & v) const
138 {
139 return derived().__plus__(v.derived());
140 }
141 template<typename M1>
142 MotionPlain operator-(const MotionDense<M1> & v) const
143 {
144 return derived().__minus__(v.derived());
145 }
146
147 template<typename M1>
148 Derived & operator+=(const MotionDense<M1> & v)
149 {
150 return derived().__pequ__(v.derived());
151 }
152 template<typename M1>
153 Derived & operator+=(const MotionBase<M1> & v)
154 {
155 v.derived().addTo(derived());
156 return derived();
157 }
158
159 template<typename M1>
160 Derived & operator-=(const MotionDense<M1> & v)
161 {
162 return derived().__mequ__(v.derived());
163 }
164
165 MotionPlain __opposite__() const
166 {
167 return MotionPlain(-linear(), -angular());
168 }
169
170 template<typename M1>
171 MotionPlain __plus__(const MotionDense<M1> & v) const
172 {
173 return MotionPlain(linear() + v.linear(), angular() + v.angular());
174 }
175
176 template<typename M1>
177 MotionPlain __minus__(const MotionDense<M1> & v) const
178 {
179 return MotionPlain(linear() - v.linear(), angular() - v.angular());
180 }
181
182 template<typename M1>
183 Derived & __pequ__(const MotionDense<M1> & v)
184 {
185 linear() += v.linear();
186 angular() += v.angular();
187 return derived();
188 }
189
190 template<typename M1>
191 Derived & __mequ__(const MotionDense<M1> & v)
192 {
193 linear() -= v.linear();
194 angular() -= v.angular();
195 return derived();
196 }
197
198 template<typename OtherScalar>
199 MotionPlain __mult__(const OtherScalar & alpha) const
200 {
201 return MotionPlain(alpha * linear(), alpha * angular());
202 }
203
204 template<typename OtherScalar>
205 MotionPlain __div__(const OtherScalar & alpha) const
206 {
207 return derived().__mult__((OtherScalar)(1) / alpha);
208 }
209
210 template<typename F1>
211 Scalar dot(const ForceBase<F1> & phi) const
212 {
213 return phi.linear().dot(linear()) + phi.angular().dot(angular());
214 }
215
216 template<typename D>
217 typename MotionAlgebraAction<D, Derived>::ReturnType cross_impl(const D & d) const
218 {
219 return d.motionAction(derived());
220 }
221
222 template<typename M1, typename M2>
223 void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
224 {
225 mout.linear() = v.linear().cross(angular()) + v.angular().cross(linear());
226 mout.angular() = v.angular().cross(angular());
227 }
228
229 template<typename M1>
230 MotionPlain motionAction(const MotionDense<M1> & v) const
231 {
232 MotionPlain res;
233 motionAction(v, res);
234 return res;
235 }
236
237 template<typename M2>
238 bool isApprox(
239 const MotionDense<M2> & m2,
240 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
241 {
242 return derived().isApprox_impl(m2, prec);
243 }
244
245 template<typename D2>
246 bool isApprox_impl(
247 const MotionDense<D2> & m2,
248 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
249 {
250 return linear().isApprox(m2.linear(), prec) && angular().isApprox(m2.angular(), prec);
251 }
252
253 bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
254 {
255 return linear().isZero(prec) && angular().isZero(prec);
256 }
257
258 template<typename S2, int O2, typename D2>
259 void se3Action_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const
260 {
261 v.angular().noalias() = m.rotation() * angular();
262 v.linear().noalias() = m.rotation() * linear() + m.translation().cross(v.angular());
263 }
264
265 template<typename S2, int O2>
266 typename SE3GroupAction<Derived>::ReturnType se3Action_impl(const SE3Tpl<S2, O2> & m) const
267 {
268 typename SE3GroupAction<Derived>::ReturnType res;
269 se3Action_impl(m, res);
270 return res;
271 }
272
273 template<typename S2, int O2, typename D2>
274 void se3ActionInverse_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const
275 {
276 v.linear().noalias() =
277 m.rotation().transpose() * (linear() - m.translation().cross(angular()));
278 v.angular().noalias() = m.rotation().transpose() * angular();
279 }
280
281 template<typename S2, int O2>
282 typename SE3GroupAction<Derived>::ReturnType
283 se3ActionInverse_impl(const SE3Tpl<S2, O2> & m) const
284 {
285 typename SE3GroupAction<Derived>::ReturnType res;
286 se3ActionInverse_impl(m, res);
287 return res;
288 }
289
290 void disp_impl(std::ostream & os) const
291 {
292 os << " v = " << linear().transpose() << std::endl
293 << " w = " << angular().transpose() << std::endl;
294 }
295
297 MotionRefType ref()
298 {
299 return derived().ref();
300 }
301
302 protected:
303 MotionDense() {};
304
305 MotionDense(const MotionDense &) = delete;
306
307 }; // class MotionDense
308
310 template<typename M1, typename M2>
311 typename traits<M1>::MotionPlain operator^(const MotionDense<M1> & v1, const MotionDense<M2> & v2)
312 {
313 return v1.derived().cross(v2.derived());
314 }
315
316 template<typename M1, typename F1>
317 typename traits<F1>::ForcePlain operator^(const MotionDense<M1> & v, const ForceBase<F1> & f)
318 {
319 return v.derived().cross(f.derived());
320 }
321
322 template<typename M1>
323 typename traits<M1>::MotionPlain
324 operator*(const typename traits<M1>::Scalar alpha, const MotionDense<M1> & v)
325 {
326 return v * alpha;
327 }
328
329} // namespace pinocchio
330
331#endif // ifndef __pinocchio_spatial_motion_dense_hpp__
Main pinocchio namespace.
Definition treeview.dox:11
void skew(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector, i.e. the antisymmetric matrix representation o...
Definition skew.hpp:22
Return type of the ation of a Motion onto an object of type D.
Definition motion.hpp:46
Common traits structure to fully define base classes for CRTP.
Definition fwd.hpp:72