Branch data Line data Source code
1 : : // Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
2 : : // Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
3 : : // RDL - Robot Dynamics Library
4 : : // Licensed under the zlib license. See LICENSE for more details.
5 : :
6 : : #ifndef __RDL_EIGENMATH_H__
7 : : #define __RDL_EIGENMATH_H__
8 : :
9 : : #include <Eigen/Dense>
10 : : #include <Eigen/StdVector>
11 : : #include <Eigen/QR>
12 : : #include <eigen3/Eigen/Eigen>
13 : :
14 : : namespace RobotDynamics
15 : : {
16 : : /** \brief Math types such as vectors and matrices and utility functions. */
17 : : namespace Math
18 : : {
19 : : typedef Eigen::Matrix<double, 6, 3> Matrix63;
20 : : typedef Eigen::VectorXd VectorNd;
21 : : typedef Eigen::MatrixXd MatrixNd;
22 : : typedef Eigen::AngleAxisd AxisAngle;
23 : : } // namespace Math
24 : : } // namespace RobotDynamics
25 : :
26 : : namespace RobotDynamics
27 : : {
28 : : namespace Math
29 : : {
30 : : struct SpatialTransform;
31 : : class Quaternion;
32 : : class Matrix3d;
33 : : class Vector3d;
34 : : class SpatialMatrix;
35 : :
36 : : /**
37 : : * @class TransformableGeometricObject
38 : : * @brief The TransformableGeometricObject class is an essential interface because it forces all geometric objects
39 : : * to implement a method that tells how to transform them. This makes in possible for frame transformations of any
40 : : * TransformableGeometricObject can be done via the FrameObject::changeFrame method.
41 : : */
42 : : class TransformableGeometricObject
43 : : {
44 : : public:
45 : : /**
46 : : * @brief Pure virtual object. This object forces objects that inherit from it to have a method that tells
47 : : * how that object is transformed.
48 : : * @param X SpatialTransform
49 : : */
50 : : virtual void transform(const RobotDynamics::Math::SpatialTransform& X) = 0;
51 : : };
52 : :
53 : : class Vector3d : public Eigen::Vector3d, public TransformableGeometricObject
54 : : {
55 : : public:
56 : : typedef Eigen::Vector3d Base;
57 : :
58 : : template <typename OtherDerived>
59 : : // cppcheck-suppress noExplicitConstructor
60 : 357625 : Vector3d(const Eigen::MatrixBase<OtherDerived>& other) : Eigen::Vector3d(other)
61 : : {
62 : 357625 : }
63 : :
64 : : template <typename OtherDerived>
65 : 16453 : Vector3d& operator=(const Eigen::MatrixBase<OtherDerived>& other)
66 : : {
67 : 16453 : this->Base::operator=(other);
68 : 16453 : return *this;
69 : : }
70 : :
71 : 825 : EIGEN_STRONG_INLINE Vector3d() : Vector3d(0., 0., 0.)
72 : : {
73 : 825 : }
74 : :
75 : 276583 : EIGEN_STRONG_INLINE Vector3d(const double& v0, const double& v1, const double& v2)
76 : 276583 : {
77 : 276584 : Base::_check_template_params();
78 : :
79 : 276584 : (*this) << v0, v1, v2;
80 : 276585 : }
81 : :
82 : 141 : void set(const Eigen::Vector3d& v)
83 : : {
84 : 141 : Base::_check_template_params();
85 : :
86 : 141 : set(v[0], v[1], v[2]);
87 : 141 : }
88 : :
89 : 384 : void set(const double& v0, const double& v1, const double& v2)
90 : : {
91 : 384 : Base::_check_template_params();
92 : :
93 : 384 : (*this) << v0, v1, v2;
94 : 384 : }
95 : :
96 : : inline void transform(const RobotDynamics::Math::SpatialTransform& X);
97 : :
98 : : inline Vector3d transform_copy(const RobotDynamics::Math::SpatialTransform& X) const;
99 : :
100 : : Matrix3d toTildeForm() const;
101 : : };
102 : :
103 : : class Matrix3d : public Eigen::Matrix3d
104 : : {
105 : : public:
106 : : typedef Eigen::Matrix3d Base;
107 : :
108 : : template <typename OtherDerived>
109 : : // cppcheck-suppress noExplicitConstructor
110 : 233302 : Matrix3d(const Eigen::MatrixBase<OtherDerived>& other) : Eigen::Matrix3d(other)
111 : : {
112 : 233307 : }
113 : :
114 : : Matrix3d(const Quaternion& o);
115 : :
116 : 1 : Matrix3d(const AxisAngle& a) : Eigen::Matrix3d(a)
117 : : {
118 : 1 : }
119 : :
120 : : template <typename OtherDerived>
121 : 436 : Matrix3d& operator=(const Eigen::MatrixBase<OtherDerived>& other)
122 : : {
123 : 436 : this->Base::operator=(other);
124 : 436 : return *this;
125 : : }
126 : :
127 : 2023 : EIGEN_STRONG_INLINE Matrix3d()
128 : 2023 : {
129 : 2023 : }
130 : :
131 : 92963 : EIGEN_STRONG_INLINE Matrix3d(const double& m00, const double& m01, const double& m02, const double& m10, const double& m11, const double& m12, const double& m20,
132 : : const double& m21, const double& m22)
133 : 92963 : {
134 : 92963 : Base::_check_template_params();
135 : :
136 : 92963 : (*this) << m00, m01, m02, m10, m11, m12, m20, m21, m22;
137 : 92963 : }
138 : :
139 : : /**
140 : : * @brief Convert rotation matrix to intrinsic ZYX euler angles
141 : : * @param m Rotation matrix to convert
142 : : * @param yaw_at_pitch_singularity At Y = +- PI/2 is a singularity in which there are multiple solutions. This will be the yaw value in the output and the
143 : : * roll value is dependent on this. To get the most accurate results at singularity, provide this value as close as possible to desired/reality and the resulting
144 : : * roll value will be near the expected
145 : : */
146 : : Vector3d toIntrinsicZYXAngles(double yaw_at_pitch_singularity = 0.) const;
147 : : };
148 : :
149 : : class Vector4d : public Eigen::Vector4d
150 : : {
151 : : public:
152 : : typedef Eigen::Vector4d Base;
153 : :
154 : : template <typename OtherDerived>
155 : : // cppcheck-suppress noExplicitConstructor
156 : 3 : Vector4d(const Eigen::MatrixBase<OtherDerived>& other) : Eigen::Vector4d(other)
157 : : {
158 : 3 : }
159 : :
160 : : template <typename OtherDerived>
161 : : Vector4d& operator=(const Eigen::MatrixBase<OtherDerived>& other)
162 : : {
163 : : this->Base::operator=(other);
164 : : return *this;
165 : : }
166 : :
167 : 94 : EIGEN_STRONG_INLINE Vector4d()
168 : 94 : {
169 : 94 : }
170 : :
171 : 270 : EIGEN_STRONG_INLINE Vector4d(const double& v0, const double& v1, const double& v2, const double& v3)
172 : 270 : {
173 : 270 : Base::_check_template_params();
174 : :
175 : 270 : (*this) << v0, v1, v2, v3;
176 : 270 : }
177 : :
178 : 3 : void set(const double& v0, const double& v1, const double& v2, const double& v3)
179 : : {
180 : 3 : Base::_check_template_params();
181 : :
182 : 3 : (*this) << v0, v1, v2, v3;
183 : 3 : }
184 : : };
185 : :
186 : : class SpatialVector : public Eigen::Matrix<double, 6, 1>
187 : : {
188 : : public:
189 : : typedef Eigen::Matrix<double, 6, 1> Base;
190 : :
191 : : template <typename OtherDerived>
192 : : // cppcheck-suppress noExplicitConstructor
193 : 101490 : SpatialVector(const Eigen::MatrixBase<OtherDerived>& other) : Eigen::Matrix<double, 6, 1>(other)
194 : : {
195 : 101489 : }
196 : :
197 : : template <typename OtherDerived>
198 : 12264 : SpatialVector& operator=(const Eigen::MatrixBase<OtherDerived>& other)
199 : : {
200 : 12264 : this->Base::operator=(other);
201 : 12264 : return *this;
202 : : }
203 : :
204 : 111080 : EIGEN_STRONG_INLINE SpatialVector()
205 : 111080 : {
206 : 111079 : (*this) << 0., 0., 0., 0., 0., 0.;
207 : 111079 : }
208 : :
209 : 65324 : EIGEN_STRONG_INLINE SpatialVector(const double& v0, const double& v1, const double& v2, const double& v3, const double& v4, const double& v5)
210 : 65324 : {
211 : 65324 : Base::_check_template_params();
212 : :
213 : 65324 : (*this) << v0, v1, v2, v3, v4, v5;
214 : 65324 : }
215 : :
216 : 2 : EIGEN_STRONG_INLINE SpatialVector(const Vector3d& angularPart, const Vector3d& linearPart)
217 : 2 : {
218 : 2 : Base::_check_template_params();
219 : :
220 : 2 : (*this) << angularPart[0], angularPart[1], angularPart[2], linearPart[0], linearPart[1], linearPart[2];
221 : 2 : }
222 : :
223 : 18171 : EIGEN_STRONG_INLINE void set(const double& v0, const double& v1, const double& v2, const double& v3, const double& v4, const double& v5)
224 : : {
225 : 18171 : Base::_check_template_params();
226 : :
227 : 18171 : (*this) << v0, v1, v2, v3, v4, v5;
228 : 18171 : }
229 : :
230 : 105275 : EIGEN_STRONG_INLINE Vector3d getAngularPart() const
231 : : {
232 : 105275 : return Vector3d(this->operator[](0), this->operator[](1), this->operator[](2));
233 : : }
234 : :
235 : 100061 : EIGEN_STRONG_INLINE Vector3d getLinearPart() const
236 : : {
237 : 100061 : return Vector3d(this->operator[](3), this->operator[](4), this->operator[](5));
238 : : }
239 : :
240 : : void zeroAngularPart()
241 : : {
242 : : this->operator[](0) = 0.;
243 : : this->operator[](1) = 0.;
244 : : this->operator[](2) = 0.;
245 : : }
246 : :
247 : : void zeroLinearPart()
248 : : {
249 : : this->operator[](3) = 0.;
250 : : this->operator[](4) = 0.;
251 : : this->operator[](5) = 0.;
252 : : }
253 : :
254 : 66911 : inline void setAngularPart(const Vector3d& v)
255 : : {
256 : 66911 : setAngularPart(v(0), v(1), v(2));
257 : 66910 : }
258 : :
259 : 66911 : inline void setAngularPart(double x, double y, double z)
260 : : {
261 : 66911 : this->operator[](0) = x;
262 : 66909 : this->operator[](1) = y;
263 : 66910 : this->operator[](2) = z;
264 : 66910 : }
265 : :
266 : 67275 : inline void setLinearPart(const Vector3d& v)
267 : : {
268 : 67275 : setLinearPart(v(0), v(1), v(2));
269 : 67275 : }
270 : :
271 : 67275 : inline void setLinearPart(double x, double y, double z)
272 : : {
273 : 67275 : this->operator[](3) = x;
274 : 67274 : this->operator[](4) = y;
275 : 67274 : this->operator[](5) = z;
276 : 67275 : }
277 : :
278 : : EIGEN_STRONG_INLINE void set(const Vector3d& angularPart, const Vector3d& linearPart)
279 : : {
280 : : Base::_check_template_params();
281 : :
282 : : (*this) << angularPart[0], angularPart[1], angularPart[2], linearPart[0], linearPart[1], linearPart[2];
283 : : }
284 : :
285 : : /**
286 : : * @brief Get the spatial motion cross matrix
287 : : * @param v
288 : : * @return \f$ v\times \f$
289 : : */
290 : : SpatialMatrix crossm();
291 : : /**
292 : : * @brief Spatial motion cross times spatial motion
293 : : * @param v1
294 : : * @param v2
295 : : * @return \f$ this \times v \f$
296 : : */
297 : : SpatialVector crossm(const SpatialVector& v);
298 : : /**
299 : : * @brief Get the spatial force cross matrix
300 : : * @param v
301 : : * @return \f$ v\times* \f$
302 : : */
303 : : SpatialMatrix crossf();
304 : : /**
305 : : * @brief Spatial motion cross spatial force
306 : : * @param v1 Spatial motion
307 : : * @param v2 Spatial force
308 : : * @return \f$ this \times* v \f$
309 : : */
310 : : SpatialVector crossf(const SpatialVector& v);
311 : : };
312 : :
313 : : class Matrix4d : public Eigen::Matrix<double, 4, 4>
314 : : {
315 : : public:
316 : : typedef Eigen::Matrix<double, 4, 4> Base;
317 : :
318 : : template <typename OtherDerived>
319 : : // cppcheck-suppress noExplicitConstructor
320 : : Matrix4d(const Eigen::MatrixBase<OtherDerived>& other) : Eigen::Matrix<double, 4, 4>(other)
321 : : {
322 : : }
323 : :
324 : : template <typename OtherDerived>
325 : : Matrix4d& operator=(const Eigen::MatrixBase<OtherDerived>& other)
326 : : {
327 : : this->Base::operator=(other);
328 : : return *this;
329 : : }
330 : :
331 : : EIGEN_STRONG_INLINE Matrix4d()
332 : : {
333 : : }
334 : :
335 : : EIGEN_STRONG_INLINE Matrix4d(const Scalar& m00, const Scalar& m01, const Scalar& m02, const Scalar& m03, const Scalar& m10, const Scalar& m11, const Scalar& m12,
336 : : const Scalar& m13, const Scalar& m20, const Scalar& m21, const Scalar& m22, const Scalar& m23, const Scalar& m30, const Scalar& m31,
337 : : const Scalar& m32, const Scalar& m33)
338 : : {
339 : : Base::_check_template_params();
340 : :
341 : : (*this) << m00, m01, m02, m03, m10, m11, m12, m13, m20, m21, m22, m23, m30, m31, m32, m33;
342 : : }
343 : :
344 : : void set(const Scalar& m00, const Scalar& m01, const Scalar& m02, const Scalar& m03, const Scalar& m10, const Scalar& m11, const Scalar& m12, const Scalar& m13,
345 : : const Scalar& m20, const Scalar& m21, const Scalar& m22, const Scalar& m23, const Scalar& m30, const Scalar& m31, const Scalar& m32, const Scalar& m33)
346 : : {
347 : : Base::_check_template_params();
348 : :
349 : : (*this) << m00, m01, m02, m03, m10, m11, m12, m13, m20, m21, m22, m23, m30, m31, m32, m33;
350 : : }
351 : : };
352 : :
353 : : class SpatialMatrix : public Eigen::Matrix<double, 6, 6>
354 : : {
355 : : public:
356 : : typedef Eigen::Matrix<double, 6, 6> Base;
357 : :
358 : : template <typename OtherDerived>
359 : : // cppcheck-suppress noExplicitConstructor
360 : 18809 : SpatialMatrix(const Eigen::MatrixBase<OtherDerived>& other) : Eigen::Matrix<double, 6, 6>(other)
361 : : {
362 : 18809 : }
363 : :
364 : : template <typename OtherDerived>
365 : 813 : SpatialMatrix& operator=(const Eigen::MatrixBase<OtherDerived>& other)
366 : : {
367 : 813 : this->Base::operator=(other);
368 : 813 : return *this;
369 : : }
370 : :
371 : 22377 : EIGEN_STRONG_INLINE SpatialMatrix()
372 : 22377 : {
373 : 22377 : Base::_check_template_params();
374 : :
375 : 22377 : (*this) << 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.;
376 : 22377 : }
377 : :
378 : 1002 : EIGEN_STRONG_INLINE SpatialMatrix(const Scalar& m00, const Scalar& m01, const Scalar& m02, const Scalar& m03, const Scalar& m04, const Scalar& m05, const Scalar& m10,
379 : : const Scalar& m11, const Scalar& m12, const Scalar& m13, const Scalar& m14, const Scalar& m15, const Scalar& m20, const Scalar& m21,
380 : : const Scalar& m22, const Scalar& m23, const Scalar& m24, const Scalar& m25, const Scalar& m30, const Scalar& m31, const Scalar& m32,
381 : : const Scalar& m33, const Scalar& m34, const Scalar& m35, const Scalar& m40, const Scalar& m41, const Scalar& m42, const Scalar& m43,
382 : : const Scalar& m44, const Scalar& m45, const Scalar& m50, const Scalar& m51, const Scalar& m52, const Scalar& m53, const Scalar& m54,
383 : : const Scalar& m55)
384 : 1002 : {
385 : 1002 : Base::_check_template_params();
386 : :
387 : 2004 : (*this) << m00, m01, m02, m03, m04, m05, m10, m11, m12, m13, m14, m15, m20, m21, m22, m23, m24, m25, m30, m31, m32, m33, m34, m35, m40, m41, m42, m43, m44, m45,
388 : 1002 : m50, m51, m52, m53, m54, m55;
389 : 1002 : }
390 : :
391 : 1 : void set(const Scalar& m00, const Scalar& m01, const Scalar& m02, const Scalar& m03, const Scalar& m04, const Scalar& m05, const Scalar& m10, const Scalar& m11,
392 : : const Scalar& m12, const Scalar& m13, const Scalar& m14, const Scalar& m15, const Scalar& m20, const Scalar& m21, const Scalar& m22, const Scalar& m23,
393 : : const Scalar& m24, const Scalar& m25, const Scalar& m30, const Scalar& m31, const Scalar& m32, const Scalar& m33, const Scalar& m34, const Scalar& m35,
394 : : const Scalar& m40, const Scalar& m41, const Scalar& m42, const Scalar& m43, const Scalar& m44, const Scalar& m45, const Scalar& m50, const Scalar& m51,
395 : : const Scalar& m52, const Scalar& m53, const Scalar& m54, const Scalar& m55)
396 : : {
397 : 1 : Base::_check_template_params();
398 : :
399 : 2 : (*this) << m00, m01, m02, m03, m04, m05, m10, m11, m12, m13, m14, m15, m20, m21, m22, m23, m24, m25, m30, m31, m32, m33, m34, m35, m40, m41, m42, m43, m44, m45,
400 : 1 : m50, m51, m52, m53, m54, m55;
401 : 1 : }
402 : : };
403 : :
404 : : /** \brief Compact representation of spatial transformations.
405 : : *
406 : : * Instead of using a verbose 6x6 matrix, this structure only stores a 3x3
407 : : * matrix and a 3-d vector to store spatial transformations. It also
408 : : * encapsulates efficient operations such as concatenations and
409 : : * transformation of spatial vectors.
410 : : */
411 : : struct SpatialTransform
412 : : {
413 : : /**
414 : : * @brief Constructor
415 : : */
416 : 52747 : SpatialTransform() : E(Matrix3d::Identity(3, 3)), r(Vector3d::Zero(3, 1))
417 : : {
418 : 52747 : }
419 : :
420 : : /**
421 : : * @brief Constructor
422 : : * @param rotation Orthogonal rotation matrix
423 : : * @param x X component
424 : : * @param y Y component
425 : : * @param z Z component
426 : : */
427 : : SpatialTransform(const Matrix3d& rotation, const double x, const double y, const double z) : E(rotation), r(x, y, z)
428 : : {
429 : : }
430 : :
431 : : SpatialTransform(const Quaternion& o, double x, double y, double z) : E(o), r(x, y, z)
432 : : {
433 : : }
434 : :
435 : : SpatialTransform(double x, double y, double z, double w);
436 : :
437 : 55 : SpatialTransform(const Quaternion& o) : E(o), r(0., 0., 0.)
438 : : {
439 : 55 : }
440 : :
441 : : /**
442 : : * @brief Constructor
443 : : * @param rotation Orthogonal rotation matrix
444 : : * @param translation 3D translational component
445 : : */
446 : 153698 : SpatialTransform(const Matrix3d& rotation, const Vector3d& translation) : E(rotation), r(translation)
447 : : {
448 : 153694 : }
449 : :
450 : : /**
451 : : * @brief Constructor
452 : : * @param rotation Orthogonal rotation matrix
453 : : */
454 : : SpatialTransform(const Matrix3d& rotation) : E(rotation), r(0., 0., 0.)
455 : : {
456 : : }
457 : :
458 : : /**
459 : : * @brief Constructor
460 : : * @param translation 3D translational component
461 : : */
462 : 115 : SpatialTransform(const Vector3d& translation) : E(1., 0., 0., 0., 1., 0., 0., 0., 1.), r(translation)
463 : : {
464 : 115 : }
465 : :
466 : : /**
467 : : * @brief Transform a spatial vector. Same as \f$ X * v \f$
468 : : * @param v_sp Spatial motion vector to be copied/transformed
469 : : *
470 : : * @returns Transformed spatial vector. \f$ \begin{bmatrix} E * w \\ - E * (r \times w) + E * v \end{bmatrix} \f$
471 : : */
472 : 3072 : SpatialVector apply(const SpatialVector& v_sp) const
473 : : {
474 : 3072 : Vector3d v_rxw(v_sp[3] - r[1] * v_sp[2] + r[2] * v_sp[1], v_sp[4] - r[2] * v_sp[0] + r[0] * v_sp[2], v_sp[5] - r[0] * v_sp[1] + r[1] * v_sp[0]);
475 : :
476 : 3072 : return SpatialVector(E(0, 0) * v_sp[0] + E(0, 1) * v_sp[1] + E(0, 2) * v_sp[2], E(1, 0) * v_sp[0] + E(1, 1) * v_sp[1] + E(1, 2) * v_sp[2],
477 : 3072 : E(2, 0) * v_sp[0] + E(2, 1) * v_sp[1] + E(2, 2) * v_sp[2], E(0, 0) * v_rxw[0] + E(0, 1) * v_rxw[1] + E(0, 2) * v_rxw[2],
478 : 9216 : E(1, 0) * v_rxw[0] + E(1, 1) * v_rxw[1] + E(1, 2) * v_rxw[2], E(2, 0) * v_rxw[0] + E(2, 1) * v_rxw[1] + E(2, 2) * v_rxw[2]);
479 : : }
480 : :
481 : : /**
482 : : * @brief Applies \f$ X^T * f \f$
483 : : * @param f_sp Spatial force
484 : : *
485 : : * @returns \f$ \begin{bmatrix} E^T * n + r \times * E^T * f \\ E^T * f \end{bmatrix} \f$
486 : : */
487 : 2 : SpatialVector applyTranspose(const SpatialVector& f_sp) const
488 : : {
489 : 2 : Vector3d E_T_f(E(0, 0) * f_sp[3] + E(1, 0) * f_sp[4] + E(2, 0) * f_sp[5], E(0, 1) * f_sp[3] + E(1, 1) * f_sp[4] + E(2, 1) * f_sp[5],
490 : 4 : E(0, 2) * f_sp[3] + E(1, 2) * f_sp[4] + E(2, 2) * f_sp[5]);
491 : :
492 : 2 : return SpatialVector(E(0, 0) * f_sp[0] + E(1, 0) * f_sp[1] + E(2, 0) * f_sp[2] - r[2] * E_T_f[1] + r[1] * E_T_f[2],
493 : 2 : E(0, 1) * f_sp[0] + E(1, 1) * f_sp[1] + E(2, 1) * f_sp[2] + r[2] * E_T_f[0] - r[0] * E_T_f[2],
494 : 6 : E(0, 2) * f_sp[0] + E(1, 2) * f_sp[1] + E(2, 2) * f_sp[2] - r[1] * E_T_f[0] + r[0] * E_T_f[1], E_T_f[0], E_T_f[1], E_T_f[2]);
495 : : }
496 : :
497 : : /**
498 : : * @brief Applies \f$ X * f \f$ where \f$ f \f$ is a spatial force
499 : : * @param f_sp Spatial force vector
500 : : *
501 : : * @return \f$ \begin{bmatrix} E * n - E * (r \times f) \\ E * f \end{bmatrix} \f$
502 : : */
503 : 2 : SpatialVector applyAdjoint(const SpatialVector& f_sp) const
504 : : {
505 : 2 : return SpatialVector(E * (f_sp.getAngularPart() - r.cross(f_sp.getLinearPart())), E * f_sp.getLinearPart());
506 : : }
507 : :
508 : : /**
509 : : * @brief Return transform as 6x6 spatial matrix
510 : : *
511 : : * @return \f$ \begin{bmatrix} E & \mathbf{0} \\ -E * r\times & E \end{bmatrix} \f$
512 : : */
513 : 3193 : SpatialMatrix toMatrix() const
514 : : {
515 : 3193 : SpatialMatrix result;
516 : :
517 : 3193 : result.block<3, 3>(0, 0) = E;
518 : 3193 : result.block<3, 3>(0, 3) = Matrix3d::Zero(3, 3);
519 : 3193 : result.block<3, 3>(3, 0) = -E * r.toTildeForm();
520 : 3193 : result.block<3, 3>(3, 3) = E;
521 : :
522 : 3193 : return result;
523 : : }
524 : :
525 : : /**
526 : : * @brief Returns Spatial transform that transforms spatial force vectors
527 : : *
528 : : * @return \f$ \begin{bmatrix} E & -E * r\times \\ \mathbf{0} & E \end{bmatrix} \f$
529 : : */
530 : 2897 : SpatialMatrix toMatrixAdjoint() const
531 : : {
532 : 2897 : SpatialMatrix result;
533 : :
534 : 2897 : result.block<3, 3>(0, 0) = E;
535 : 2897 : result.block<3, 3>(0, 3) = -E * r.toTildeForm();
536 : 2897 : result.block<3, 3>(3, 0) = Matrix3d::Zero(3, 3);
537 : 2897 : result.block<3, 3>(3, 3) = E;
538 : :
539 : 2897 : return result;
540 : : }
541 : :
542 : : /**
543 : : * @brief Returns spatial force transform transposed
544 : : *
545 : : * @return \f$ \begin{bmatrix} E^{T} & (-E r\times)^{T} \\ \mathbf{0} & E^{T} \end{bmatrix} \f$
546 : : */
547 : 2703 : SpatialMatrix toMatrixTranspose() const
548 : : {
549 : 2703 : Matrix3d _Erx = E * Matrix3d(0., -r[2], r[1], r[2], 0., -r[0], -r[1], r[0], 0.);
550 : 2703 : SpatialMatrix result;
551 : :
552 : 2703 : result.block<3, 3>(0, 0) = E.transpose();
553 : 2703 : result.block<3, 3>(0, 3) = -_Erx.transpose();
554 : 2703 : result.block<3, 3>(3, 0) = Matrix3d::Zero(3, 3);
555 : 2703 : result.block<3, 3>(3, 3) = E.transpose();
556 : :
557 : 5406 : return result;
558 : : }
559 : :
560 : : /**
561 : : * @brief Returns inverse of transform
562 : : *
563 : : * @return \f$ X^{-1} \f$
564 : : */
565 : 51083 : SpatialTransform inverse() const
566 : : {
567 : 51083 : return SpatialTransform(E.transpose(), -E * r);
568 : : }
569 : :
570 : : /**
571 : : * @brief Inverts in place. \f$ this = this^{-1} \f$
572 : : */
573 : 1 : void invert()
574 : : {
575 : 1 : r = -E * r;
576 : 1 : E.transposeInPlace();
577 : 1 : }
578 : :
579 : : /**
580 : : * @brief Overloaded * operator for combining transforms
581 : : * @param XT
582 : : * @return Combined rotation
583 : : */
584 : 74900 : SpatialTransform operator*(const SpatialTransform& XT) const
585 : : {
586 : 74900 : return SpatialTransform(E * XT.E, XT.r + XT.E.transpose() * r);
587 : : }
588 : :
589 : 1 : void operator*=(const SpatialTransform& XT)
590 : : {
591 : 1 : r = XT.r + XT.E.transpose() * r;
592 : 1 : E *= XT.E;
593 : 1 : }
594 : :
595 : : Matrix3d E;
596 : : Vector3d r;
597 : : };
598 : :
599 : 4 : Vector3d Vector3d::transform_copy(const RobotDynamics::Math::SpatialTransform& X) const
600 : : {
601 : 8 : return X.E * (*this);
602 : : }
603 : :
604 : 242 : void Vector3d::transform(const RobotDynamics::Math::SpatialTransform& X)
605 : : {
606 : 242 : *this = X.E * (*this);
607 : 242 : }
608 : :
609 : : /** \brief Quaternion that are used for \ref joint_singularities "singularity free" joints.
610 : : *
611 : : * order: x,y,z,w
612 : : */
613 : : class Quaternion : public Vector4d, public TransformableGeometricObject
614 : : {
615 : : public:
616 : : /**
617 : : * @brief Constructor
618 : : */
619 : 14 : Quaternion() : Vector4d(0., 0., 0., 1.)
620 : : {
621 : 14 : }
622 : :
623 : : // cppcheck-suppress noExplicitConstructor
624 : : Quaternion(const Eigen::Quaterniond& q) : Vector4d(q.x(), q.y(), q.z(), q.w())
625 : : {
626 : : }
627 : :
628 : : // cppcheck-suppress noExplicitConstructor
629 : 15 : Quaternion(const RobotDynamics::Math::Quaternion& q) : Vector4d(q.x(), q.y(), q.z(), q.w())
630 : : {
631 : 15 : }
632 : :
633 : : // cppcheck-suppress noExplicitConstructor
634 : : Quaternion(const Eigen::Product<Eigen::Matrix<double, 3, 3>, Eigen::Matrix<double, 3, 3>, 0> E) : Quaternion(Matrix3d(E))
635 : : {
636 : : }
637 : :
638 : : // cppcheck-suppress noExplicitConstructor
639 : : Quaternion(const Eigen::Matrix3d& E);
640 : :
641 : : // cppcheck-suppress noExplicitConstructor
642 : 7 : Quaternion(const Eigen::Matrix3d::ConstTransposeReturnType& E) : Quaternion(Eigen::Matrix3d(E))
643 : : {
644 : 7 : }
645 : :
646 : : Quaternion(Eigen::Transpose<Eigen::Matrix<double, 3, 3> > E) : Quaternion(Eigen::Matrix3d(E))
647 : : {
648 : : }
649 : :
650 : 3 : Quaternion(const Vector4d& v) : Vector4d(v[0], v[1], v[2], v[3])
651 : : {
652 : 3 : }
653 : :
654 : : // cppcheck-suppress noExplicitConstructor
655 : 4 : Quaternion(const AxisAngle& axis_angle) : Quaternion(axis_angle.axis(), axis_angle.angle())
656 : : {
657 : 4 : }
658 : :
659 : 52 : Quaternion(Vector3d axis, double angle)
660 : 52 : {
661 : 52 : double d = axis.norm();
662 : 52 : double s2 = std::sin(angle * 0.5) / d;
663 : 52 : (*this) << axis[0] * s2, axis[1] * s2, axis[2] * s2, std::cos(angle * 0.5);
664 : 52 : }
665 : :
666 : : /**
667 : : * @brief Constructor
668 : : * @param x
669 : : * @param y
670 : : * @param z
671 : : * @param w
672 : : */
673 : 238 : Quaternion(double x, double y, double z, double w) : Vector4d(x, y, z, w)
674 : : {
675 : 238 : }
676 : :
677 : 7 : AxisAngle toAxisAngle() const
678 : : {
679 : 7 : double n = vec().norm();
680 : 7 : if (n < std::numeric_limits<double>::epsilon())
681 : : {
682 : 0 : n = vec().stableNorm();
683 : : }
684 : :
685 : 7 : AxisAngle axisAngle;
686 : 7 : if (n > 0.0)
687 : : {
688 : 7 : axisAngle.angle() = 2.0 * atan2(n, w());
689 : 7 : axisAngle.axis() = vec() / n;
690 : : }
691 : : else
692 : : {
693 : 0 : axisAngle.angle() = 0.0;
694 : 0 : axisAngle.axis() << 1.0, 0.0, 0.0;
695 : : }
696 : :
697 : 14 : return axisAngle;
698 : : }
699 : :
700 : 25 : bool isApprox(Quaternion o, double eps) const
701 : : {
702 : 25 : if ((std::abs(x() - o.x()) < eps && std::abs(y() - o.y()) < eps && std::abs(z() - o.z()) < eps && std::abs(w() - o.w()) < eps) ||
703 : 0 : (std::abs(x() + o.x()) < eps && std::abs(y() + o.y()) < eps && std::abs(z() + o.z()) < eps && std::abs(w() + o.w()) < eps))
704 : : {
705 : 25 : return true;
706 : : }
707 : :
708 : 0 : return false;
709 : : }
710 : :
711 : 5 : bool isApprox(Matrix3d E, double eps) const
712 : : {
713 : 5 : return isApprox(Quaternion(E), eps);
714 : : }
715 : :
716 : 50 : EIGEN_STRONG_INLINE double& x()
717 : : {
718 : 50 : return this->operator[](0);
719 : : }
720 : :
721 : 140 : EIGEN_STRONG_INLINE double x() const
722 : : {
723 : 140 : return this->operator[](0);
724 : : }
725 : :
726 : 50 : EIGEN_STRONG_INLINE double& y()
727 : : {
728 : 50 : return this->operator[](1);
729 : : }
730 : :
731 : 140 : EIGEN_STRONG_INLINE double y() const
732 : : {
733 : 140 : return this->operator[](1);
734 : : }
735 : :
736 : 52 : EIGEN_STRONG_INLINE double& z()
737 : : {
738 : 52 : return this->operator[](2);
739 : : }
740 : :
741 : 140 : EIGEN_STRONG_INLINE double z() const
742 : : {
743 : 140 : return this->operator[](2);
744 : : }
745 : :
746 : 59 : EIGEN_STRONG_INLINE double& w()
747 : : {
748 : 59 : return this->operator[](3);
749 : : }
750 : :
751 : 147 : EIGEN_STRONG_INLINE double w() const
752 : : {
753 : 147 : return this->operator[](3);
754 : : }
755 : :
756 : : /**
757 : : * @brief Get vector part
758 : : * @return Vector part
759 : : */
760 : 25 : EIGEN_STRONG_INLINE Vector3d vec() const
761 : : {
762 : 25 : return Vector3d(this->operator[](0), this->operator[](1), this->operator[](2));
763 : : }
764 : :
765 : 1 : Quaternion& operator=(const Eigen::Quaterniond& q)
766 : : {
767 : 1 : (*this) << q.x(), q.y(), q.z(), q.w();
768 : 1 : return *this;
769 : : }
770 : :
771 : : /**
772 : : * @brief sanitize the quaternion by negating each element if the w element is less than zero
773 : : */
774 : 2 : void sanitize()
775 : : {
776 : 2 : if (!std::signbit(w()))
777 : : {
778 : 1 : return;
779 : : }
780 : :
781 : 1 : x() *= -1.0;
782 : 1 : y() *= -1.0;
783 : 1 : z() *= -1.0;
784 : 1 : w() *= -1.0;
785 : : }
786 : :
787 : 45 : Quaternion& operator=(const RobotDynamics::Math::Quaternion& q)
788 : : {
789 : 45 : (*this) << q.x(), q.y(), q.z(), q.w();
790 : 45 : return *this;
791 : : }
792 : :
793 : : Quaternion& operator=(const Vector4d& q)
794 : : {
795 : : (*this) << q[0], q[1], q[2], q[3];
796 : : return *this;
797 : : }
798 : :
799 : : Quaternion& operator=(const Matrix3d& E)
800 : : {
801 : : *this = Quaternion(E);
802 : : return *this;
803 : : }
804 : :
805 : : Quaternion& operator=(const AxisAngle& axis_angle)
806 : : {
807 : : *this = Quaternion(axis_angle);
808 : : return *this;
809 : : }
810 : :
811 : : /**
812 : : * @brief Method to scale the elements of a quaternion by a constant. Normalization is NOT performed
813 : : * @param s
814 : : * @return Scaled quaternion
815 : : */
816 : 11 : Quaternion operator*(const double& s) const
817 : : {
818 : 11 : return Quaternion((*this)[0] * s, (*this)[1] * s, (*this)[2] * s, (*this)[3] * s);
819 : : }
820 : :
821 : : /**
822 : : * @brief Quaternion multiplication
823 : : * @param q Quaternion to multiply by
824 : : * @return New multiplied quaternion result
825 : : */
826 : 72 : Quaternion operator*(const Quaternion& q) const
827 : : {
828 : : return Quaternion(
829 : 144 : (*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1], (*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2],
830 : 216 : (*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0], (*this)[3] * q[3] - (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2]);
831 : : }
832 : :
833 : : /**
834 : : * @brief Overloaded *= operator for quaternion multiplication
835 : : * @param q Quaternion to multiply by
836 : : * @return Modified result of the multiplication
837 : : */
838 : 1 : Quaternion& operator*=(const Quaternion& q)
839 : : {
840 : 1 : set((*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1], (*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2],
841 : 1 : (*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0], (*this)[3] * q[3] - (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2]);
842 : 1 : return *this;
843 : : }
844 : :
845 : 7 : void transform(const RobotDynamics::Math::SpatialTransform& X)
846 : : {
847 : 7 : *this = Quaternion(X.E.transpose()) * (*this);
848 : 7 : }
849 : :
850 : : /**
851 : : * @brief From Wikipedia: In computer graphics, Slerp is shorthand for spherical linear interpolation,
852 : : * introduced by Ken Shoemake in the context of quaternion interpolation for the
853 : : * purpose of animating 3D rotation. It refers to constant-speed motion along a unit-radius
854 : : * great circle arc, given the ends and an interpolation parameter between 0 and 1
855 : : * @note Only unit quaternions are valid rotations, so make sure to normalize
856 : : * @param alpha Interpolation parameter. Should be between 0 and 1
857 : : * @param quat Quaternion to interpolate between
858 : : * @return Interpolated quaternion
859 : : */
860 : : Quaternion slerp(double alpha, const Quaternion& quat) const;
861 : :
862 : 12 : Quaternion conjugate() const
863 : : {
864 : 12 : return Quaternion(-(*this)[0], -(*this)[1], -(*this)[2], (*this)[3]);
865 : : }
866 : :
867 : 1 : Quaternion timeStep(const Vector3d& omega, double dt)
868 : : {
869 : 1 : double omega_norm = omega.norm();
870 : :
871 : 2 : return Quaternion(omega / omega_norm, dt * omega_norm) * (*this);
872 : : }
873 : :
874 : 1 : Vector3d rotate(const Vector3d& vec) const
875 : : {
876 : 1 : Vector3d vn(vec);
877 : 1 : Quaternion vec_quat(vn[0], vn[1], vn[2], 0.f), res_quat;
878 : :
879 : 1 : res_quat = vec_quat * (*this);
880 : 1 : res_quat = conjugate() * res_quat;
881 : :
882 : 2 : return Vector3d(res_quat[0], res_quat[1], res_quat[2]);
883 : : }
884 : :
885 : : /**
886 : : * @brief Decompose a quaternion into a swing then twist quaternion where the twist is about the given axis
887 : : * @arg twist_axis Unit vector for the axis of the twist, e.g. (0., 0., 1.) for the z-axis
888 : : * @arg swing Modified. Location for the resulting swing quaternion to be stored
889 : : * @arg twist Modified. Location for the resulting twist quaternion to be stored
890 : : *
891 : : * @note This implementation can be found in PRZEMYSLAW DOBROWOLSKI's thesis titled, "SWING-TWIST DECOMPOSITION IN CLIFFORD ALGEBRA"
892 : : */
893 : 9 : void swingTwistDecomposition(const Vector3d& twist_axis, Quaternion& swing, Quaternion& twist)
894 : : {
895 : 9 : double u = twist_axis.dot(vec());
896 : 9 : double n = twist_axis.squaredNorm();
897 : 9 : double m = w() * n;
898 : 9 : double l = std::sqrt(std::pow(m, 2) + std::pow(u, 2) * n);
899 : 9 : twist << twist_axis.x() * (u / l), twist_axis.y() * (u / l), twist_axis.z() * (u / l), m / l;
900 : 9 : swing = *this * twist.conjugate();
901 : 9 : }
902 : :
903 : : /**
904 : : * @brief Convert to intrinsic ZYX euler angles
905 : : * @param yaw_at_pitch_singularity At Y = +- PI/2 is a singularity in which there are multiple solutions. This will be the yaw value in the output and the
906 : : * roll value is dependent on this. To get the most accurate results at singularity, provide this value as close as possible to desired/reality and the resulting
907 : : * roll value will be near the expected
908 : : */
909 : : Vector3d toIntrinsicZYXAngles(double yaw_at_pitch_singularity = 0.) const;
910 : : };
911 : :
912 : : /**
913 : : * @brief Convert YPR angles to quaternion
914 : : */
915 : 12 : static inline Quaternion intrinsicZYXAnglesToQuaternion(double yaw, double pitch, double roll)
916 : : {
917 : 24 : return Quaternion(Vector3d(0., 0., 1.), yaw) * Quaternion(Vector3d(0., 1., 0.), pitch) * Quaternion(Vector3d(1., 0., 0.), roll);
918 : : }
919 : :
920 : : /**
921 : : * @brief Convert YPR angles to quaternion
922 : : */
923 : 11 : static inline Quaternion intrinsicZYXAnglesToQuaternion(const Vector3d& zyx_angles)
924 : : {
925 : 11 : return intrinsicZYXAnglesToQuaternion(zyx_angles[0], zyx_angles[1], zyx_angles[2]);
926 : : }
927 : :
928 : : /**
929 : : * @brief Convert PRY angles to quaternion
930 : : */
931 : 1 : static inline Quaternion intrinsicYXZAnglesToQuaternion(double pitch, double roll, double yaw)
932 : : {
933 : 2 : return Quaternion(Vector3d(0., 1., 0.), pitch) * Quaternion(Vector3d(1., 0., 0.), roll) * Quaternion(Vector3d(0., 0., 1.), yaw);
934 : : }
935 : :
936 : : /**
937 : : * @brief Convert PRY angles to quaternion
938 : : */
939 : : static inline Quaternion intrinsicYXZAnglesToQuaternion(const Vector3d& yxz_angles)
940 : : {
941 : : return intrinsicYXZAnglesToQuaternion(yxz_angles[0], yxz_angles[1], yxz_angles[2]);
942 : : }
943 : :
944 : : /**
945 : : * @brief Convert RPY angles to quaternion
946 : : */
947 : 1 : static inline Quaternion intrinsicXYZAnglesToQuaternion(double roll, double pitch, double yaw)
948 : : {
949 : 2 : return Quaternion(Vector3d(1., 0., 0.), roll) * Quaternion(Vector3d(0., 1., 0.), pitch) * Quaternion(Vector3d(0., 0., 1.), yaw);
950 : : }
951 : :
952 : : /**
953 : : * @brief Convert RPY angles to quaternion
954 : : */
955 : : static inline Quaternion intrinsicXYZAnglesToQuaternion(const Vector3d& xyz_angles)
956 : : {
957 : : return intrinsicXYZAnglesToQuaternion(xyz_angles[0], xyz_angles[1], xyz_angles[2]);
958 : : }
959 : :
960 : : static inline Matrix3d toTildeForm(const double x, const double y, const double z)
961 : : {
962 : : return Matrix3d(0., -z, y, z, 0., -x, -y, z, 0.);
963 : : }
964 : :
965 : : inline std::ostream& operator<<(std::ostream& output, const SpatialTransform& X)
966 : : {
967 : : output << "X.E = " << std::endl << X.E << std::endl;
968 : : output << "X.r = " << X.r.transpose();
969 : : return output;
970 : : }
971 : :
972 : : /**
973 : : * @brief Get spatial transform from angle and axis
974 : : * @param angle_rad angle magnitude
975 : : * @param axis normalized 3d vector
976 : : * @return Spatial transform
977 : : */
978 : 6 : inline SpatialTransform Xrot(double angle_rad, const Vector3d& axis)
979 : : {
980 : : double s, c;
981 : :
982 : 6 : s = sin(angle_rad);
983 : 6 : c = cos(angle_rad);
984 : :
985 : 6 : return SpatialTransform(Matrix3d(axis[0] * axis[0] * (1.0f - c) + c, axis[1] * axis[0] * (1.0f - c) + axis[2] * s, axis[0] * axis[2] * (1.0f - c) - axis[1] * s,
986 : :
987 : 6 : axis[0] * axis[1] * (1.0f - c) - axis[2] * s, axis[1] * axis[1] * (1.0f - c) + c, axis[1] * axis[2] * (1.0f - c) + axis[0] * s,
988 : :
989 : 6 : axis[0] * axis[2] * (1.0f - c) + axis[1] * s, axis[1] * axis[2] * (1.0f - c) - axis[0] * s, axis[2] * axis[2] * (1.0f - c) + c
990 : :
991 : : ),
992 : 18 : Vector3d(0., 0., 0.));
993 : : }
994 : :
995 : : /**
996 : : * @brief Get transform with zero translation and pure rotation about x axis
997 : : * @param xrot
998 : : * @return Transform with zero translation and x-rotation
999 : : */
1000 : 2869 : inline SpatialTransform Xrotx(const double& xrot)
1001 : : {
1002 : : double s, c;
1003 : :
1004 : 2869 : s = sin(xrot);
1005 : 2869 : c = cos(xrot);
1006 : 5739 : return SpatialTransform(Matrix3d(1., 0., 0., 0., c, s, 0., -s, c), Vector3d(0., 0., 0.));
1007 : : }
1008 : :
1009 : : /**
1010 : : * @brief Get transform with zero translation and pure rotation about y axis
1011 : : * @param yrot
1012 : : * @return Transform with zero translation and y-rotation
1013 : : */
1014 : 4602 : inline SpatialTransform Xroty(const double& yrot)
1015 : : {
1016 : : double s, c;
1017 : :
1018 : 4602 : s = sin(yrot);
1019 : 4602 : c = cos(yrot);
1020 : 9204 : return SpatialTransform(Matrix3d(c, 0., -s, 0., 1., 0., s, 0., c), Vector3d(0., 0., 0.));
1021 : : }
1022 : :
1023 : : /**
1024 : : * @brief Get transform with zero translation and pure rotation about z axis
1025 : : * @param zrot
1026 : : * @return Transform with zero translation and z-rotation
1027 : : */
1028 : 3505 : inline SpatialTransform Xrotz(const double& zrot)
1029 : : {
1030 : : double s, c;
1031 : :
1032 : 3505 : s = sin(zrot);
1033 : 3505 : c = cos(zrot);
1034 : 7010 : return SpatialTransform(Matrix3d(c, s, 0., -s, c, 0., 0., 0., 1.), Vector3d(0., 0., 0.));
1035 : : }
1036 : :
1037 : : /**
1038 : : * @brief Get transform with zero translation and intrinsic euler z/y/x rotation
1039 : : * @param yaw
1040 : : * @param pitch
1041 : : * @param roll
1042 : : * @return Transform with zero translation and z/y/x rotation
1043 : : */
1044 : 11 : inline SpatialTransform XeulerZYX(double yaw, double pitch, double roll)
1045 : : {
1046 : 22 : return Xrotx(roll) * Xroty(pitch) * Xrotz(yaw);
1047 : : }
1048 : :
1049 : : /**
1050 : : * @brief Get transform defined by intrinsic YPR(yaw->pitch->roll) euler angles
1051 : : * @param ypr Vector of euler angles where ypr[0] is yaw, ypr[1] is pitch, and ypr[2] is roll
1052 : : *
1053 : : * @return spatial transform where rotation component is defined by the YPR rotations
1054 : : */
1055 : 11 : inline SpatialTransform XeulerZYX(const Vector3d& ypr)
1056 : : {
1057 : 11 : return XeulerZYX(ypr[0], ypr[1], ypr[2]);
1058 : : }
1059 : :
1060 : : /**
1061 : : * @brief Get transform with zero translation and intrinsic euler x/y/z rotation
1062 : : * @param roll
1063 : : * @param pitch
1064 : : * @param yaw
1065 : : * @return Transform with zero translation and x/y/z rotation
1066 : : */
1067 : : inline SpatialTransform XeulerXYZ(double roll, double pitch, double yaw)
1068 : : {
1069 : : return Xrotz(yaw) * Xroty(pitch) * Xrotx(roll);
1070 : : }
1071 : :
1072 : : /**
1073 : : * @brief Get transform with zero translation and euler x/y/z rotation
1074 : : * @param xyz_angles xyz_angles xyz angles where element 0 is roll, 1 is pitch, and 2 is yaw
1075 : : * @return Transform with zero translation and x/y/z rotation
1076 : : */
1077 : : inline SpatialTransform XeulerXYZ(const Vector3d& xyz_angles)
1078 : : {
1079 : : return XeulerXYZ(xyz_angles[0], xyz_angles[1], xyz_angles[2]);
1080 : : }
1081 : :
1082 : : inline SpatialTransform XrotQuat(double x, double y, double z, double w)
1083 : : {
1084 : : return SpatialTransform(Quaternion(x, y, z, w));
1085 : : }
1086 : :
1087 : 3 : inline SpatialTransform XrotQuat(const Quaternion& orientation)
1088 : : {
1089 : 3 : return SpatialTransform(orientation);
1090 : : }
1091 : :
1092 : : /**
1093 : : * @brief Get pure translation transform
1094 : : * @param r
1095 : : * @return Transform with identity rotation and translation \f$ r \f$
1096 : : */
1097 : 15020 : inline SpatialTransform Xtrans(const Vector3d& r)
1098 : : {
1099 : 15020 : return SpatialTransform(Matrix3d::Identity(3, 3), r);
1100 : : }
1101 : :
1102 : : inline SpatialTransform Xtrans(double x, double y, double z)
1103 : : {
1104 : : return SpatialTransform(Matrix3d::Identity(3, 3), Vector3d(x, y, z));
1105 : : }
1106 : : } // namespace Math
1107 : : } // namespace RobotDynamics
1108 : :
1109 : : #endif // ifndef __RDL_EIGENMATH_H__
|
