Branch data Line data Source code
1 : : // Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
2 : : // RDL - Robot Dynamics Library
3 : : // Licensed under the zlib license. See LICENSE for more details.
4 : :
5 : : #ifndef __RDL_MOTION_VECTOR_HPP__
6 : : #define __RDL_MOTION_VECTOR_HPP__
7 : :
8 : : /**
9 : : * @file MotionVector.hpp
10 : : * @brief Contains various geometric objects that have methods for transforming themselves into different frames of reference
11 : : */
12 : :
13 : : #include "rdl_dynamics/ForceVector.hpp"
14 : : #include "rdl_dynamics/rdl_eigenmath.hpp"
15 : :
16 : : namespace RobotDynamics
17 : : {
18 : : namespace Math
19 : : {
20 : : class MotionVector : public SpatialVector, public TransformableGeometricObject
21 : : {
22 : : public:
23 : : /**
24 : : * @brief Constructor
25 : : * @param other
26 : : */
27 : : template <typename OtherDerived>
28 : : // cppcheck-suppress noExplicitConstructor
29 : 84395 : MotionVector(const Eigen::MatrixBase<OtherDerived>& other) : SpatialVector(other)
30 : : {
31 : 84395 : }
32 : :
33 : 115462 : MotionVector(const MotionVector& v) : SpatialVector(v)
34 : : {
35 : 115459 : }
36 : :
37 : : /**
38 : : * @brief Overload equal operator
39 : : * @param other
40 : : * @return A motion vector
41 : : */
42 : 3462 : MotionVector& operator=(const MotionVector& other)
43 : : {
44 : 3462 : SpatialVector::operator=(other);
45 : 3462 : return *this;
46 : : }
47 : :
48 : : /**
49 : : * @brief Empty constructor
50 : : */
51 : 2240 : EIGEN_STRONG_INLINE MotionVector() : SpatialVector(0., 0., 0., 0., 0., 0.)
52 : : {
53 : 2240 : }
54 : :
55 : : /**
56 : : * @brief Constructor
57 : : * @param v0 x-angular
58 : : * @param v1 y-angular
59 : : * @param v2 z-angular
60 : : * @param v3 x-linear
61 : : * @param v4 y-linear
62 : : * @param v5 z-linear
63 : : */
64 : 11877 : MotionVector(const double v0, const double v1, const double v2, const double v3, const double v4, const double v5)
65 : 11877 : {
66 : 11876 : Base::_check_template_params();
67 : :
68 : 11876 : (*this) << v0, v1, v2, v3, v4, v5;
69 : 11876 : }
70 : :
71 : : /**
72 : : * @brief Get a copy of a MotionVector as a SpatialVector
73 : : * @return SpatialVector copy of a MotionVectors
74 : : */
75 : : EIGEN_STRONG_INLINE SpatialVector toSpatialVector() const
76 : : {
77 : : return *this;
78 : : }
79 : :
80 : : /**
81 : : * @brief Setter
82 : : * @param v Sets the values equal to those stored in v
83 : : */
84 : 41921 : EIGEN_STRONG_INLINE void set(const MotionVector& v)
85 : : {
86 : 41921 : (*this) << v[0], v[1], v[2], v[3], v[4], v[5];
87 : 41921 : }
88 : :
89 : : /**
90 : : * @brief Get a reference to the angular-x component
91 : : * @return A reference to the angular x-component
92 : : */
93 : 16601 : EIGEN_STRONG_INLINE double& wx()
94 : : {
95 : 16601 : return this->operator[](0);
96 : : }
97 : :
98 : : /**
99 : : * @brief Get a reference to the angular-y component
100 : : * @return A reference to the angular y-component
101 : : */
102 : 18124 : EIGEN_STRONG_INLINE double& wy()
103 : : {
104 : 18124 : return this->operator[](1);
105 : : }
106 : :
107 : : /**
108 : : * @brief Get a reference to the angular-z component
109 : : * @return A copy reference to the angular z-component
110 : : */
111 : 17288 : EIGEN_STRONG_INLINE double& wz()
112 : : {
113 : 17288 : return this->operator[](2);
114 : : }
115 : :
116 : : /**
117 : : * @brief Get a copy of the angular-x component
118 : : * @return A copy of the angular x-component
119 : : */
120 : 10772 : EIGEN_STRONG_INLINE double wx() const
121 : : {
122 : 10772 : return this->operator[](0);
123 : : }
124 : :
125 : : /**
126 : : * @brief Get a copy of the angular-y component
127 : : * @return A copy of the angular y-component
128 : : */
129 : 10772 : EIGEN_STRONG_INLINE double wy() const
130 : : {
131 : 10772 : return this->operator[](1);
132 : : }
133 : :
134 : : /**
135 : : * @brief Get a copy of the angular-z component
136 : : * @return A copy of the angular z-component
137 : : */
138 : 10773 : EIGEN_STRONG_INLINE double wz() const
139 : : {
140 : 10773 : return this->operator[](2);
141 : : }
142 : :
143 : : /**
144 : : * @brief Get a reference to the linear-x component
145 : : * @return A reference to the linear x-component
146 : : */
147 : 14187 : EIGEN_STRONG_INLINE double& vx()
148 : : {
149 : 14187 : return this->operator[](3);
150 : : }
151 : :
152 : : /**
153 : : * @brief Get a reference to the linear-y component
154 : : * @return A reference to the linear y-component
155 : : */
156 : 14187 : EIGEN_STRONG_INLINE double& vy()
157 : : {
158 : 14187 : return this->operator[](4);
159 : : }
160 : :
161 : : /**
162 : : * @brief Get a reference to the linear-z component
163 : : * @return A reference to the linear z-component
164 : : */
165 : 14187 : EIGEN_STRONG_INLINE double& vz()
166 : : {
167 : 14187 : return this->operator[](5);
168 : : }
169 : :
170 : : /**
171 : : * @brief Get a copy of the linear-x component
172 : : * @return A copy of the linear x-component
173 : : */
174 : 10773 : EIGEN_STRONG_INLINE double vx() const
175 : : {
176 : 10773 : return this->operator[](3);
177 : : }
178 : :
179 : : /**
180 : : * @brief Get a copy of the linear-y component
181 : : * @return A copy of the linear y-component
182 : : */
183 : 10772 : EIGEN_STRONG_INLINE double vy() const
184 : : {
185 : 10772 : return this->operator[](4);
186 : : }
187 : :
188 : : /**
189 : : * @brief Get a copy of the linear-z component
190 : : * @return A copy of the linear z-component
191 : : */
192 : 10773 : EIGEN_STRONG_INLINE double vz() const
193 : : {
194 : 10773 : return this->operator[](5);
195 : : }
196 : :
197 : : /**
198 : : * @brief Transforms a motion vector. Performs \f$ v= X v \f$
199 : : * @param X
200 : : */
201 : 37631 : inline void transform(const SpatialTransform& X)
202 : : {
203 : 37631 : this->setLinearPart(-X.E * X.r.cross(this->getAngularPart()) + X.E * this->getLinearPart());
204 : 37631 : this->setAngularPart(X.E * this->getAngularPart());
205 : 37631 : }
206 : :
207 : : /**
208 : : * @brief Copies, transforms, and returns a MotionVector. Performs \f$ v_2=X v_1 \f$
209 : : * @param X
210 : : * @return Copied, transformed MotionVector
211 : : */
212 : 37750 : MotionVector transform_copy(const SpatialTransform& X) const
213 : : {
214 : 37750 : MotionVector v = *this;
215 : 37749 : v.transform(X);
216 : 37749 : return v;
217 : : }
218 : :
219 : : /**
220 : : * @brief See V. Duindum thesis p.25 for an explanation of what \f$ad_T\f$ operator is.
221 : : * It is also in Featherstone p. 25 eq. 2.31 & 2.32. For featherstone notation,
222 : : * it is essentially the \f$\times\f$ operator for spatial vectors. Given two SpatialMotion vectors, \f$v_1\f$ and
223 : : * \f$v_2\f$, this method returns \f$ v_3 = (v_1\times) v_2 \f$. Expanded, it looks like,
224 : : ** \f[
225 : : * v_3 = (v_1 \times) v_2 = ad_{v_1} v_2 =
226 : : * \begin{bmatrix}
227 : : * \omega_{v_1} \times & \mathbf{0} \\
228 : : * v_{v_1}\times & \omega_{v_1}\times
229 : : * \end{bmatrix} v_2
230 : : * \f]
231 : : * The 3d vector \f$\times\f$ operator is equivalent to the \f$\sim\f$ operator. See Math::toTildeForm.
232 : : */
233 : : MotionVector cross(const MotionVector& v);
234 : :
235 : : /**
236 : : * @brief See Featherstone p. 25 eq. 2.31 & 2.32. For featherstone notation,
237 : : * it is essentially the \f$\times *\f$ operator for spatial vectors. Given a SpatialMotion vector, \f$v_1\f$, and
238 : : * SpatialForceVector, \f$f_1\f$, this method returns \f$ f_2 = (v_1\times *) f_1 \f$. Expanded, it looks like,
239 : : * \f[
240 : : * =(v_1\times *) f_1 =
241 : : * \begin{bmatrix}
242 : : * \omega_{v_1}\times & v_{v_1}\times \\
243 : : * \mathbf{0} & \omega_{v_1}\times
244 : : * \end{bmatrix} f
245 : : * \f]
246 : : * The 3d vector \f$\times\f$ operator is equivalent to the \f$\sim\f$ operator. See Math::toTildeForm.
247 : : */
248 : : ForceVector cross(const ForceVector& v);
249 : :
250 : : /**
251 : : * @brief Get the spatial motion cross matrix,
252 : : * \f[
253 : : * m\times =
254 : : * \begin{bmatrix}
255 : : * \omega\times & \mathbf{0} \\
256 : : * v\times & \omega\times
257 : : * \end{bmatrix}
258 : : * \f]
259 : : * @return \f$ v\times \f$
260 : : */
261 : 1066 : inline SpatialMatrix crossm()
262 : : {
263 : 1066 : return SpatialMatrix(0, -this->operator[](2), this->operator[](1), 0, 0, 0, this->operator[](2), 0, -this->operator[](0), 0, 0, 0, -this->operator[](1),
264 : 3198 : this->operator[](0), 0, 0, 0, 0, 0, -this->operator[](5), this->operator[](4), 0, -this->operator[](2), this->operator[](1),
265 : 3198 : this->operator[](5), 0, -this->operator[](3), this->operator[](2), 0, -this->operator[](0), -this->operator[](4), this->operator[](3), 0,
266 : 7462 : -this->operator[](1), this->operator[](0), 0);
267 : : }
268 : :
269 : : /**
270 : : * @brief Get the spatial force cross matrix
271 : : * @return \f$ v\times* \f$
272 : : */
273 : 2 : inline SpatialMatrix crossf()
274 : : {
275 : 4 : return SpatialMatrix(0, -this->operator[](2), this->operator[](1), 0, -this->operator[](5), this->operator[](4), this->operator[](2), 0, -this->operator[](0),
276 : 6 : this->operator[](5), 0, -this->operator[](3), -this->operator[](1), this->operator[](0), 0, -this->operator[](4), this->operator[](3), 0, 0,
277 : 2 : 0, 0, 0, -this->operator[](2), this->operator[](1), 0, 0, 0, this->operator[](2), 0, -this->operator[](0), 0, 0, 0, -this->operator[](1),
278 : 14 : this->operator[](0), 0);
279 : : }
280 : :
281 : : /**
282 : : * @brief Operator for performing the RBDA \f$\times\f$ operator for two motion vectors, i.e.
283 : : **\f$m_2=(m_1\times)m_2\f$.
284 : : * @param v
285 : : * @return A MotionVector
286 : : */
287 : 10633 : inline MotionVector operator%=(const MotionVector& v)
288 : : {
289 : 10633 : return this->cross(v);
290 : : }
291 : :
292 : : /**
293 : : * @brief Operator for performing the RBDA \f$ \times * \f$ operator for a motion vector and a force vector, i.e.
294 : : **\f$ f=(m\times *)f \f$.
295 : : * @param v
296 : : * @return A ForceVector
297 : : */
298 : 2174 : inline ForceVector operator%=(const ForceVector& v)
299 : : {
300 : 2174 : return this->cross(v);
301 : : }
302 : :
303 : : /**
304 : : * @brief Overloaded += operator for a MotionVector
305 : : * @return MotionVector
306 : : */
307 : 1 : inline MotionVector operator+=(const MotionVector& v)
308 : : {
309 : 1 : (*this) << (this->wx() += v.wx()), (this->wy() += v.wy()), (this->wz() += v.wz()), (this->vx() += v.vx()), (this->vy() += v.vy()), (this->vz() += v.vz());
310 : 1 : return *this;
311 : : }
312 : : };
313 : :
314 : : /**
315 : : * @brief Operator for transforming a MotionVector
316 : : * @param X SpatialTransform to the desired frame
317 : : * @param v
318 : : * @return Transformed MotionVector
319 : : */
320 : 1 : inline MotionVector operator*(const SpatialTransform& X, MotionVector v)
321 : : {
322 : 1 : v.transform(X);
323 : 1 : return v;
324 : : }
325 : :
326 : : /**
327 : : * Operator for performing the spatial vector \f$\times\f$ operator.
328 : : * @param v
329 : : * @param v2
330 : : * @return Returns \f$ ret = (v \times)v2 \f$
331 : : */
332 : 3032 : inline MotionVector operator%(MotionVector v, const MotionVector& v2)
333 : : {
334 : 3032 : return v.MotionVector::operator%=(v2);
335 : : }
336 : :
337 : : /**
338 : : * Operator for performing the spatial vector \f$\times*\f$ operator
339 : : * @param v
340 : : * @param v2
341 : : * @return Returns \f$ ret = (v\times*)v2 \f$
342 : : */
343 : 2174 : inline ForceVector operator%(MotionVector v, const ForceVector& v2)
344 : : {
345 : 2174 : return v.MotionVector::operator%=(v2);
346 : : }
347 : : typedef std::vector<MotionVector, Eigen::aligned_allocator<MotionVector>> MotionVectorV;
348 : : } // namespace Math
349 : : } // namespace RobotDynamics
350 : :
351 : : #endif
|
