Kindr
Kinematics and Dynamics for Robotics
Rotation.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 "kindr/common/common.hpp"
36 
37 namespace kindr {
38 
39 template<typename PrimType_>
40 class AngleAxis;
41 
42 template<typename PrimType_>
44 
45 template<typename PrimType_>
47 
48 template<typename PrimType_>
50 
51 template<typename PrimType_>
52 class EulerAnglesZyx;
53 
54 template<typename PrimType_>
55 class EulerAnglesXyz;
56 
57 
58 namespace internal {
59 
60 template<typename PrimType_>
61 class get_scalar<Eigen::Matrix<PrimType_, 3, 1>> {
62  public:
63  typedef PrimType_ Scalar;
64 };
65 
66 
67 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
68  * Multiplication Traits
69  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
70 
73 template<typename Left_, typename Right_>
74 class MultiplicationTraits<RotationBase<Left_>, RotationBase<Right_> > {
75  public:
77  inline static Left_ mult(const RotationBase<Left_>& lhs, const RotationBase<Right_>& rhs) {
79  (RotationQuaternion<typename Left_::Scalar>(lhs.derived())).toImplementation() *
80  (RotationQuaternion<typename Right_::Scalar>(rhs.derived())).toImplementation()
81  ));
82 
83  }
84 };
85 
88 template<typename LeftAndRight_>
89 class MultiplicationTraits<RotationBase<LeftAndRight_>, RotationBase<LeftAndRight_> > {
90  public:
92  inline static LeftAndRight_ mult(const RotationBase<LeftAndRight_>& lhs, const RotationBase<LeftAndRight_>& rhs) {
96  ));
97 
98  }
99 };
100 
101 
102 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
103  * Rotation Traits
104  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
105 
106 template<typename Rotation_>
107 class RotationTraits<RotationBase<Rotation_>> {
108  public:
110  template<typename get_matrix3X<Rotation_>::IndexType Cols>
111  inline static typename get_matrix3X<Rotation_>::template Matrix3X<Cols> rotate(const RotationBase<Rotation_>& rotation, const typename internal::get_matrix3X<Rotation_>::template Matrix3X<Cols>& m){
112  return RotationMatrix<typename Rotation_::Scalar>(rotation.derived()).toImplementation()*m;
113  }
115  template<typename Vector_>
116  inline static Vector_ rotate(const RotationBase<Rotation_>& rotation, const Vector_& vector){
117  return static_cast<Vector_>(rotation.derived().rotate(vector.toImplementation()));
118  }
119 
120 };
121 
122 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
123  * Comparison Traits
124  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
125 
129 template<typename Left_, typename Right_>
130 class ComparisonTraits<RotationBase<Left_>, RotationBase<Right_>> {
131  public:
132  inline static bool isEqual(const RotationBase<Left_>& left, const RotationBase<Right_>& right) {
133  return left.derived().toImplementation() == RotationBase<Left_>(right).derived().toImplementation();
134  }
135 };
136 
137 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
138  * Map Traits
139  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
140 
141 template<typename Rotation_>
142 class MapTraits<RotationBase<Rotation_>> {
143  public:
144  inline static Rotation_ set_exponential_map(const typename internal::get_matrix3X<Rotation_>::template Matrix3X<1>& vector) {
145  typedef typename get_scalar<Rotation_>::Scalar Scalar;
146  return Rotation_(RotationVector<Scalar>(vector));
147  }
148 
149  inline static typename internal::get_matrix3X<Rotation_>::template Matrix3X<1> get_logarithmic_map(const Rotation_& rotation) {
150  typedef typename get_scalar<Rotation_>::Scalar Scalar;
151  return RotationVector<Scalar>(rotation).getUnique().toImplementation(); // unique is required
152  }
153 
154 };
155 
156 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
157  * Box Operation Traits
158  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
159 
160 template<typename Left_, typename Right_>
161 class BoxOperationTraits<RotationBase<Left_>, RotationBase<Right_>> {
162  public:
163  inline static typename internal::get_matrix3X<Left_>::template Matrix3X<1> box_minus(const RotationBase<Left_>& lhs, const RotationBase<Right_>& rhs) {
164  return (lhs.derived()*rhs.derived().inverted()).logarithmicMap();
165  }
166 
167  inline static Left_ box_plus(const RotationBase<Left_>& rotation, const typename internal::get_matrix3X<Left_>::template Matrix3X<1>& vector) {
168  return Left_(MapTraits<RotationBase<Left_>>::set_exponential_map(vector)*rotation.derived());
169  }
170 };
171 
172 
173 
174 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
175  * SetFromVectors Traits
176  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
177 
178 template<typename Rotation_>
179 class SetFromVectorsTraits<RotationBase<Rotation_>> {
180  public:
181  template<typename PrimType_>
182  inline static void setFromVectors(Rotation_& rot, const Eigen::Matrix<PrimType_, 3, 1>& v1, const Eigen::Matrix<PrimType_, 3, 1>& v2) {
183  KINDR_ASSERT_TRUE(std::runtime_error, v1.norm()*v2.norm() != static_cast<PrimType_>(0.0), "At least one vector has zero length.");
184 
185  Eigen::Quaternion<PrimType_> eigenQuat;
186  eigenQuat.setFromTwoVectors(v1, v2);
187  rot = kindr::RotationQuaternion<PrimType_>(eigenQuat);
188 //
189 // const PrimType_ angle = acos(v1.dot(v2)/temp);
190 // const PrimType_ tol = 1e-3;
191 //
192 // if(0 <= angle && angle < tol) {
193 // rot.setIdentity();
194 // } else if(M_PI - tol < angle && angle < M_PI + tol) {
195 // rot = AngleAxis<PrimType_>(angle, 1, 0, 0);
196 // } else {
197 // const Eigen::Matrix<PrimType_, 3, 1> axis = (v1.cross(v2)).normalized();
198 // rot = AngleAxis<PrimType_>(angle, axis);
199 // }
200  }
201 };
202 
203 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
204  * Disparity Angle Traits
205  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
206 
210 template<typename Left_, typename Right_>
211 class DisparityAngleTraits<RotationBase<Left_>, RotationBase<Right_>> {
212  public:
220  inline static typename Left_::Scalar compute(const RotationBase<Left_>& left, const RotationBase<Right_>& right) {
221  typedef typename Left_::Scalar Scalar;
222  return std::abs(floatingPointModulo(AngleAxis<Scalar>(left.derived()*right.derived().inverted()).angle() + Scalar(M_PI), Scalar(2.0*M_PI))-M_PI);
223  }
224 };
225 
226 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
227  * Random Traits
228  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
229 template<typename Rotation_>
230 class RandomTraits<RotationBase<Rotation_>> {
231  public:
232  inline static void set_random(Rotation_& rot) {
233  typedef typename Rotation_::Scalar Scalar;
234  Eigen::Matrix<Scalar, 3, 1> vector;
235  setUniformRandom<Scalar, 3>(vector, -M_PI, M_PI);
236  rot = Rotation_(RotationVector<Scalar>(vector));
237  }
238 };
239 
240 } // namespace internal
241 } // namespace kindr
242 
243 
250 
251 
PrimType_ Scalar
The primitive type. Float/Double.
Derived_ & derived()
Gets the derived rotation. (only for advanced users)
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...
T floatingPointModulo(T x, T y)
Floating-point modulo.
Definition: common.hpp:46
Implementation of a rotation vector based on Eigen::Matrix<Scalar, 3, 1>
Definition: Rotation.hpp:43
Implementation of matrix rotation based on Eigen::Matrix<Scalar, 3, 3>
Definition: Rotation.hpp:49
Implementation of Euler angles (X-Y&#39;-Z&#39;&#39; / roll-pitch-yaw) rotation based on Eigen::Matrix<Scalar,3,1>
Implementation of an angle axis rotation based on Eigen::AngleAxis.
Definition: AngleAxis.hpp:54
Implementation of quaternion rotation based on Eigen::Quaternion.
Definition: Rotation.hpp:46
Implementation & toImplementation()
Cast to the implementation type.
#define KINDR_ASSERT_TRUE(exceptionType, condition, message)