Kindr
Kinematics and Dynamics for Robotics
EulerAnglesZyx.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 
53 template<typename PrimType_>
54 class EulerAnglesZyx : public RotationBase<EulerAnglesZyx<PrimType_>> {
55  private:
58  typedef Eigen::Matrix<PrimType_, 3, 1> Base;
59 
62  Base zyx_;
63  public:
67  typedef Base Implementation;
71  typedef PrimType_ Scalar;
72 
75  typedef Base Vector;
76 
80  : zyx_(Base::Zero()) {
81  }
82 
88  EulerAnglesZyx(Scalar yaw, Scalar pitch, Scalar roll)
89  : zyx_(yaw,pitch,roll) {
90  }
91 
95  explicit EulerAnglesZyx(const Base& other)
96  : zyx_(other) {
97  }
98 
102  template<typename OtherDerived_>
103  inline explicit EulerAnglesZyx(const RotationBase<OtherDerived_>& other)
104  : zyx_(internal::ConversionTraits<EulerAnglesZyx, OtherDerived_>::convert(other.derived()).toImplementation()) {
105  }
106 
111  template<typename OtherDerived_>
113  this->toImplementation() = internal::ConversionTraits<EulerAnglesZyx, OtherDerived_>::convert(other.derived()).toImplementation();
114  return *this;
115  }
116 
117 
122  template<typename OtherDerived_>
124  this->toImplementation() = internal::ConversionTraits<EulerAnglesZyx, OtherDerived_>::convert(other.derived()).toImplementation();
125  return *this;
126  }
127 
133  }
134 
139  *this = this->inverted();
140  return *this;
141  }
142 
146  inline const Vector vector() const {
147  return this->toImplementation();
148  }
149 
153  inline Base& toImplementation() {
154  return static_cast<Base&>(zyx_);
155  }
156 
160  inline const Base& toImplementation() const {
161  return static_cast<const Base&>(zyx_);
162  }
163 
167  inline Scalar yaw() const {
168  return zyx_(0);
169  }
170 
174  inline Scalar pitch() const {
175  return zyx_(1);
176  }
177 
181  inline Scalar roll() const {
182  return zyx_(2);
183  }
184 
187  inline void setYaw(Scalar yaw) {
188  zyx_(0) = yaw;
189  }
190 
193  inline void setPitch(Scalar pitch) {
194  zyx_(1) = pitch;
195  }
196 
199  inline void setRoll(Scalar roll) {
200  zyx_(2) = roll;
201  }
202 
206  inline Scalar z() const {
207  return zyx_(0);
208  }
209 
213  inline Scalar y() const {
214  return zyx_(1);
215  }
216 
220  inline Scalar x() const {
221  return zyx_(2);
222  }
223 
227  inline void setZ(Scalar z) {
228  zyx_(0) = z;
229  }
230 
234  inline void setY(Scalar y) {
235  zyx_(1) = y;
236  }
237 
241  inline void setX(Scalar x) {
242  zyx_(2) = x;
243  }
244 
249  zyx_.setZero();
250  return *this;
251  }
252 
253 // /*! \brief Returns a unique Euler angles rotation with angles in [-pi,pi),[-pi/2,pi/2),[-pi,pi).
254 // * This function is used to compare different rotations.
255 // * \returns copy of the Euler angles rotation which is unique
256 // */
257 // EulerAnglesZyx getUnique() const {
258 // //return EulerAnglesZyx<Scalar>(RotationQuaternion<Scalar>(*this).getUnique());
259 // Base zyx(kindr::floatingPointModulo(z()+M_PI,2*M_PI)-M_PI,
260 // kindr::floatingPointModulo(y()+M_PI,2*M_PI)-M_PI,
261 // kindr::floatingPointModulo(x()+M_PI,2*M_PI)-M_PI); // wrap all angles into [-pi,pi)
262 //
263 // const double tol = 1e-3;
264 // Scalar& z = zyx(0);
265 // Scalar& y = zyx(1);
266 // Scalar& x = zyx(2);
267 //
268 // // wrap angles into [-pi,pi),[-pi/2,pi/2),[-pi,pi)
269 // if(y < -M_PI/2 - tol)
270 // {
271 // if(z < 0) {
272 // z = z + M_PI;
273 // } else {
274 // z = z - M_PI;
275 // }
276 //
277 // y = -(y + M_PI);
278 //
279 // if(x < 0) {
280 // x = x + M_PI;
281 // } else {
282 // x = x - M_PI;
283 // }
284 // }
285 // else if(-M_PI/2 - tol <= y && y <= -M_PI/2 + tol)
286 // {
287 // z += x;
288 // x = Scalar(0.0);
289 // }
290 // else if(-M_PI/2 + tol < y && y < M_PI/2 - tol)
291 // {
292 // // ok
293 // }
294 // else if(M_PI/2 - tol <= y && y <= M_PI/2 + tol)
295 // {
296 // // todo: M_PI/2 should not be in range, other formula?
297 // z -= x;
298 // x = 0;
299 // }
300 // else // M_PI/2 + tol < zyx.y()
301 // {
302 // if(z < 0) {
303 // z = z + M_PI;
304 // } else {
305 // z = z - M_PI;
306 // }
307 //
308 // y = -(y - M_PI);
309 //
310 // if(x < 0) {
311 // x = x + M_PI;
312 // } else {
313 // x = x - M_PI;
314 // }
315 // }
316 //
317 // return EulerAnglesZyx(zyx);
318 // }
319 
320 
326  Base zyx(kindr::floatingPointModulo(z()+M_PI,2*M_PI)-M_PI,
327  kindr::floatingPointModulo(y()+M_PI,2*M_PI)-M_PI,
328  kindr::floatingPointModulo(x()+M_PI,2*M_PI)-M_PI); // wrap all angles into [-pi,pi)
329 
330  const double tol = 1e-3;
331 
332  // wrap angles into [-pi,pi),[-pi/2,pi/2),[-pi,pi)
333  if(zyx.y() < -M_PI/2 - tol)
334  {
335  if(zyx.x() < 0) {
336  zyx.x() = zyx.x() + M_PI;
337  } else {
338  zyx.x() = zyx.x() - M_PI;
339  }
340 
341  zyx.y() = -(zyx.y() + M_PI);
342 
343  if(zyx.z() < 0) {
344  zyx.z() = zyx.z() + M_PI;
345  } else {
346  zyx.z() = zyx.z() - M_PI;
347  }
348  }
349  else if(-M_PI/2 - tol <= zyx.y() && zyx.y() <= -M_PI/2 + tol)
350  {
351  zyx.x() -= zyx.z();
352  zyx.z() = 0;
353  }
354  else if(-M_PI/2 + tol < zyx.y() && zyx.y() < M_PI/2 - tol)
355  {
356  // ok
357  }
358  else if(M_PI/2 - tol <= zyx.y() && zyx.y() <= M_PI/2 + tol)
359  {
360  // todo: M_PI/2 should not be in range, other formula?
361  zyx.x() += zyx.z();
362  zyx.z() = 0;
363  }
364  else // M_PI/2 + tol < zyx.y()
365  {
366  if(zyx.x() < 0) {
367  zyx.x() = zyx.x() + M_PI;
368  } else {
369  zyx.x() = zyx.x() - M_PI;
370  }
371 
372  zyx.y() = -(zyx.y() - M_PI);
373 
374  if(zyx.z() < 0) {
375  zyx.z() = zyx.z() + M_PI;
376  } else {
377  zyx.z() = zyx.z() - M_PI;
378  }
379  }
380 
381  return EulerAnglesZyx(zyx);
382  }
383 
387  EulerAnglesZyx& setUnique() { // wraps angles into [-pi,pi),[-pi/2,pi/2),[-pi,pi)
388  *this = getUnique();
389  return *this;
390  }
391 
392 
393  typename Eigen::Matrix<PrimType_, 3, 3> getMappingFromDiffToLocalAngularVelocity() 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 = cos(x);
401  const PrimType_ t3 = cos(y);
402  const PrimType_ t4 = sin(x);
403  mat(0, 0) = -sin(y);
404  mat(0, 2) = 1.0;
405  mat(1, 0) = t3*t4;
406  mat(1, 1) = t2;
407  mat(2, 0) = t2*t3;
408  mat(2, 1) = -t4;
409  return mat;
410  }
411 
412  typename Eigen::Matrix<PrimType_, 3, 3> getMappingFromLocalAngularVelocityToDiff() const {
413  using std::sin;
414  using std::cos;
415  Eigen::Matrix<PrimType_, 3, 3> mat = Eigen::Matrix<PrimType_, 3, 3>::Zero();
416  const PrimType_ x = this->x();
417  const PrimType_ y = this->y();
418  const PrimType_ z = this->z();
419  const PrimType_ t2 = cos(y);
420  const PrimType_ t3 = 1.0/t2;
421  KINDR_ASSERT_TRUE(std::runtime_error, t2 != PrimType_(0.0), "Gimbal lock: cos(y) is zero!");
422  const PrimType_ t4 = cos(x);
423  const PrimType_ t5 = sin(x);
424  const PrimType_ t6 = sin(y);
425  mat(0,1) = t3*t5;
426  mat(0,2) = t3*t4;
427  mat(1,1) = t4;
428  mat(1,2) = -t5;
429  mat(2,0) = 1.0;
430  mat(2,1) = t3*t5*t6;
431  mat(2,2) = t3*t4*t6;
432  return mat;
433  }
434 
439  using RotationBase<EulerAnglesZyx<PrimType_>>::operator*;
440 
445  using RotationBase<EulerAnglesZyx<PrimType_>>::operator==;
446 
450  friend std::ostream& operator << (std::ostream& out, const EulerAnglesZyx& zyx) {
451  out << zyx.toImplementation().transpose();
452  return out;
453  }
454 };
455 
464 
466 template <typename PrimType_>
468 
477 
478 
479 
480 namespace internal {
481 
482 template<typename PrimType_>
483 class get_scalar<EulerAnglesZyx<PrimType_>> {
484  public:
485  typedef PrimType_ Scalar;
486 };
487 
488 template<typename PrimType_>
489 class get_matrix3X<EulerAnglesZyx<PrimType_>>{
490  public:
491  typedef int IndexType;
492 
493  template <IndexType Cols>
494  using Matrix3X = Eigen::Matrix<PrimType_, 3, Cols>;
495 };
496 
497 
498 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
499  * Conversion Traits
500  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
501 template<typename DestPrimType_, typename SourcePrimType_>
502 class ConversionTraits<EulerAnglesZyx<DestPrimType_>, AngleAxis<SourcePrimType_>> {
503  public:
504  inline static EulerAnglesZyx<DestPrimType_> convert(const AngleAxis<SourcePrimType_>& aa) {
506  }
507 };
508 
509 template<typename DestPrimType_, typename SourcePrimType_>
510 class ConversionTraits<EulerAnglesZyx<DestPrimType_>, RotationMatrix<SourcePrimType_>> {
511  public:
512  inline static EulerAnglesZyx<DestPrimType_> convert(const RotationMatrix<SourcePrimType_>& R) {
514  }
515 };
516 
517 template<typename DestPrimType_, typename SourcePrimType_>
518 class ConversionTraits<EulerAnglesZyx<DestPrimType_>, RotationVector<SourcePrimType_>> {
519  public:
520  inline static EulerAnglesZyx<DestPrimType_> convert(const RotationVector<SourcePrimType_>& rotationVector) {
522  }
523 };
524 
525 template<typename DestPrimType_, typename SourcePrimType_>
526 class ConversionTraits<EulerAnglesZyx<DestPrimType_>, RotationQuaternion<SourcePrimType_>> {
527  public:
529  const Eigen::Matrix<DestPrimType_, 3, 1> vec = (q.toImplementation().toRotationMatrix().eulerAngles(2, 1, 0)).template cast<DestPrimType_>();
530  return EulerAnglesZyx<DestPrimType_>(vec(0), vec(1), vec(2));
531  }
532 };
533 
534 template<typename DestPrimType_, typename SourcePrimType_>
535 class ConversionTraits<EulerAnglesZyx<DestPrimType_>, EulerAnglesXyz<SourcePrimType_>> {
536  public:
537  inline static EulerAnglesZyx<DestPrimType_> convert(const EulerAnglesXyz<SourcePrimType_>& xyz) {
539  }
540 };
541 
542 template<typename DestPrimType_, typename SourcePrimType_>
543 class ConversionTraits<EulerAnglesZyx<DestPrimType_>, EulerAnglesZyx<SourcePrimType_>> {
544  public:
545  inline static EulerAnglesZyx<DestPrimType_> convert(const EulerAnglesZyx<SourcePrimType_>& zyx) {
546  return EulerAnglesZyx<DestPrimType_>(zyx.toImplementation().template cast<DestPrimType_>());
547  }
548 };
549 
550 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
551  * Multiplication Traits
552  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
553 
554 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
555  * Rotation Traits
556  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
557 
558 
559 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
560  * Comparison Traits
561  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
562 
563 
564 } // namespace internal
565 } // namespace kindr
566 
567 
Base Vector
Euler angles as 3x1-matrix.
Scalar pitch() const
Gets pitch (Y&#39;) angle.
PrimType_ Scalar
The primitive type. Float/Double.
Base zyx_
vector of Euler angles [yaw; pitch; roll]
EulerAnglesYpr< float > EulerAnglesYprF
Active Euler angles rotation (Z,Y&#39;,X&#39;&#39; / yaw,pitch,roll) with float primitive type.
EulerAnglesZyx & operator()(const RotationBase< OtherDerived_ > &other)
Parenthesis operator to convert from another rotation.
void setX(Scalar x)
Writing access to roll (X&#39;&#39;) angle.
EulerAnglesZyx 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.
EulerAnglesZyx< PrimType_ > & derived()
Gets the derived rotation. (only for advanced users)
EulerAnglesZyx & operator=(const RotationBase< OtherDerived_ > &other)
Assignment operator using another rotation.
Eigen::Matrix< PrimType_, 3, 3 > getMappingFromDiffToLocalAngularVelocity() const
Scalar y() const
Reading access to pitch (Y&#39;) angle.
EulerAnglesZyx inverted() const
Returns the inverse of the rotation.
friend std::ostream & operator<<(std::ostream &out, const EulerAnglesZyx &zyx)
Used for printing the object with std::cout.
EulerAnglesZyx< double > EulerAnglesZyxD
Active Euler angles rotation (Z,Y&#39;,X&#39;&#39; / yaw,pitch,roll) with double primitive type.
Base & toImplementation()
Cast to the implementation type.
Implementation of Euler angles (Z-Y&#39;-X&#39;&#39; / yaw-pitch-roll) rotation based on Eigen::Matrix<Scalar, 3, 1>
Representation of a generic rotationThis class defines the generic interface for a rotation...
Eigen::Matrix< PrimType_, 3, 1 > Base
The base type.
Scalar x() const
Reading access to roll (X&#39;&#39;) angle.
EulerAnglesZyx< float > EulerAnglesZyxPF
Passive Euler angles rotation (Z,Y&#39;,X&#39;&#39; / yaw,pitch,roll) with float primitive type.
EulerAnglesZyx & setIdentity()
Sets the rotation to identity.
Base Implementation
The implementation type. The implementation type is always an Eigen object.
T floatingPointModulo(T x, T y)
Floating-point modulo.
Definition: common.hpp:46
EulerAnglesYpr< double > EulerAnglesYprD
Active Euler angles rotation (Z,Y&#39;,X&#39;&#39; / yaw,pitch,roll) with double primitive type.
void setPitch(Scalar pitch)
Sets pitch (Y&#39;) angle.
Implementation & toImplementation()
Cast to the implementation type.
EulerAnglesZyx(const Base &other)
Constructor using Eigen::Matrix.
Implementation of a rotation vector based on Eigen::Matrix<Scalar, 3, 1>
Definition: Rotation.hpp:43
void setZ(Scalar z)
Writing access to yaw (Z) angle.
EulerAnglesYpr< double > EulerAnglesYprPD
Passive Euler angles rotation (Z,Y&#39;,X&#39;&#39; / yaw,pitch,roll) with double primitive type.
EulerAnglesZyx< double > EulerAnglesZyxPD
Passive Euler angles rotation (Z,Y&#39;,X&#39;&#39; / yaw,pitch,roll) with double primitive type.
void setRoll(Scalar roll)
Sets roll (X&#39;&#39;) angle.
EulerAnglesZyx()
Default constructor using identity rotation.
Implementation of matrix rotation based on Eigen::Matrix<Scalar, 3, 3>
Definition: Rotation.hpp:49
EulerAnglesYpr< float > EulerAnglesYprPF
Passive Euler angles rotation (Z,Y&#39;,X&#39;&#39; / yaw,pitch,roll) with float primitive type.
EulerAnglesZyx & setUnique()
Modifies the Euler angles rotation such that the angles lie in [-pi,pi),[-pi/2,pi/2),[-pi,pi).
EulerAnglesZyx(const RotationBase< OtherDerived_ > &other)
Constructor using another rotation.
Scalar yaw() const
Gets yaw (Z) angle.
Implementation of Euler angles (X-Y&#39;-Z&#39;&#39; / roll-pitch-yaw) rotation based on Eigen::Matrix<Scalar,3,1>
RotationQuaternion inverted() const
Returns the inverse of the rotation.
Eigen::Matrix< PrimType_, 3, 3 > getMappingFromLocalAngularVelocityToDiff() const
void setYaw(Scalar yaw)
Sets yaw (Z) angle.
Scalar roll() const
Gets roll (X&#39;&#39;) angle.
const Vector vector() const
Returns the Euler angles in a vector.
Implementation of an angle axis rotation based on Eigen::AngleAxis.
Definition: AngleAxis.hpp:54
EulerAnglesZyx< float > EulerAnglesZyxF
Active Euler angles rotation (Z,Y&#39;,X&#39;&#39; / yaw,pitch,roll) with float primitive type.
Scalar z() const
Reading access to yaw (Z) angle.
Implementation of quaternion rotation based on Eigen::Quaternion.
Definition: Rotation.hpp:46
EulerAnglesZyx & invert()
Inverts the rotation.
void setY(Scalar y)
Writing access to pitch (Y&#39;) angle.
const Base & toImplementation() const
Cast to the implementation type.
EulerAnglesZyx(Scalar yaw, Scalar pitch, Scalar roll)
Constructor using three scalars.
#define KINDR_ASSERT_TRUE(exceptionType, condition, message)