Kindr
Kinematics and Dynamics for Robotics
RotationMatrixDiff.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 
50 template<typename PrimType_>
51 class RotationMatrixDiff : public RotationDiffBase<RotationMatrixDiff<PrimType_>>, private Eigen::Matrix<PrimType_, 3, 3> {
52  private:
55  typedef Eigen::Matrix<PrimType_, 3, 3> Base;
56 
57  public:
61  typedef Base Implementation;
62 
63  typedef Base Matrix3x3;
64 
65 
69  typedef PrimType_ Scalar;
70 
72  : Base(Base::Zero()) {
73  }
74 
78  explicit RotationMatrixDiff(const Base& other) // explicit on purpose
79  : Base(other) {
80  }
81 
93  RotationMatrixDiff(Scalar r11, Scalar r12, Scalar r13,
94  Scalar r21, Scalar r22, Scalar r23,
95  Scalar r31, Scalar r32, Scalar r33) {
96  *this << r11,r12,r13,r21,r22,r23,r31,r32,r33;
97  }
98 
104  template<typename RotationDerived_, typename OtherDerived_>
106  : Base(internal::RotationDiffConversionTraits<RotationMatrixDiff, OtherDerived_, RotationDerived_>::convert(rotation.derived(), other.derived()).toImplementation()){
107  }
108 
109 
114  template<typename OtherDerived_, typename RotationDerived_>
115  OtherDerived_ cast(const RotationBase<RotationDerived_>& rotation) const {
116  return internal::RotationDiffConversionTraits<OtherDerived_, RotationMatrixDiff, RotationDerived_>::convert(rotation.derived(), *this);
117  }
118 
122  inline Implementation& toImplementation() {
123  return static_cast<Implementation&>(*this);
124  }
125 
129  inline const Implementation& toImplementation() const {
130  return static_cast<const Implementation&>(*this);
131  }
132 
136  inline const Matrix3x3& matrix() const {
137  return this->toImplementation();
138  }
139 
143  inline Matrix3x3& matrix() {
144  return this->toImplementation();
145  }
146 
151  this->toImplementation().setZero();
152  return *this;
153  }
154 
158  friend std::ostream& operator << (std::ostream& out, const RotationMatrixDiff& diff) {
159  out << diff.toImplementation();
160  return out;
161  }
162 };
163 
164 
173 
174 
175 namespace internal {
176 
177 template<typename PrimType_>
178 class RotationDiffConversionTraits<RotationMatrixDiff<PrimType_>, LocalAngularVelocity<PrimType_>, RotationMatrix<PrimType_>> {
179  public:
180  inline static RotationMatrixDiff<PrimType_> convert(const RotationMatrix<PrimType_>& rotationMatrix, const LocalAngularVelocity<PrimType_>& angularVelocity) {
181  return RotationMatrixDiff<PrimType_>(rotationMatrix.matrix()*getSkewMatrixFromVector(angularVelocity.vector()));
182  }
183 };
184 
185 template<typename PrimType_>
186 class RotationDiffConversionTraits<RotationMatrixDiff<PrimType_>, GlobalAngularVelocity<PrimType_>, RotationMatrix<PrimType_>> {
187  public:
188  inline static RotationMatrixDiff<PrimType_> convert(const RotationMatrix<PrimType_>& rotationMatrix, const GlobalAngularVelocity<PrimType_>& angularVelocity) {
189  return RotationMatrixDiff<PrimType_>(getSkewMatrixFromVector(angularVelocity.vector())*rotationMatrix.matrix());
190  }
191 };
192 
193 } // namespace internal
194 } // namespace kindr
RotationMatrixDiff(const RotationBase< RotationDerived_ > &rotation, const RotationDiffBase< OtherDerived_ > &other)
Constructor using a time derivative with a different parameterization.
OtherDerived_ cast(const RotationBase< RotationDerived_ > &rotation) const
Cast to another representation of the time derivative of a rotation.
Derived_ & derived()
Gets the derived rotation. (only for advanced users)
Implementation matrix() const
Reading access to the rotation matrix.
Matrix3x3 & matrix()
Writing access to the time derivative of the rotation matrix.
RotationMatrixDiff< float > RotationMatrixDiffPF
Time derivative of a rotation quaternion with primitive type float.
Implementation & toImplementation()
Cast to the implementation type.
RotationMatrixDiff< double > RotationMatrixDiffPD
Time derivative of a rotation quaternion with primitive type double.
Representation of a generic rotationThis class defines the generic interface for a rotation...
RotationMatrixDiff< float > RotationMatrixDiffF
Time derivative of a rotation quaternion with primitive type float.
RotationMatrixDiff< double > RotationMatrixDiffD
Time derivative of a rotation quaternion with primitive type double.
RotationMatrixDiff(const Base &other)
Constructor using Eigen::Matrix<Scalar, 3, 3>.
Time derivative of a rotation matrix.
Implementation of matrix rotation based on Eigen::Matrix<Scalar, 3, 3>
Definition: Rotation.hpp:49
const Matrix3x3 & matrix() const
Reading access to the time derivative of the rotation matrix.
RotationMatrixDiff(Scalar r11, Scalar r12, Scalar r13, Scalar r21, Scalar r22, Scalar r23, Scalar r31, Scalar r32, Scalar r33)
Constructor using nine scalars.
Base Implementation
The implementation type. The implementation type is always an Eigen object.
const RotationMatrixDiff< PrimType_ > & derived() const
Gets the derived time derivative of a rotation. (only for advanced users)
Interface for an angular velocity.
RotationMatrixDiff & setZero()
Sets all time derivatives to zero.
friend std::ostream & operator<<(std::ostream &out, const RotationMatrixDiff &diff)
Used for printing the object with std::cout.
PrimType_ Scalar
The primitive type. Float/Double.
static Eigen::Matrix< PrimType_, 3, 3 > getSkewMatrixFromVector(const Eigen::Matrix< PrimType_, 3, 1 > &vec)
Gets a skew-symmetric matrix from a (column) vector.
Eigen::Matrix< PrimType_, 3, 3 > Base
The base type.
const Implementation & toImplementation() const
Cast to the implementation type.