Kindr
Kinematics and Dynamics for Robotics
EulerAnglesXyzDiff.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 #pragma once
29 
30 #include <Eigen/Core>
31 
33 #include "kindr/common/common.hpp"
38 
39 namespace kindr {
40 
50 template<typename PrimType_>
51 class EulerAnglesXyzDiff : public RotationDiffBase<EulerAnglesXyzDiff<PrimType_>> {
52  private:
55  typedef Eigen::Matrix<PrimType_, 3, 1> Base;
56 
59  Base xyzDiff_;
60  public:
64  typedef Base Implementation;
68  typedef PrimType_ Scalar;
69 
73  : xyzDiff_(Base::Zero()) {
74  }
75 
81  EulerAnglesXyzDiff(Scalar roll, Scalar pitch, Scalar yaw)
82  : xyzDiff_(roll,pitch,yaw) {
83  }
84 
88  explicit EulerAnglesXyzDiff(const Base& other)
89  : xyzDiff_(other) {
90  }
91 
92 
98  template<typename RotationDerived_, typename OtherDerived_>
100  : xyzDiff_(internal::RotationDiffConversionTraits<EulerAnglesXyzDiff, OtherDerived_, RotationDerived_>::convert(rotation.derived(), other.derived()).toImplementation()){
101  }
102 
107  template<typename OtherDerived_, typename RotationDerived_>
108  OtherDerived_ cast(const RotationBase<RotationDerived_>& rotation) const {
109  return internal::RotationDiffConversionTraits<OtherDerived_, EulerAnglesXyzDiff, RotationDerived_>::convert(rotation.derived(), *this);
110  }
111 
112 
116  inline Base& toImplementation() {
117  return static_cast<Base&>(xyzDiff_);
118  }
119 
123  inline const Base& toImplementation() const {
124  return static_cast<const Base&>(xyzDiff_);
125  }
126 
127  inline Base& vector() {
128  return toImplementation();
129  }
130 
131  inline const Base& vector() const {
132  return toImplementation();
133  }
134 
135 
139  inline Scalar roll() const {
140  return toImplementation()(0);
141  }
142 
146  inline Scalar pitch() const {
147  return toImplementation()(1);
148  }
149 
153  inline Scalar yaw() const {
154  return toImplementation()(2);
155  }
156 
160  inline Scalar& roll() {
161  return toImplementation()(0);
162  }
163 
167  inline Scalar& pitch() {
168  return toImplementation()(1);
169  }
170 
174  inline Scalar& yaw() {
175  return toImplementation()(2);
176  }
177 
181  inline Scalar x() const {
182  return toImplementation()(0);
183  }
184 
188  inline Scalar y() const {
189  return toImplementation()(1);
190  }
191 
195  inline Scalar z() const {
196  return toImplementation()(2);
197  }
198 
202  inline Scalar& x() {
203  return toImplementation()(0);
204  }
205 
209  inline Scalar& y() {
210  return toImplementation()(1);
211  }
212 
216  inline Scalar& z() {
217  return toImplementation()(2);
218  }
219 
224  this->toImplementation().setZero();
225  return *this;
226  }
227 
228 
229  using RotationDiffBase<EulerAnglesXyzDiff<PrimType_>>::operator+; // otherwise ambiguous RotationDiffBase and Eigen
230 
231  using RotationDiffBase<EulerAnglesXyzDiff<PrimType_>>::operator-; // otherwise ambiguous RotationDiffBase and Eigen
232 
233  Eigen::Matrix<PrimType_, 3, 3> getMappingFromLocalAngularVelocityToSecondDiff(const EulerAnglesXyz<PrimType_>& rotation) const {
234  using std::sin;
235  using std::cos;
236  Eigen::Matrix<PrimType_, 3, 3> mat = Eigen::Matrix<PrimType_, 3, 3>::Zero();
237  const PrimType_ x = rotation.x();
238  const PrimType_ y = rotation.y();
239  const PrimType_ z = rotation.z();
240  const PrimType_ dx = this->x();
241  const PrimType_ dy = this->y();
242  const PrimType_ dz = this->z();
243  const PrimType_ t2 = cos(y);
244  KINDR_ASSERT_TRUE(std::runtime_error, t2 != PrimType_(0), "Gimbal lock: cos(y) is zero!");
245  const PrimType_ t3 = 1.0/t2;
246  const PrimType_ t4 = cos(z);
247  const PrimType_ t5 = 1.0/(t2*t2);
248  const PrimType_ t6 = sin(y);
249  const PrimType_ t7 = sin(z);
250  const PrimType_ t8 = t6*t6;
251  mat(0,0) = -dz*t3*t7+dy*t4*t5*t6;
252  mat(0,1) = -dz*t3*t4-dy*t5*t6*t7;
253  mat(1,0) = dz*t4;
254  mat(1,1) = -dz*t7;
255  mat(2,0) = -dy*(t4+t4*t5*t8)+dz*t3*t6*t7;
256  mat(2,1) = dy*(t7+t5*t7*t8)+dz*t3*t4*t6;
257 
258  return mat;
259  }
260 
261  Eigen::Matrix<PrimType_, 3, 3> getMappingFromSecondDiffToLocalAngularVelocity(const EulerAnglesXyz<PrimType_>& rotation) const {
262  using std::sin;
263  using std::cos;
264  Eigen::Matrix<PrimType_, 3, 3> mat = Eigen::Matrix<PrimType_, 3, 3>::Zero();
265  const PrimType_ x = rotation.x();
266  const PrimType_ y = rotation.y();
267  const PrimType_ z = rotation.z();
268  const PrimType_ dx = this->x();
269  const PrimType_ dy = this->y();
270  const PrimType_ dz = this->z();
271  const PrimType_ t2 = cos(z);
272  const PrimType_ t3 = cos(y);
273  const PrimType_ t4 = sin(y);
274  const PrimType_ t5 = sin(z);
275  mat(0,0) = -dy*t2*t4-dz*t3*t5;
276  mat(0,1) = dz*t2;
277  mat(1,0) = dy*t4*t5-dz*t2*t3;
278  mat(1,1) = -dz*t5;
279  mat(2,0) = dy*t3;
280  return mat;
281  }
282 
283  Eigen::Matrix<PrimType_, 3, 3> getMappingFromGlobalAngularVelocityToSecondDiff(const EulerAnglesXyz<PrimType_>& rotation) const {
284  using std::sin;
285  using std::cos;
286  Eigen::Matrix<PrimType_, 3, 3> mat = Eigen::Matrix<PrimType_, 3, 3>::Zero();
287  const PrimType_ x = rotation.x();
288  const PrimType_ y = rotation.y();
289  const PrimType_ z = rotation.z();
290  const PrimType_ dx = this->x();
291  const PrimType_ dy = this->y();
292  const PrimType_ dz = this->z();
293  const PrimType_ t2 = sin(x);
294  const PrimType_ t3 = sin(y);
295  const PrimType_ t4 = cos(y);
296  const PrimType_ t5 = cos(x);
297  KINDR_ASSERT_TRUE(std::runtime_error, t4 != PrimType_(0), "Gimbal lock: cos(y) is zero!");
298  const PrimType_ t6 = 1.0/(t4*t4);
299  const PrimType_ t7 = t3*t3;
300  const PrimType_ t8 = 1.0/t4;
301  mat(0,1) = dy*(t2+t2*t6*t7)+dx*t3*t5*t8;
302  mat(0,2) = -dy*(t5+t5*t6*t7)+dx*t2*t3*t8;
303  mat(1,1) = -dx*t2;
304  mat(1,2) = dx*t5;
305  mat(2,1) = -dx*t5*t8-dy*t2*t3*t6;
306  mat(2,2) = -dx*t2*t8+dy*t3*t5*t6;
307 
308  return mat;
309  }
310 
311  Eigen::Matrix<PrimType_, 3, 3> getMappingFromSecondDiffToGlobalAngularVelocity(const EulerAnglesXyz<PrimType_>& rotation) const {
312  using std::sin;
313  using std::cos;
314  Eigen::Matrix<PrimType_, 3, 3> mat = Eigen::Matrix<PrimType_, 3, 3>::Zero();
315  const PrimType_ x = rotation.x();
316  const PrimType_ y = rotation.y();
317  const PrimType_ z = rotation.z();
318  const PrimType_ dx = this->x();
319  const PrimType_ dy = this->y();
320  const PrimType_ dz = this->z();
321  const PrimType_ t2 = cos(y);
322  const PrimType_ t3 = sin(x);
323  const PrimType_ t4 = cos(x);
324  const PrimType_ t5 = sin(y);
325  mat(0, 2) = dy*t2;
326  mat(1, 1) = -dx*t3;
327  mat(1, 2) = -dx*t2*t4+dy*t3*t5;
328  mat(2, 1) = dx*t4;
329  mat(2, 2) = -dx*t2*t3-dy*t4*t5;
330  return mat;
331  }
332 
338  friend std::ostream& operator << (std::ostream& out, const EulerAnglesXyzDiff& diff) {
339  out << diff.toImplementation().transpose();
340  return out;
341  }
342 };
343 
352 
353 
354 
355 namespace internal {
356 
357 template<typename PrimType_>
358 class RotationDiffConversionTraits<EulerAnglesXyzDiff<PrimType_>, LocalAngularVelocity<PrimType_>, EulerAnglesXyz<PrimType_>> {
359  public:
360  inline static EulerAnglesXyzDiff<PrimType_> convert(const EulerAnglesXyz<PrimType_>& eulerAngles, const LocalAngularVelocity<PrimType_>& angularVelocity) {
361  return EulerAnglesXyzDiff<PrimType_>(eulerAngles.getMappingFromLocalAngularVelocityToDiff()*angularVelocity.vector());
362  }
363 };
364 
365 
366 } // namespace internal
367 } // namespace kindr
368 
369 
Derived_ & derived()
Gets the derived rotation. (only for advanced users)
const Base & toImplementation() const
Cast to the implementation type.
EulerAnglesXyzDiff(const RotationBase< RotationDerived_ > &rotation, const RotationDiffBase< OtherDerived_ > &other)
Constructor using a time derivative with a different parameterization.
Representation of a generic rotationThis class defines the generic interface for a rotation...
Eigen::Matrix< PrimType_, 3, 3 > getMappingFromSecondDiffToLocalAngularVelocity(const EulerAnglesXyz< PrimType_ > &rotation) const
EulerAnglesXyzDiff< float > EulerAnglesXyzDiffF
Time derivative of Euler angles with x-y-z convention and primitive type float.
Base Implementation
The implementation type. The implementation type is always an Eigen object.
Scalar yaw() const
Reading access to time derivative of yaw (Z&#39;&#39;) angle.
Scalar & pitch()
Writing access to time derivative of pitch (Y&#39;) angle.
Eigen::Matrix< PrimType_, 3, 1 > Base
The base type.
Scalar & yaw()
Writing access to time derivative of yaw (Z&#39;&#39;) angle.
Eigen::Matrix< PrimType_, 3, 3 > getMappingFromSecondDiffToGlobalAngularVelocity(const EulerAnglesXyz< PrimType_ > &rotation) const
Scalar y() const
Gets pitch (Y&#39;) angle.
EulerAnglesXyzDiff(const Base &other)
Constructor using Eigen::Matrix.
EulerAnglesXyzDiff< double > EulerAnglesXyzDiffPD
Time derivative of Euler angles with x-y-z convention and primitive type double.
EulerAnglesXyzDiff & setZero()
Sets all time derivatives to zero.
Scalar x() const
Reading access to time derivative of roll (X) angle.
const EulerAnglesXyzDiff< PrimType_ > & derived() const
Gets the derived time derivative of a rotation. (only for advanced users)
Scalar & roll()
Writing access to time derivative of roll (X) angle.
OtherDerived_ cast(const RotationBase< RotationDerived_ > &rotation) const
Cast to another representation of the time derivative of a rotation.
Eigen::Matrix< PrimType_, 3, 3 > getMappingFromLocalAngularVelocityToSecondDiff(const EulerAnglesXyz< PrimType_ > &rotation) const
EulerAnglesXyzDiff()
Default constructor.
friend std::ostream & operator<<(std::ostream &out, const EulerAnglesXyzDiff &diff)
Used for printing the object with std::cout.
Scalar z() const
Gets yaw (Z&#39;&#39;) angle.
EulerAnglesXyzDiff< double > EulerAnglesXyzDiffD
Time derivative of Euler angles with x-y-z convention and primitive type double.
Scalar pitch() const
Reading access to time derivative of pitch (Y&#39;) angle.
Base xyzDiff_
data container [roll; pitch; yaw]
Implementation of Euler angles (X-Y&#39;-Z&#39;&#39; / roll-pitch-yaw) rotation based on Eigen::Matrix<Scalar,3,1>
Scalar & y()
Writing access to time derivative of pitch (Y&#39;) angle.
Eigen::Matrix< PrimType_, 3, 3 > getMappingFromGlobalAngularVelocityToSecondDiff(const EulerAnglesXyz< PrimType_ > &rotation) const
PrimType_ Scalar
The primitive type. Float/Double.
Base & toImplementation()
Cast to the implementation type.
Implementation of time derivatives of Euler angles (X,Y&#39;,Z&#39;&#39; / roll,pitch,yaw) based on Eigen::Matrix...
Interface for an angular velocity.
EulerAnglesXyzDiff< float > EulerAnglesXyzDiffPF
Time derivative of Euler angles with x-y-z convention and primitive type float.
Scalar roll() const
Reading access to time derivative of roll (X) angle.
Scalar y() const
Reading access to time derivative of pitch (Y&#39;) angle.
Scalar & z()
Writing access to time derivative of yaw (Z&#39;&#39;) angle.
EulerAnglesXyzDiff(Scalar roll, Scalar pitch, Scalar yaw)
Constructor using three scalars.
Scalar z() const
Reading access to time derivative of yaw (Z&#39;&#39;) angle.
const Base & vector() const
Eigen::Matrix< PrimType_, 3, 3 > getMappingFromLocalAngularVelocityToDiff() const
Scalar x() const
Gets roll (X) angle.
#define KINDR_ASSERT_TRUE(exceptionType, condition, message)
Scalar & x()
Writing access to time derivative of roll (X) angle.