pinocchio  3.9.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
Loading...
Searching...
No Matches
motion-ref.hpp
1//
2// Copyright (c) 2017-2019 CNRS INRIA
3//
4
5#ifndef __pinocchio_spatial_motion_ref_hpp__
6#define __pinocchio_spatial_motion_ref_hpp__
7
8namespace pinocchio
9{
10
11 template<typename Vector6ArgType>
12 struct traits<MotionRef<Vector6ArgType>>
13 {
14 typedef typename Vector6ArgType::Scalar Scalar;
15 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6;
16 enum
17 {
18 LINEAR = 0,
19 ANGULAR = 3,
20 Options = Vector6::Options
21 };
22 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
23 typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4;
24 typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
25 typedef Matrix6 ActionMatrixType;
26 typedef Matrix4 HomogeneousMatrixType;
27 typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type LinearType;
28 typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type AngularType;
29 typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
30 typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
31 typedef MotionTpl<Scalar, Options> MotionPlain;
32 typedef MotionPlain PlainReturnType;
33 typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) DataRefType;
34 typedef DataRefType ToVectorReturnType;
35 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType;
36 typedef ConstDataRefType ToVectorConstReturnType;
37 typedef MotionRef<Vector6ArgType> MotionRefType;
38
39 }; // traits MotionRef
40
41 template<typename Vector6ArgType>
42 struct SE3GroupAction<MotionRef<Vector6ArgType>>
43 {
44 typedef typename traits<MotionRef<Vector6ArgType>>::MotionPlain ReturnType;
45 };
46
47 template<typename Vector6ArgType, typename MotionDerived>
48 struct MotionAlgebraAction<MotionRef<Vector6ArgType>, MotionDerived>
49 {
50 typedef typename traits<MotionRef<Vector6ArgType>>::MotionPlain ReturnType;
51 };
52
53 namespace internal
54 {
55 template<typename Vector6ArgType, typename Scalar>
56 struct RHSScalarMultiplication<MotionRef<Vector6ArgType>, Scalar>
57 {
58 typedef typename pinocchio::traits<MotionRef<Vector6ArgType>>::MotionPlain ReturnType;
59 };
60
61 template<typename Vector6ArgType, typename Scalar>
62 struct LHSScalarMultiplication<MotionRef<Vector6ArgType>, Scalar>
63 {
64 typedef typename traits<MotionRef<Vector6ArgType>>::MotionPlain ReturnType;
65 };
66 } // namespace internal
67
68 template<typename Vector6ArgType>
69 class MotionRef : public MotionDense<MotionRef<Vector6ArgType>>
70 {
71 public:
72 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
73 typedef MotionDense<MotionRef> Base;
74 typedef typename traits<MotionRef>::DataRefType DataRefType;
75 MOTION_TYPEDEF_TPL(MotionRef);
76
77 using Base::operator=;
78 using Base::angular;
79 using Base::linear;
80
81 using Base::__mequ__;
82 using Base::__minus__;
83 using Base::__mult__;
84 using Base::__opposite__;
85 using Base::__pequ__;
86 using Base::__plus__;
87
89 MotionRef(typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) v_like)
90 : m_ref(v_like)
91 {
92 EIGEN_STATIC_ASSERT(
93 Vector6ArgType::ColsAtCompileTime == 1, YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
94 assert(v_like.size() == 6);
95 }
96
98 MotionRef(const MotionRef & other)
99 : m_ref(other.m_ref)
100 {
101 }
102
103 ToVectorConstReturnType toVector_impl() const
104 {
105 return m_ref;
106 }
107 ToVectorReturnType toVector_impl()
108 {
109 return m_ref;
110 }
111
112 // Getters
113 ConstAngularType angular_impl() const
114 {
115 return ConstAngularType(m_ref.derived(), ANGULAR);
116 }
117 ConstLinearType linear_impl() const
118 {
119 return ConstLinearType(m_ref.derived(), LINEAR);
120 }
121 AngularType angular_impl()
122 {
123 return m_ref.template segment<3>(ANGULAR);
124 }
125 LinearType linear_impl()
126 {
127 return m_ref.template segment<3>(LINEAR);
128 }
129
130 template<typename V3>
131 void angular_impl(const Eigen::MatrixBase<V3> & w)
132 {
133 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3, 3);
134 angular_impl() = w;
135 }
136
137 template<typename V3>
138 void linear_impl(const Eigen::MatrixBase<V3> & v)
139 {
140 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3, 3);
141 linear_impl() = v;
142 }
143
144 // Specific operators for MotionTpl and MotionRef
145 template<typename S1, int O1>
146 MotionPlain __plus__(const MotionTpl<S1, O1> & v) const
147 {
148 return MotionPlain(m_ref + v.toVector());
149 }
150
151 template<typename Vector6Like>
152 MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const
153 {
154 return MotionPlain(m_ref + v.toVector());
155 }
156
157 template<typename S1, int O1>
158 MotionPlain __minus__(const MotionTpl<S1, O1> & v) const
159 {
160 return MotionPlain(m_ref - v.toVector());
161 }
162
163 template<typename Vector6Like>
164 MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const
165 {
166 return MotionPlain(m_ref - v.toVector());
167 }
168
169 template<typename S1, int O1>
170 MotionRef & __pequ__(const MotionTpl<S1, O1> & v)
171 {
172 m_ref += v.toVector();
173 return *this;
174 }
175
176 template<typename Vector6Like>
177 MotionRef & __pequ__(const MotionRef<Vector6ArgType> & v)
178 {
179 m_ref += v.toVector();
180 return *this;
181 }
182
183 template<typename S1, int O1>
184 MotionRef & __mequ__(const MotionTpl<S1, O1> & v)
185 {
186 m_ref -= v.toVector();
187 return *this;
188 }
189
190 template<typename Vector6Like>
191 MotionRef & __mequ__(const MotionRef<Vector6ArgType> & v)
192 {
193 m_ref -= v.toVector();
194 return *this;
195 }
196
197 template<typename OtherScalar>
198 MotionPlain __mult__(const OtherScalar & alpha) const
199 {
200 return MotionPlain(alpha * m_ref);
201 }
202
203 MotionRef & ref()
204 {
205 return *this;
206 }
207
208 inline PlainReturnType plain() const
209 {
210 return PlainReturnType(m_ref);
211 }
212
213 protected:
214 DataRefType m_ref;
215
216 }; // class MotionRef<Vector6Like>
217
218 template<typename Vector6ArgType>
219 struct traits<MotionRef<const Vector6ArgType>>
220 {
221 typedef typename Vector6ArgType::Scalar Scalar;
222 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6;
223 enum
224 {
225 LINEAR = 0,
226 ANGULAR = 3,
227 Options = Vector6::Options
228 };
229 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
230 typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4;
231 typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
232 typedef Matrix6 ActionMatrixType;
233 typedef Matrix4 HomogeneousMatrixType;
234 typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
235 typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
236 typedef ConstLinearType LinearType;
237 typedef ConstAngularType AngularType;
238 typedef MotionTpl<Scalar, Options> MotionPlain;
239 typedef MotionPlain PlainReturnType;
240 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType;
241 typedef ConstDataRefType ToVectorConstReturnType;
242 typedef ConstDataRefType DataRefType;
243 typedef DataRefType ToVectorReturnType;
244 typedef MotionRef<const Vector6ArgType> MotionRefType;
245
246 }; // traits MotionRef<const Vector6ArgType>
247
248 template<typename Vector6ArgType>
249 class MotionRef<const Vector6ArgType> : public MotionDense<MotionRef<const Vector6ArgType>>
250 {
251 public:
252 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
253 typedef MotionDense<MotionRef> Base;
254 typedef typename traits<MotionRef>::DataRefType DataRefType;
255 MOTION_TYPEDEF_TPL(MotionRef);
256
257 using Base::operator=;
258 using Base::angular;
259 using Base::linear;
260
261 using Base::__minus__;
262 using Base::__mult__;
263 using Base::__opposite__;
264 using Base::__plus__;
265
266 MotionRef(typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) v_like)
267 : m_ref(v_like)
268 {
269 EIGEN_STATIC_ASSERT(
270 Vector6ArgType::ColsAtCompileTime == 1, YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
271 assert(v_like.size() == 6);
272 }
273
275 MotionRef(const MotionRef & other)
276 : m_ref(other.m_ref)
277 {
278 }
279
280 ToVectorConstReturnType toVector_impl() const
281 {
282 return m_ref;
283 }
284
285 // Getters
286 ConstAngularType angular_impl() const
287 {
288 return ConstAngularType(m_ref.derived(), ANGULAR);
289 }
290 ConstLinearType linear_impl() const
291 {
292 return ConstLinearType(m_ref.derived(), LINEAR);
293 }
294
295 // Specific operators for MotionTpl and MotionRef
296 template<typename S1, int O1>
297 MotionPlain __plus__(const MotionTpl<S1, O1> & v) const
298 {
299 return MotionPlain(m_ref + v.toVector());
300 }
301
302 template<typename Vector6Like>
303 MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const
304 {
305 return MotionPlain(m_ref + v.toVector());
306 }
307
308 template<typename S1, int O1>
309 MotionPlain __minus__(const MotionTpl<S1, O1> & v) const
310 {
311 return MotionPlain(m_ref - v.toVector());
312 }
313
314 template<typename Vector6Like>
315 MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const
316 {
317 return MotionPlain(m_ref - v.toVector());
318 }
319
320 template<typename OtherScalar>
321 MotionPlain __mult__(const OtherScalar & alpha) const
322 {
323 return MotionPlain(alpha * m_ref);
324 }
325
326 const MotionRef & ref() const
327 {
328 return *this;
329 }
330
331 inline PlainReturnType plain() const
332 {
333 return PlainReturnType(m_ref);
334 }
335
336 protected:
337 DataRefType m_ref;
338
339 }; // class MotionRef<const Vector6Like>
340
341} // namespace pinocchio
342
343#endif // ifndef __pinocchio_spatial_motion_ref_hpp__
pinocchio::MotionRef::MotionRef
MotionRef(typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) v_like)
Default constructor from a 6 dimensional vector.
Definition motion-ref.hpp:89
pinocchio::MotionRef::MotionRef
MotionRef(const MotionRef &other)
Copy constructor from another MotionRef.
Definition motion-ref.hpp:98
pinocchio
Main pinocchio namespace.
Definition treeview.dox:11
pinocchio::MotionAlgebraAction
Return type of the ation of a Motion onto an object of type D.
Definition motion.hpp:46
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition fwd.hpp:72