Kindr
Kinematics and Dynamics for Robotics
RotationQuaternionDiff.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"
39 
40 namespace kindr {
41 
50 template<typename PrimType_>
51 class RotationQuaternionDiff : public RotationDiffBase<RotationQuaternionDiff<PrimType_>>, private Quaternion<PrimType_> {
52  private:
56 
57  public:
62 
66  typedef PrimType_ Scalar;
67 
69  typedef Eigen::Matrix<PrimType_, 3, 1> Imaginary;
70 
72  typedef Eigen::Matrix<PrimType_,4,1> Vector4;
73 
78  : Base() {
79  }
80 
81  explicit RotationQuaternionDiff(const Base& other) // explicit on purpose
82  : Base(other) {
83  }
84 
91  RotationQuaternionDiff(Scalar w, Scalar x, Scalar y, Scalar z)
92  : Base(w,x,y,z) {
93  }
94 
95  RotationQuaternionDiff(const Vector4 & vec)
96  : Base(vec(0),vec(1),vec(2),vec(3)) {
97  }
98 
99  RotationQuaternionDiff(const Scalar& real, const Imaginary& imag)
100  : Base(real, imag) {
101  }
102 
108  template<typename RotationDerived_, typename OtherDerived_>
110  : Base(internal::RotationDiffConversionTraits<RotationQuaternionDiff, OtherDerived_, RotationDerived_>::convert(rotation.derived(), other.derived()).toQuaternion()){
111  }
112 
117  template<typename OtherDerived_, typename RotationDerived_>
118  OtherDerived_ cast(const RotationBase<RotationDerived_>& rotation) const {
119  return internal::RotationDiffConversionTraits<OtherDerived_, RotationQuaternionDiff, RotationDerived_>::convert(rotation.derived(), *this);
120  }
121 
122 
126  inline Implementation& toImplementation() {
127  return this->toQuaternion().toImplementation();
128  }
129 
133  inline const Implementation& toImplementation() const {
134  return this->toQuaternion().toImplementation();
135  }
136 
140  Base& toQuaternion() {
141  return static_cast<Base&>(*this);
142  }
143 
144 
148  const Base& toQuaternion() const {
149  return static_cast<const Base&>(*this);
150  }
151 
152 
153  using Base::w;
154  using Base::x;
155  using Base::y;
156  using Base::z;
157 
158  using Base::real;
159  using Base::imaginary;
160  using Base::vector;
161 
166  Base::setZero();
167  return *this;
168  }
169 
173  friend std::ostream& operator << (std::ostream& out, const RotationQuaternionDiff& diff) {
174  out << diff.w() << " " << diff.x() << " " << diff.y() << " " << diff.z();
175  return out;
176  }
177 };
178 
179 
188 
189 
190 namespace internal {
191 
192 template<typename PrimType_>
193 class RotationDiffConversionTraits<RotationQuaternionDiff<PrimType_>, LocalAngularVelocity<PrimType_>, RotationQuaternion<PrimType_>> {
194  public:
195  inline static RotationQuaternionDiff<PrimType_> convert(const RotationQuaternion<PrimType_>& rquat, const LocalAngularVelocity<PrimType_>& angularVelocity) {
196  return RotationQuaternionDiff<PrimType_>(Quaternion<PrimType_>(0.5*(rquat.getLocalQuaternionDiffMatrix().transpose()*angularVelocity.vector())));
197  }
198 };
199 
200 template<typename PrimType_>
201 class RotationDiffConversionTraits<RotationQuaternionDiff<PrimType_>, GlobalAngularVelocity<PrimType_>, RotationQuaternion<PrimType_>> {
202  public:
203  inline static RotationQuaternionDiff<PrimType_> convert(const RotationQuaternion<PrimType_>& rquat, const GlobalAngularVelocity<PrimType_>& angularVelocity) {
204  return RotationQuaternionDiff<PrimType_>(Quaternion<PrimType_>(0.5*(rquat.getGlobalQuaternionDiffMatrix().transpose()*angularVelocity.vector())));
205  }
206 };
207 
208 
209 } // namespace internal
210 } // namespace kindr
211 
PrimType_ Scalar
The primitive type. Float/Double.
Implementation of a Quaternion based on Eigen::Quaternion.
Definition: Quaternion.hpp:55
RotationQuaternionDiff(Scalar w, Scalar x, Scalar y, Scalar z)
Constructor using four scalars.
Derived_ & derived()
Gets the derived rotation. (only for advanced users)
Imaginary imaginary() const
Definition: Quaternion.hpp:212
Implementation & toImplementation()
Definition: Quaternion.hpp:166
Scalar real() const
Definition: Quaternion.hpp:208
Scalar x() const
Definition: Quaternion.hpp:180
Representation of a generic rotationThis class defines the generic interface for a rotation...
RotationQuaternionDiff< double > RotationQuaternionDiffD
Time derivative of a rotation quaternion with primitive type double.
friend std::ostream & operator<<(std::ostream &out, const RotationQuaternionDiff &diff)
Used for printing the object with std::cout.
Eigen::Matrix< PrimType_, 3, 4 > getGlobalQuaternionDiffMatrix() const
Returns the global quaternion diff matrix H: GlobalAngularVelocity = 2*H*qdiff, qdiff = 0...
Eigen::Matrix< PrimType_, 4, 1 > Vector4
quaternion as 4x1 matrix: [w; x; y; z]
Base::Implementation Implementation
The implementation type. The implementation type is always an Eigen object.
Scalar w() const
Definition: Quaternion.hpp:176
RotationQuaternionDiff & setZero()
Sets all time derivatives to zero.
Scalar y() const
Definition: Quaternion.hpp:184
Implementation & toImplementation()
Cast to the implementation type.
Quaternion< PrimType_ > Base
The base type.
RotationQuaternionDiff()
Default constructor sets all derivatives to zero.
Vector4 vector() const
Definition: Quaternion.hpp:216
RotationQuaternionDiff(const RotationBase< RotationDerived_ > &rotation, const RotationDiffBase< OtherDerived_ > &other)
Constructor using a time derivative with a different parameterization.
const Base & toQuaternion() const
Cast to the base type.
const RotationQuaternionDiff< PrimType_ > & derived() const
Gets the derived time derivative of a rotation. (only for advanced users)
OtherDerived_ cast(const RotationBase< RotationDerived_ > &rotation) const
Cast to another representation of the time derivative of a rotation.
Eigen::Matrix< PrimType_, 3, 4 > getLocalQuaternionDiffMatrix() const
Returns the local quaternion diff matrix HBar: LocalAngularVelocity = 2*HBar*qdiff, qdiff = 0.5*HBar^T*LocalAngularVelocity.
const Implementation & toImplementation() const
Cast to the implementation type.
RotationQuaternionDiff< float > RotationQuaternionDiffPF
Time derivative of a rotation quaternion with primitive type float.
Quaternion & setZero()
Definition: Quaternion.hpp:235
RotationQuaternionDiff< double > RotationQuaternionDiffPD
Time derivative of a rotation quaternion with primitive type double.
RotationQuaternionDiff(const Scalar &real, const Imaginary &imag)
Eigen::Matrix< PrimType_, 3, 1 > Imaginary
the imaginary type, i.e., Eigen::Quaternion<>
Interface for an angular velocity.
Scalar z() const
Definition: Quaternion.hpp:188
Base Implementation
the implementation type, i.e., Eigen::Quaternion<>
Definition: Quaternion.hpp:60
Base & toQuaternion()
Cast to the base type.
Implementation of quaternion rotation based on Eigen::Quaternion.
Definition: Rotation.hpp:46
RotationQuaternionDiff< float > RotationQuaternionDiffF
Time derivative of a rotation quaternion with primitive type float.
Time derivative of a rotation quaternion.