Kindr
Kinematics and Dynamics for Robotics
EulerAnglesXyz.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  * * Redistributions of source code must retain the above copyright
8  * notice, this list of conditions and the following disclaimer.
9  * * Redistributions in binary form must reproduce the above copyright
10  * notice, this list of conditions and the following disclaimer in the
11  * documentation and/or other materials provided with the distribution.
12  * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
13  * names of its contributors may be used to endorse or promote products
14  * derived from this software without specific prior written permission.
15  *
16  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19  * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
20  * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
21  * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
22  * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
23  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
24  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26  *
27 */
28 
29 #pragma once
30 
31 #include <cmath>
32 
33 #include <Eigen/Geometry>
34 
35 #include "kindr/common/common.hpp"
38 
39 namespace kindr {
40 
41 
55 template<typename PrimType_>
56 class EulerAnglesXyz : public RotationBase<EulerAnglesXyz<PrimType_>> {
57  private:
60  typedef Eigen::Matrix<PrimType_, 3, 1> Base;
61 
64  Base xyz_;
65 
66  public:
70  typedef Base Implementation;
74  typedef PrimType_ Scalar;
75 
78  typedef Base Vector;
79 
83  : xyz_(Base::Zero()) {
84  }
85 
91  EulerAnglesXyz(Scalar roll, Scalar pitch, Scalar yaw)
92  : xyz_(roll,pitch,yaw) {
93  }
94 
98  explicit EulerAnglesXyz(const Base& other)
99  : xyz_(other) {
100  }
101 
105  template<typename OtherDerived_>
106  inline explicit EulerAnglesXyz(const RotationBase<OtherDerived_>& other)
107  : xyz_(internal::ConversionTraits<EulerAnglesXyz, OtherDerived_>::convert(other.derived()).toImplementation()) {
108  }
109 
114  template<typename OtherDerived_>
116  this->toImplementation() = internal::ConversionTraits<EulerAnglesXyz, OtherDerived_>::convert(other.derived()).toImplementation();
117  return *this;
118  }
119 
124  template<typename OtherDerived_>
126  this->toImplementation() = internal::ConversionTraits<EulerAnglesXyz, OtherDerived_>::convert(other.derived()).toImplementation();
127  return *this;
128  }
129 
135  }
136 
141  *this = this->inverted();
142  return *this;
143  }
144 
148  inline const Vector vector() const {
149  return this->toImplementation();
150  }
151 
155  inline Base& toImplementation() {
156  return static_cast<Base&>(xyz_);
157  }
158 
162  inline const Base& toImplementation() const {
163  return static_cast<const Base&>(xyz_);
164  }
165 
169  inline Scalar roll() const {
170  return xyz_(0);
171  }
172 
176  inline Scalar pitch() const {
177  return xyz_(1);
178  }
179 
183  inline Scalar yaw() const {
184  return xyz_(2);
185  }
186 
189  inline void setRoll(Scalar roll) {
190  xyz_(0) = roll;
191  }
192 
195  inline void setPitch(Scalar pitch) {
196  xyz_(1) = pitch;
197  }
198 
201  inline void setYaw(Scalar yaw) {
202  xyz_(2) = yaw;
203  }
204 
208  inline Scalar x() const {
209  return xyz_(0);
210  }
211 
215  inline Scalar y() const {
216  return xyz_(1);
217  }
218 
222  inline Scalar z() const {
223  return xyz_(2);
224  }
225 
228  inline void setX(Scalar x) {
229  xyz_(0) = x;
230  }
231 
234  inline void setY(Scalar y) {
235  xyz_(1) = y;
236  }
237 
240  inline void setZ(Scalar z) {
241  xyz_(2) = z;
242  }
243 
248  xyz_.setZero();
249  return *this;
250  }
251 
257  Base xyz(kindr::floatingPointModulo(x()+M_PI,2*M_PI)-M_PI,
258  kindr::floatingPointModulo(y()+M_PI,2*M_PI)-M_PI,
259  kindr::floatingPointModulo(z()+M_PI,2*M_PI)-M_PI); // wrap all angles into [-pi,pi)
260 
261  const double tol = 1e-3;
262 
263  // wrap angles into [-pi,pi),[-pi/2,pi/2),[-pi,pi)
264  if(xyz.y() < -M_PI/2 - tol)
265  {
266  if(xyz.x() < 0) {
267  xyz.x() = xyz.x() + M_PI;
268  } else {
269  xyz.x() = xyz.x() - M_PI;
270  }
271 
272  xyz.y() = -(xyz.y() + M_PI);
273 
274  if(xyz.z() < 0) {
275  xyz.z() = xyz.z() + M_PI;
276  } else {
277  xyz.z() = xyz.z() - M_PI;
278  }
279  }
280  else if(-M_PI/2 - tol <= xyz.y() && xyz.y() <= -M_PI/2 + tol)
281  {
282  xyz.x() -= xyz.z();
283  xyz.z() = 0;
284  }
285  else if(-M_PI/2 + tol < xyz.y() && xyz.y() < M_PI/2 - tol)
286  {
287  // ok
288  }
289  else if(M_PI/2 - tol <= xyz.y() && xyz.y() <= M_PI/2 + tol)
290  {
291  // todo: M_PI/2 should not be in range, other formula?
292  xyz.x() += xyz.z();
293  xyz.z() = 0;
294  }
295  else // M_PI/2 + tol < xyz.y()
296  {
297  if(xyz.x() < 0) {
298  xyz.x() = xyz.x() + M_PI;
299  } else {
300  xyz.x() = xyz.x() - M_PI;
301  }
302 
303  xyz.y() = -(xyz.y() - M_PI);
304 
305  if(xyz.z() < 0) {
306  xyz.z() = xyz.z() + M_PI;
307  } else {
308  xyz.z() = xyz.z() - M_PI;
309  }
310  }
311 
312  return EulerAnglesXyz(xyz);
313  }
314 
319  *this = getUnique();
320  return *this;
321  }
322 
323  typename Eigen::Matrix<PrimType_, 3, 3> getMappingFromDiffToLocalAngularVelocity() const {
324  using std::sin;
325  using std::cos;
326  Eigen::Matrix<PrimType_, 3, 3> mat = Eigen::Matrix<PrimType_, 3, 3>::Zero();
327  const PrimType_ x = this->x();
328  const PrimType_ y = this->y();
329  const PrimType_ z = this->z();
330 
331  const PrimType_ t2 = cos(y);
332  const PrimType_ t3 = sin(z);
333  const PrimType_ t4 = cos(z);
334  mat(0, 0) = t2*t4;
335  mat(0, 1) = t3;
336  mat(1, 0) = -t2*t3;
337  mat(1, 1) = t4;
338  mat(2, 0) = sin(y);
339  mat(2, 2) = 1.0;
340 
341  return mat;
342  }
343 
344  typename Eigen::Matrix<PrimType_, 3, 3> getMappingFromLocalAngularVelocityToDiff() const {
345  using std::sin;
346  using std::cos;
347  Eigen::Matrix<PrimType_, 3, 3> mat = Eigen::Matrix<PrimType_, 3, 3>::Zero();
348  const PrimType_ x = this->x();
349  const PrimType_ y = this->y();
350  const PrimType_ z = this->z();
351 
352  const PrimType_ t2 = cos(y);
353  KINDR_ASSERT_TRUE(std::runtime_error, t2 != PrimType_(0.0), "Gimbal lock: cos(y) is zero!");
354  const PrimType_ t3 = 1.0/t2;
355  const PrimType_ t4 = sin(z);
356  const PrimType_ t5 = cos(z);
357  const PrimType_ t6 = sin(y);
358  mat(0,0) = t3*t5;
359  mat(0,1) = -t3*t4;
360  mat(1,0) = t4;
361  mat(1,1) = t5;
362  mat(2,0) = -t3*t5*t6;
363  mat(2,1) = t3*t4*t6;
364  mat(2,2) = 1.0;
365 
366  return mat;
367  }
368 
369  typename Eigen::Matrix<PrimType_, 3, 3> getMappingFromGlobalAngularVelocityToDiff() const {
370  using std::sin;
371  using std::cos;
372  Eigen::Matrix<PrimType_, 3, 3> mat = Eigen::Matrix<PrimType_, 3, 3>::Zero();
373  const PrimType_ x = this->x();
374  const PrimType_ y = this->y();
375  const PrimType_ z = this->z();
376  const PrimType_ t2 = cos(y);
377  KINDR_ASSERT_TRUE(std::runtime_error, t2 != PrimType_(0.0), "Gimbal lock: cos(y) is zero!");
378  const PrimType_ t3 = 1.0/t2;
379  const PrimType_ t4 = sin(y);
380  const PrimType_ t5 = cos(x);
381  const PrimType_ t6 = sin(x);
382  mat(0,0) = 1.0;
383  mat(0,1) = t3*t4*t6;
384  mat(0,2) = -t3*t4*t5;
385  mat(1,1) = t5;
386  mat(1,2) = t6;
387  mat(2,1) = -t3*t6;
388  mat(2,2) = t3*t5;
389  return mat;
390  }
391 
392 
393  typename Eigen::Matrix<PrimType_, 3, 3> getMappingFromDiffToGlobalAngularVelocity() const {
394  using std::sin;
395  using std::cos;
396  Eigen::Matrix<PrimType_, 3, 3> mat = Eigen::Matrix<PrimType_, 3, 3>::Zero();
397  const PrimType_ x = this->x();
398  const PrimType_ y = this->y();
399  const PrimType_ z = this->z();
400  const PrimType_ t2 = sin(x);
401  const PrimType_ t3 = cos(x);
402  const PrimType_ t4 = cos(y);
403  mat(0,0) = 1.0;
404  mat(0,2) = sin(y);
405  mat(1,1) = t3;
406  mat(1,2) = -t2*t4;
407  mat(2,1) = t2;
408  mat(2,2) = t3*t4;
409  return mat;
410  }
411 
412 
417  using RotationBase<EulerAnglesXyz<PrimType_>>::operator*;
418 
423  using RotationBase<EulerAnglesXyz<PrimType_>>::operator==;
424 
428  friend std::ostream& operator << (std::ostream& out, const EulerAnglesXyz& xyz) {
429  out << xyz.toImplementation().transpose();
430  return out;
431  }
432 };
433 
442 
444 template <typename PrimType_>
446 
455 
456 
457 
458 namespace internal {
459 
460 template<typename PrimType_>
461 class get_scalar<EulerAnglesXyz<PrimType_>> {
462  public:
463  typedef PrimType_ Scalar;
464 };
465 
466 template<typename PrimType_>
467 class get_matrix3X<EulerAnglesXyz<PrimType_>>{
468  public:
469  typedef int IndexType;
470 
471  template <IndexType Cols>
472  using Matrix3X = Eigen::Matrix<PrimType_, 3, Cols>;
473 };
474 
475 
476 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
477  * Conversion Traits
478  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
479 template<typename DestPrimType_, typename SourcePrimType_>
480 class ConversionTraits<EulerAnglesXyz<DestPrimType_>, AngleAxis<SourcePrimType_>> {
481  public:
482  inline static EulerAnglesXyz<DestPrimType_> convert(const AngleAxis<SourcePrimType_>& aa) {
483 // return EulerAnglesXyz<DestPrimType_>(getRpyFromAngleAxis<SourcePrimType_, DestPrimType_>(aa.toImplementation()));
485  }
486 };
487 
488 template<typename DestPrimType_, typename SourcePrimType_>
489 class ConversionTraits<EulerAnglesXyz<DestPrimType_>, RotationVector<SourcePrimType_>> {
490  public:
491  inline static EulerAnglesXyz<DestPrimType_> convert(const RotationVector<SourcePrimType_>& rotationVector) {
493  }
494 };
495 
496 template<typename DestPrimType_, typename SourcePrimType_>
497 class ConversionTraits<EulerAnglesXyz<DestPrimType_>, RotationQuaternion<SourcePrimType_>> {
498  public:
500  Eigen::Matrix<DestPrimType_, 3, 1> vec = q.toImplementation().toRotationMatrix().eulerAngles(0, 1, 2).template cast<DestPrimType_>();
501  return EulerAnglesXyz<DestPrimType_>(vec(0), vec(1), vec(2));
502  }
503 };
504 
505 
506 template<typename DestPrimType_, typename SourcePrimType_>
507 class ConversionTraits<EulerAnglesXyz<DestPrimType_>, RotationMatrix<SourcePrimType_>> {
508  public:
509  inline static EulerAnglesXyz<DestPrimType_> convert(const RotationMatrix<SourcePrimType_>& R) {
510  return EulerAnglesXyz<DestPrimType_>(RotationQuaternion<DestPrimType_>(R));
511  }
512 };
513 
514 template<typename DestPrimType_, typename SourcePrimType_>
515 class ConversionTraits<EulerAnglesXyz<DestPrimType_>, EulerAnglesXyz<SourcePrimType_>> {
516  public:
517  inline static EulerAnglesXyz<DestPrimType_> convert(const EulerAnglesXyz<SourcePrimType_>& xyz) {
518  return EulerAnglesXyz<DestPrimType_>(xyz.toImplementation().template cast<DestPrimType_>());
519  }
520 };
521 
522 template<typename DestPrimType_, typename SourcePrimType_>
523 class ConversionTraits<EulerAnglesXyz<DestPrimType_>, EulerAnglesZyx<SourcePrimType_>> {
524  public:
525  inline static EulerAnglesXyz<DestPrimType_> convert(const EulerAnglesZyx<SourcePrimType_>& zyx) {
526  return EulerAnglesXyz<DestPrimType_>(RotationQuaternion<DestPrimType_>(zyx));
527  }
528 };
529 
530 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
531  * Multiplication Traits
532  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
533 
534 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
535  * Rotation Traits
536  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
537 
538 
539 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
540  * Comparison Traits
541  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
542 
543 
544 
545 } // namespace internal
546 } // namespace kindr
void setPitch(Scalar pitch)
Sets pitch (Y&#39;) angle.
Eigen::Matrix< PrimType_, 3, 3 > getMappingFromDiffToLocalAngularVelocity() const
EulerAnglesXyz< PrimType_ > & derived()
Gets the derived rotation. (only for advanced users)
Scalar x() const
Gets roll (X) angle.
EulerAnglesXyz inverted() const
Returns the inverse of the rotation.
EulerAnglesXyz(const RotationBase< OtherDerived_ > &other)
Constructor using another rotation.
Scalar y() const
Gets pitch (Y&#39;) angle.
Eigen::Matrix< PrimType_, 3, 1 > Base
The base type.
Base xyz_
vector of Euler angles [roll; pitch; yaw]
EulerAnglesRpy< float > EulerAnglesRpyF
Active Euler angles rotation (X,Y&#39;,Z&#39;&#39; / roll,pitch,yaw) with float primitive type.
EulerAnglesRpy< double > EulerAnglesRpyPD
Passive Euler angles rotation (X,Y&#39;,Z&#39;&#39; / roll,pitch,yaw) with double primitive type.
const Vector vector() const
Returns the Euler angles in a vector.
Implementation of Euler angles (Z-Y&#39;-X&#39;&#39; / yaw-pitch-roll) rotation based on Eigen::Matrix<Scalar, 3, 1>
EulerAnglesXyz(Scalar roll, Scalar pitch, Scalar yaw)
Constructor using three scalars.
Representation of a generic rotationThis class defines the generic interface for a rotation...
RotationQuaternion inverted() const
Returns the inverse of the rotation.
EulerAnglesXyz(const Base &other)
Constructor using Eigen::Matrix.
EulerAnglesXyz & operator()(const RotationBase< OtherDerived_ > &other)
Parenthesis operator to convert from another rotation.
EulerAnglesXyz & operator=(const RotationBase< OtherDerived_ > &other)
Assignment operator using another rotation.
Eigen::Matrix< PrimType_, 3, 3 > getMappingFromLocalAngularVelocityToDiff() const
EulerAnglesXyz< float > EulerAnglesXyzF
Active Euler angles rotation (X,Y&#39;,Z&#39;&#39; / roll,pitch,yaw) with float primitive type.
EulerAnglesXyz< double > EulerAnglesXyzD
Active Euler angles rotation (X,Y&#39;,Z&#39;&#39; / roll,pitch,yaw) with double primitive type.
void setYaw(Scalar yaw)
Sets yaw (Z&#39;&#39;) angle.
void setX(Scalar x)
Sets roll (X) angle.
T floatingPointModulo(T x, T y)
Floating-point modulo.
Definition: common.hpp:46
Implementation & toImplementation()
Cast to the implementation type.
Scalar pitch() const
Returns pitch (Y&#39;) angle.
void setRoll(Scalar roll)
Sets roll (X) angle.
EulerAnglesXyz()
Default constructor using identity rotation.
PrimType_ Scalar
The primitive type. Float/Double.
Implementation of a rotation vector based on Eigen::Matrix<Scalar, 3, 1>
Definition: Rotation.hpp:43
friend std::ostream & operator<<(std::ostream &out, const EulerAnglesXyz &xyz)
Used for printing the object with std::cout.
EulerAnglesRpy< double > EulerAnglesRpyD
Active Euler angles rotation (X,Y&#39;,Z&#39;&#39; / roll,pitch,yaw) with double primitive type.
Implementation of matrix rotation based on Eigen::Matrix<Scalar, 3, 3>
Definition: Rotation.hpp:49
EulerAnglesXyz< float > EulerAnglesXyzPF
Passive Euler angles rotation (X,Y&#39;,Z&#39;&#39; / roll,pitch,yaw) with float primitive type.
Eigen::Matrix< PrimType_, 3, 3 > getMappingFromDiffToGlobalAngularVelocity() const
Implementation of Euler angles (X-Y&#39;-Z&#39;&#39; / roll-pitch-yaw) rotation based on Eigen::Matrix<Scalar,3,1>
Scalar yaw() const
Returns yaw (Z&#39;&#39;) angle.
const Base & toImplementation() const
Cast to the implementation type.
Base Vector
Rotation Vector as 3x1-matrix.
Scalar roll() const
Returns roll (X) angle.
Base Implementation
The implementation type. The implementation type is always an Eigen object.
EulerAnglesXyz & invert()
Inverts the rotation.
EulerAnglesXyz & setUnique()
Modifies the Euler angles rotation such that the angles lie in [-pi,pi),[-pi/2,pi/2),[-pi,pi).
void setY(Scalar y)
Sets pitch (Y&#39;) angle.
Eigen::Matrix< PrimType_, 3, 3 > getMappingFromGlobalAngularVelocityToDiff() const
Base & toImplementation()
Cast to the implementation type.
Implementation of an angle axis rotation based on Eigen::AngleAxis.
Definition: AngleAxis.hpp:54
void setZ(Scalar z)
Sets yaw (Z&#39;&#39;) angle.
Implementation of quaternion rotation based on Eigen::Quaternion.
Definition: Rotation.hpp:46
EulerAnglesRpy< float > EulerAnglesRpyPF
Passive Euler angles rotation (X,Y&#39;,Z&#39;&#39; / roll,pitch,yaw) with float primitive type.
EulerAnglesXyz< double > EulerAnglesXyzPD
Passive Euler angles rotation (X,Y&#39;,Z&#39;&#39; / roll,pitch,yaw) with double primitive type.
EulerAnglesXyz & setIdentity()
Sets the rotation to identity.
EulerAnglesXyz getUnique() const
Returns a unique Euler angles rotation with angles in [-pi,pi),[-pi/2,pi/2),[-pi,pi). This function is used to compare different rotations.
Scalar z() const
Gets yaw (Z&#39;&#39;) angle.
#define KINDR_ASSERT_TRUE(exceptionType, condition, message)