Kindr
Kinematics and Dynamics for Robotics
EulerAnglesZyxDiff.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 <Eigen/Core>
32 
34 #include "kindr/common/common.hpp"
39 
40 namespace kindr {
41 
51 template<typename PrimType_>
52 class EulerAnglesZyxDiff : public RotationDiffBase<EulerAnglesZyxDiff<PrimType_>> {
53  private:
56  typedef Eigen::Matrix<PrimType_, 3, 1> Base;
57 
60  Base zyxDiff_;
61 
62  public:
66  typedef Base Implementation;
67 
71  typedef PrimType_ Scalar;
72 
76  : zyxDiff_(Base::Zero()) {
77  }
78 
84  EulerAnglesZyxDiff(Scalar yaw, Scalar pitch, Scalar roll)
85  : zyxDiff_(yaw,pitch,roll) {
86  }
87 
93  template<typename RotationDerived_, typename OtherDerived_>
95  : zyxDiff_(internal::RotationDiffConversionTraits<EulerAnglesZyxDiff, OtherDerived_, RotationDerived_>::convert(rotation.derived(), other.derived()).toImplementation()){
96  }
97 
102  template<typename OtherDerived_, typename RotationDerived_>
103  OtherDerived_ cast(const RotationBase<RotationDerived_>& rotation) const {
104  return internal::RotationDiffConversionTraits<OtherDerived_, EulerAnglesZyxDiff, RotationDerived_>::convert(rotation.derived(), *this);
105  }
106 
110  explicit EulerAnglesZyxDiff(const Base& other)
111  : zyxDiff_(other) {
112  }
113 
117  inline Base& toImplementation() {
118  return static_cast<Base&>(zyxDiff_);
119  }
120 
124  inline const Base& toImplementation() const {
125  return static_cast<const Base&>(zyxDiff_);
126  }
127 
128 
129  inline Base& vector() {
130  return toImplementation();
131  }
132 
133  inline const Base& vector() const {
134  return toImplementation();
135  }
136 
137 
141  inline Scalar yaw() const {
142  return toImplementation()(0);
143  }
144 
148  inline Scalar pitch() const {
149  return toImplementation()(1);
150  }
151 
155  inline Scalar roll() const {
156  return toImplementation()(2);
157  }
158 
162  inline Scalar& yaw() {
163  return toImplementation()(0);
164  }
165 
169  inline Scalar& pitch() {
170  return toImplementation()(1);
171  }
172 
176  inline Scalar& roll() {
177  return toImplementation()(2);
178  }
179 
183  inline Scalar z() const {
184  return toImplementation()(0);
185  }
186 
190  inline Scalar y() const {
191  return toImplementation()(1);
192  }
193 
197  inline Scalar x() const {
198  return toImplementation()(2);
199  }
200 
204  inline Scalar& z() {
205  return toImplementation()(0);
206  }
207 
211  inline Scalar& y() {
212  return toImplementation()(1);
213  }
214 
218  inline Scalar& x() {
219  return toImplementation()(2);
220  }
221 
226  this->toImplementation().setZero();
227  return *this;
228  }
229 
232  using RotationDiffBase<EulerAnglesZyxDiff<PrimType_>>::operator+; // otherwise ambiguous EulerAnglesDiffBase and Eigen
233 
236  using RotationDiffBase<EulerAnglesZyxDiff<PrimType_>>::operator-; // otherwise ambiguous EulerAnglesDiffBase and Eigen
237 
238 
244  friend std::ostream& operator << (std::ostream& out, const EulerAnglesZyxDiff& diff) {
245  out << diff.toImplementation().transpose();
246  return out;
247  }
248 };
249 
258 
259 
260 
261 namespace internal {
262 
263 
264 template<typename PrimType_>
265 class RotationDiffConversionTraits<EulerAnglesZyxDiff<PrimType_>, LocalAngularVelocity<PrimType_>, EulerAnglesZyx<PrimType_>> {
266  public:
267  inline static EulerAnglesZyxDiff<PrimType_> convert(const EulerAnglesZyx<PrimType_>& eulerAngles, const LocalAngularVelocity<PrimType_>& angularVelocity) {
268  return EulerAnglesZyxDiff<PrimType_>(eulerAngles.getMappingFromLocalAngularVelocityToDiff()*angularVelocity.vector());
269  }
270 };
271 
272 
273 } // namespace internal
274 } // namespace kindr
275 
EulerAnglesZyxDiff< float > EulerAnglesZyxDiffF
Time derivative of Euler angles with z-y-x convention and primitive type float.
Scalar y() const
Reading access to time derivative of pitch (Y&#39;) angle.
Derived_ & derived()
Gets the derived rotation. (only for advanced users)
Scalar & yaw()
Writing access to time derivative of yaw (Z) angle.
Scalar yaw() const
Reading access to time derivative of yaw (Z) angle.
EulerAnglesZyxDiff(const Base &other)
Constructor using Eigen::Matrix.
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...
EulerAnglesZyxDiff< double > EulerAnglesZyxDiffPD
Time derivative of Euler angles with z-y-x convention and primitive type double.
Scalar & x()
Writing access to time derivative of roll (X&#39;&#39;) angle.
Scalar & y()
Writing access to time derivative of pitch (Y&#39;) angle.
PrimType_ Scalar
The primitive type. Float/Double.
Implementation of time derivatives of Euler angles (Z,Y&#39;,X&#39;&#39; / yaw,pitch,roll) based on Eigen::Matrix...
const Base & vector() const
EulerAnglesZyxDiff< double > EulerAnglesZyxDiffD
Time derivative of Euler angles with z-y-x convention and primitive type double.
Eigen::Matrix< PrimType_, 3, 1 > Base
The base type.
EulerAnglesZyxDiff(const RotationBase< RotationDerived_ > &rotation, const RotationDiffBase< OtherDerived_ > &other)
Constructor using a time derivative with a different parameterization.
const EulerAnglesZyxDiff< PrimType_ > & derived() const
Gets the derived time derivative of a rotation. (only for advanced users)
Base Implementation
The implementation type. The implementation type is always an Eigen object.
EulerAnglesZyxDiff & setZero()
Sets all time derivatives to zero.
Scalar z() const
Reading access to time derivative of yaw (Z) angle.
EulerAnglesZyxDiff< float > EulerAnglesZyxDiffPF
Time derivative of Euler angles with z-y-x convention and primitive type float.
Scalar & roll()
Writing access to time derivative of roll (X&#39;&#39;) angle.
Base zyxDiff_
data container [yaw; pitch; roll]
OtherDerived_ cast(const RotationBase< RotationDerived_ > &rotation) const
Cast to another representation of the time derivative of a rotation.
Scalar pitch() const
Reading access to time derivative of pitch (Y&#39;) angle.
Eigen::Matrix< PrimType_, 3, 3 > getMappingFromLocalAngularVelocityToDiff() const
friend std::ostream & operator<<(std::ostream &out, const EulerAnglesZyxDiff &diff)
Used for printing the object with std::cout.
Interface for an angular velocity.
Scalar & pitch()
Writing access to time derivative of pitch (Y&#39;) angle.
Scalar roll() const
Reading access to time derivative of roll (X&#39;&#39;) angle.
EulerAnglesZyxDiff(Scalar yaw, Scalar pitch, Scalar roll)
Constructor using three scalars.
EulerAnglesZyxDiff()
Default constructor.
Base & toImplementation()
Cast to the implementation type.
Scalar & z()
Writing access to time derivative of yaw (Z) angle.
Scalar x() const
Reading access to time derivative of roll (X&#39;&#39;) angle.
const Base & toImplementation() const
Cast to the implementation type.