Kindr
Kinematics and Dynamics for Robotics
gtest_rotations.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 /*
30  * gtest_rotations.hpp
31  *
32  * Created on: Jun 16, 2016
33  * Author: Christian Gehring
34  */
35 
36 #pragma once
37 
38 #include <gtest/gtest.h>
39 #include <kindr/Core>
42 
43 namespace kindr {
44 
46 
57 template<typename OtherRotation_, typename OtherVelocity_, typename PrimType_>
59  public:
60  inline static void testConcatenation() {
63  OtherRotation_ otherQuat1;
65 
66  OtherRotation_ otherQuat2;
68 
69  // Concatenate with Kindr
70  kindr::RotationQuaternion<PrimType_> kindrQuatKindrConcat = kindrQuat2*kindrQuat1;
71 
72  // Concatenate with other rotation
73  OtherRotation_ otherQuatOtherConcat;
74  RotationConversion<OtherRotation_, OtherVelocity_, PrimType_>::concatenate(otherQuatOtherConcat, otherQuat1, otherQuat2);
75 
76  kindr::RotationQuaternion<PrimType_> kindrQuatOtherConcat;
78 
79  // Test quaternions
80  ASSERT_TRUE(kindrQuatOtherConcat.isNear(kindrQuatKindrConcat, 1.0e-3));
81  }
82 
83  inline static void testRotationMatrix() {
85  kindr::RotationMatrix<PrimType_> kindrMatrix(kindrQuat);
86  OtherRotation_ otherQuat;
88 
89  Eigen::Matrix<PrimType_, 3, 3> eigenOtherMatrix;
91 
92  Eigen::Matrix<PrimType_, 3, 3> eigenKindrMatrix = kindrMatrix.matrix();
93 
94  // Test matrices
95  KINDR_ASSERT_DOUBLE_MX_EQ(eigenKindrMatrix, eigenOtherMatrix, 1.0e-3, "rotation matrix");
96 
97  }
98 
99 
100  inline static void testGeometricalInterpretation() {
101  /* Consider two coordinate frames A and B. A is fixed, while B is rotated around the z-axis by alpha = pi/4 w.r.t. A.
102  * The unit vectors of B in B frame are the vectors of the canonical base of R^3.
103  * Using a geometrical approach, it can easily be shown that the components of these vectors expressed in A frame are:
104  *
105  * A_r_Bx = [ cos(alpha) sin(alpha) 0]^T;
106  * A_r_By = [-sin(alpha) cos(alpha) 0]^T;
107  * A_r_Bz = [ 0 0 1]^T;
108  *
109  * Stacking these transformations column-wise returns a rotation matrix that projects the coordinates of a vector
110  * expressed in the B frame to those of a vector expressed in A frame:
111  *
112  * C_AB = [ A_r_Bx A_r_By A_r_Bz]
113  *
114  * A_r = C_AB * B_r
115  */
116  using std::cos;
117  using std::sin;
118  typedef Eigen::Matrix<PrimType_, 3, 1> Vector;
119 
120  // Angle of elementary rotation
121  const PrimType_ alpha = M_PI_4;
123 
124  Vector expected_A_r_Bx, expected_A_r_By, expected_A_r_Bz;
125 
126  // Elementary rotation around z-axis
127  rotationBToA(kindr::EulerAnglesZyx<PrimType_>(alpha, 0.0, 0.0));
128  expected_A_r_Bx = Vector(cos(alpha), sin(alpha), PrimType_(0.0));
129  expected_A_r_By = Vector(-sin(alpha), cos(alpha), PrimType_(0.0));
130  expected_A_r_Bz = Vector(PrimType_(0.0), PrimType_(0.0), PrimType_(1.0));
131  checkGeometricalInterpretation(rotationBToA, expected_A_r_Bx, expected_A_r_By, expected_A_r_Bz);
132 
133  // Elementary rotation around y-axis
134  rotationBToA(kindr::EulerAnglesZyx<PrimType_>(0.0, alpha, 0.0));
135  expected_A_r_Bx = Vector(cos(alpha), PrimType_(0.0), -sin(alpha));
136  expected_A_r_By = Vector(PrimType_(0.0), PrimType_(1.0), PrimType_(0.0));
137  expected_A_r_Bz = Vector(sin(alpha), PrimType_(0.0), cos(alpha));
138  checkGeometricalInterpretation(rotationBToA, expected_A_r_Bx, expected_A_r_By, expected_A_r_Bz);
139 
140  // Elementary rotation around x-axis
141  rotationBToA(kindr::EulerAnglesZyx<PrimType_>(0.0, 0.0, alpha));
142  expected_A_r_Bx = Vector(PrimType_(1.0), PrimType_(0.0), PrimType_(0.0));
143  expected_A_r_By = Vector(PrimType_(0.0), cos(alpha), sin(alpha));
144  expected_A_r_Bz = Vector(PrimType_(0.0), -sin(alpha), cos(alpha));
145  checkGeometricalInterpretation(rotationBToA, expected_A_r_Bx, expected_A_r_By, expected_A_r_Bz);
146  }
147 
149  Eigen::Matrix<PrimType_, 3, 1> expected_A_r_Bx,
150  Eigen::Matrix<PrimType_, 3, 1> expected_A_r_By,
151  Eigen::Matrix<PrimType_, 3, 1> expected_A_r_Bz) {
152 
153  typedef Eigen::Matrix<PrimType_, 3, 1> Vector;
154  using std::cos;
155  using std::sin;
156 
157  /* Consider two coordinate frames A and B. A is fixed, while B is rotated by alpha = pi/4 w.r.t. A.
158  * The unit vectors of B in B frame are the vectors of the canonical base of R^3.
159  * Using a geometrical approach, it can easily be shown that the components of these vectors expressed in A frame are:
160  *
161  * A_r_Bx = [ cos(alpha) sin(alpha) 0]^T;
162  * A_r_By = [-sin(alpha) cos(alpha) 0]^T;
163  * A_r_Bz = [ 0 0 1]^T;
164  *
165  * Stacking these transformations column-wise returns a rotation matrix that projects the coordinates of a vector
166  * expressed in the B frame to those of a vector expressed in A frame:
167  *
168  * C_AB = [ A_r_Bx A_r_By A_r_Bz]
169  */
170  const Vector B_r_Bx = Vector::UnitX();
171  const Vector B_r_By = Vector::UnitY();
172  const Vector B_r_Bz = Vector::UnitZ();
173 
174  OtherRotation_ otherRotationBToA;
176 
177  Eigen::Matrix<PrimType_, 3, 3> eigenRotationMatrix;
179 
180 
181  // Compute unit vectors with matrix multiplication
182  Vector computed_A_r_Bx = eigenRotationMatrix*B_r_Bx;
183  Vector computed_A_r_By = eigenRotationMatrix*B_r_By;
184  Vector computed_A_r_Bz = eigenRotationMatrix*B_r_Bz;
185 
186  // Test unit vectors
187  KINDR_ASSERT_DOUBLE_MX_EQ(expected_A_r_Bx, computed_A_r_Bx, 1.0e-3, "matrix multiply A_r_Bx");
188  KINDR_ASSERT_DOUBLE_MX_EQ(expected_A_r_By, computed_A_r_By, 1.0e-3, "matrix multiply A_r_By");
189  KINDR_ASSERT_DOUBLE_MX_EQ(expected_A_r_Bz, computed_A_r_Bz, 1.0e-3, "matrix multiply A_r_Bz");
190 
191 
192  computed_A_r_Bx.setZero();
193  computed_A_r_By.setZero();
194  computed_A_r_Bz.setZero();
195 
196 
197  // Compute unit vectors with custom rotation operator
198  RotationConversion<OtherRotation_, OtherVelocity_, PrimType_>::rotateVector(computed_A_r_Bx, otherRotationBToA, B_r_Bx);
199  RotationConversion<OtherRotation_, OtherVelocity_, PrimType_>::rotateVector(computed_A_r_By, otherRotationBToA, B_r_By);
200  RotationConversion<OtherRotation_, OtherVelocity_, PrimType_>::rotateVector(computed_A_r_Bz, otherRotationBToA, B_r_Bz);
201 
202  // Test unit vectors
203  KINDR_ASSERT_DOUBLE_MX_EQ(expected_A_r_Bx, computed_A_r_Bx, 1.0e-3, "rotate A_r_Bx");
204  KINDR_ASSERT_DOUBLE_MX_EQ(expected_A_r_By, computed_A_r_By, 1.0e-3, "rotate A_r_By");
205  KINDR_ASSERT_DOUBLE_MX_EQ(expected_A_r_Bz, computed_A_r_Bz, 1.0e-3, "rotate A_r_Bz");
206 
207  }
208 
209  inline static void testBoxPlus() {
211  Eigen::Matrix<PrimType_, 3, 1> kindrVelocity(0.5, 0.7, 0.9);
212  OtherRotation_ otherQuat;
214 
215  OtherVelocity_ otherVelocity;
217 
218  OtherRotation_ otherQuatOtherPlus;
219  RotationConversion<OtherRotation_, OtherVelocity_, PrimType_>::boxPlus(otherQuatOtherPlus, otherQuat, otherVelocity);
220  kindr::RotationQuaternion<PrimType_> kindrQuatKindrPlus = kindrQuat.boxPlus(kindrVelocity);
221 
222  kindr::RotationQuaternion<PrimType_> kindrQuatOtherPlus;
224 
225  // Test quaternions
226  ASSERT_TRUE(kindrQuatKindrPlus.isNear(kindrQuatOtherPlus, 1.0e-3));
227 
228  }
229 };
230 
231 } // namespace
232 
static void checkGeometricalInterpretation(kindr::RotationQuaternion< PrimType_ > rotationBToA, Eigen::Matrix< PrimType_, 3, 1 > expected_A_r_Bx, Eigen::Matrix< PrimType_, 3, 1 > expected_A_r_By, Eigen::Matrix< PrimType_, 3, 1 > expected_A_r_Bz)
Vector in n-dimensional-space.
Definition: Vector.hpp:52
Implementation matrix() const
Reading access to the rotation matrix.
static void testConcatenation()
static void convertToOtherVelocityVector(OtherVelocity_ &otherVelocity, const OtherRotation_ &rotation, const Eigen::Matrix< PrimType_, 3, 1 > &velocityIn)
bool isNear(const RotationBase< OtherDerived_ > &other, typename internal::get_scalar< RotationQuaternion< PrimType_ > >::Scalar tol) const
Compares two rotations with a tolerance.
Implementation of Euler angles (Z-Y&#39;-X&#39;&#39; / yaw-pitch-roll) rotation based on Eigen::Matrix<Scalar, 3, 1>
static Vector< PhysicalType_, PrimType_, Dimension_ > UnitX()
Get the unity vector in x.
Definition: Vector.hpp:159
static Vector< PhysicalType_, PrimType_, Dimension_ > UnitZ()
Get the unity vector in z.
Definition: Vector.hpp:173
static void testRotationMatrix()
static void testGeometricalInterpretation()
static void concatenate(OtherRotation_ &result, const OtherRotation_ &rot1, OtherRotation_ &rot2)
static void getRotationMatrixFromRotation(Eigen::Matrix< PrimType_, 3, 3 > &rotationMatrix, const OtherRotation_ &rotation)
static Vector< PhysicalType_, PrimType_, Dimension_ > UnitY()
Get the unity vector in y.
Definition: Vector.hpp:166
Implementation of matrix rotation based on Eigen::Matrix<Scalar, 3, 3>
Definition: Rotation.hpp:49
static void convertToKindr(kindr::RotationQuaternion< PrimType_ > &quaternion, const OtherRotation_ &otherRotation)
#define KINDR_ASSERT_DOUBLE_MX_EQ(A, B, PERCENT_TOLERANCE, MSG)
static void rotateVector(Eigen::Matrix< PrimType_, 3, 1 > &A_r, const OtherRotation_ &rotationBToA, const Eigen::Matrix< PrimType_, 3, 1 > &B_r)
static void convertToOtherRotation(OtherRotation_ &otherRotation, const kindr::RotationQuaternion< PrimType_ > &quaternionIn)
static void boxPlus(OtherRotation_ &result, const OtherRotation_ &rotation, const OtherVelocity_ &velocity)
RotationQuaternion< PrimType_ > boxPlus(const typename internal::get_matrix3X< RotationQuaternion< PrimType_ > >::template Matrix3X< 1 > &vector) const
Applies the box plus operation.
Vector< PhysicalType_, PrimType_, Dimension_ > & setZero()
Sets all components of the vector to zero.
Definition: Vector.hpp:136
Implementation of quaternion rotation based on Eigen::Quaternion.
Definition: Rotation.hpp:46
Convention tests that need to be fulfilled.