QuaternionTests.cpp
Go to the documentation of this file.
1 //
2 // Created by jordan on 4/20/17.
3 //
4 
5 #include <gtest/gtest.h>
6 #include <Eigen/Dense>
7 #include "rdl_dynamics/Quaternion.h"
8 #include "rdl_dynamics/SpatialAlgebraOperators.h"
9 #include "UnitTestUtils.hpp"
10 
11 struct QuaternionFixture : public testing::Test
12 {
13  QuaternionFixture()
14  {
15  f = Eigen::IOFormat(Eigen::FullPrecision);
16  }
17 
18  Eigen::IOFormat f;
19 };
20 
21 TEST_F(QuaternionFixture, Sanitize)
22 {
23  RobotDynamics::Math::Quaternion q1(0.1, 0.2, 0.3, 0.4);
24  RobotDynamics::Math::Quaternion q2(-0.1, -0.2, -0.3, -0.4);
25  q1.normalize();
26  q2.normalize();
27  q2.sanitize();
28 
29  EXPECT_TRUE(q1.isApprox(q2, 1.e-12));
30 
31  RobotDynamics::Math::Quaternion q3(0.1, 0.2, 0.3, 0.4);
32  RobotDynamics::Math::Quaternion q4(0.1, 0.2, 0.3, 0.4);
33 
34  q3.normalize();
35  q4.normalize();
36  q4.sanitize();
37 
38  EXPECT_TRUE(q3.isApprox(q4, 1.e-12));
39 }
40 
41 TEST_F(QuaternionFixture, EigenConversion)
42 {
43  RobotDynamics::Math::Quaternion q;
44  Eigen::Quaterniond q_eig(1., 2., 3., 4.);
45 
46  EXPECT_EQ(q.x(), 0.);
47  EXPECT_EQ(q.y(), 0.);
48  EXPECT_EQ(q.z(), 0.);
49  EXPECT_EQ(q.w(), 1.);
50 
51  q = q_eig;
52 
53  EXPECT_EQ(q.x(), 2.);
54  EXPECT_EQ(q.y(), 3.);
55  EXPECT_EQ(q.z(), 4.);
56  EXPECT_EQ(q.w(), 1.);
57 }
58 
59 TEST_F(QuaternionFixture, AxisAngle)
60 {
61  RobotDynamics::Math::Vector3d v(-0.1, -0.2, 0.3);
62  v.normalize();
63  RobotDynamics::Math::AxisAngle aa1(0.5, v);
64  RobotDynamics::Math::Quaternion o1 = aa1;
65  RobotDynamics::Math::AxisAngle aa2 = RobotDynamics::Math::toAxisAngle(o1);
66 
67  EXPECT_NEAR(aa1.angle(), aa2.angle(), unit_test_utils::TEST_PREC);
68  EXPECT_TRUE(aa1.axis().isApprox(aa2.axis()));
69 
70  RobotDynamics::Math::SpatialTransform X = RobotDynamics::Math::Xrot(aa1.angle(), aa1.axis());
71  RobotDynamics::Math::Quaternion o2 = X.E;
72  EXPECT_TRUE(o2.isApprox(o1, unit_test_utils::TEST_PREC));
73 }
74 
75 TEST_F(QuaternionFixture, element_accessors)
76 {
77  RobotDynamics::Math::Quaternion q1(0.2, 0.3, 0.4, 0.1);
78 
79  q1.x() = 1.1;
80  q1.y() = 2.1;
81  q1.z() = 3.1;
82  q1.w() = 4.1;
83 
84  EXPECT_EQ(1.1, q1.x());
85  EXPECT_EQ(2.1, q1.y());
86  EXPECT_EQ(3.1, q1.z());
87  EXPECT_EQ(4.1, q1.w());
88 
89  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(q1.vec(), Vector3d(1.1, 2.1, 3.1), unit_test_utils::TEST_PREC));
90  EXPECT_EQ(q1.w(), 4.1);
91 }
92 
93 TEST_F(QuaternionFixture, multiplication)
94 {
95  RobotDynamics::Math::Quaternion q1(0.2, 0.3, 0.4, 0.1), q2(0.2, 0.1, 0.5, 0.3);
96  q1.normalize();
97  q2.normalize();
98 
99  RobotDynamics::Math::Quaternion q_exp(0.5554700788944518, 0.2338821384818745, 0.3800584750330459, -0.7016464154456233);
100 
101  EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(q1 * q2, q_exp, unit_test_utils::TEST_PREC));
102 
103  q1 *= q2;
104 
105  EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(q1, q_exp, unit_test_utils::TEST_PREC));
106 }
107 
108 TEST_F(QuaternionFixture, slerp)
109 {
110  RobotDynamics::Math::Quaternion q1(0.2, 0.3, 0.4, 0.1), q2(0.2, 0.1, 0.5, 0.3);
111  q1.normalize();
112  q2.normalize();
113 
114  RobotDynamics::Math::Quaternion q_exp(0.3633161731320073, 0.4782736431579663, 0.7599826545340369, 0.2483587031060483);
115 
116  EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(q_exp, q1.slerp(0.2, q2), unit_test_utils::TEST_PREC));
117 
118  q1 = q1 * -1.;
119  q2 = q2 * -1.;
120 
121  EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(q_exp * -1., q1.slerp(0.2, q2), unit_test_utils::TEST_PREC));
122 
123  q1 = q1 * -1.;
124  q2 = q2 * -1.;
125 
126  EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(q2, q1.slerp(1., q2), unit_test_utils::TEST_PREC));
127 }
128 
129 TEST_F(QuaternionFixture, conjugate)
130 {
131  RobotDynamics::Math::Quaternion q(0.1, 0.2, -0.3, -0.4);
132  EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(RobotDynamics::Math::Quaternion(-0.1, -0.2, 0.3, -0.4), q.conjugate(), unit_test_utils::TEST_PREC));
133 }
134 
135 TEST_F(QuaternionFixture, rotate)
136 {
137  RobotDynamics::Math::Quaternion q1(0.1, 0.2, -0.3, -0.4), q2(-0.1, -0.3, -0.2, 0.);
138  RobotDynamics::Math::Vector3d v(-0.1, -0.3, -0.2);
139  q1.normalize();
140 
141  RobotDynamics::Math::Quaternion q_exp = q1.conjugate() * q2 * q1;
142  RobotDynamics::Math::Vector3d v_out = q1.rotate(v);
143 
144  EXPECT_NEAR(v_out.x(), q_exp.x(), unit_test_utils::TEST_PREC);
145  EXPECT_NEAR(v_out.y(), q_exp.y(), unit_test_utils::TEST_PREC);
146  EXPECT_NEAR(v_out.z(), q_exp.z(), unit_test_utils::TEST_PREC);
147 }
148 
149 TEST_F(QuaternionFixture, swingTwistDecomposition)
150 {
151  double thx = M_PI / 4.;
152  double thy = M_PI / 3.;
153  double thz = -M_PI / 6.;
154 
155  RobotDynamics::Math::Quaternion qx(std::sin(thx / 2.), 0., 0., std::cos(thx / 2.)), qy(0., std::sin(thy / 2.), 0., std::cos(thy / 2.)),
156  qz(0., 0., std::sin(thz / 2.), std::cos(thz / 2.));
157  RobotDynamics::Math::Quaternion q = qx * qy * qz;
158  RobotDynamics::Math::Quaternion q_twist, q_swing;
159 
160  q.swingTwistDecomposition(Vector3d(0., 0., 1.), q_swing, q_twist);
161  EXPECT_TRUE(q.isApprox(q_swing * q_twist, 1.e-10));
162 
163  q.swingTwistDecomposition(Vector3d(0., 1., 0.), q_swing, q_twist);
164  EXPECT_TRUE(q.isApprox(q_swing * q_twist, 1.e-10));
165 
166  q.swingTwistDecomposition(Vector3d(1., 0., 0.), q_swing, q_twist);
167  EXPECT_TRUE(q.isApprox(q_swing * q_twist, 1.e-10));
168 
169  q = qz * qy * qx;
170 
171  q.swingTwistDecomposition(Vector3d(0., 0., 1.), q_swing, q_twist);
172  EXPECT_TRUE(q.isApprox(q_swing * q_twist, 1.e-10));
173 
174  q.swingTwistDecomposition(Vector3d(0., 1., 0.), q_swing, q_twist);
175  EXPECT_TRUE(q.isApprox(q_swing * q_twist, 1.e-10));
176 
177  q.swingTwistDecomposition(Vector3d(1., 0., 0.), q_swing, q_twist);
178  EXPECT_TRUE(q.isApprox(q_swing * q_twist, 1.e-10));
179 }
180 
181 TEST_F(QuaternionFixture, swingTwistDecompositionEdgeCases)
182 {
183  RobotDynamics::Math::Quaternion q(0., 0., 0., 1);
184  RobotDynamics::Math::Quaternion q_twist, q_swing;
185 
186  q.swingTwistDecomposition(Vector3d(0., 0., 1.), q_swing, q_twist);
187 
188  EXPECT_TRUE(q.isApprox(q_swing * q_twist, 1.e-10));
189 
190  q.set(0., 0., 0., -1);
191  q.swingTwistDecomposition(Vector3d(0., 1., 0.), q_swing, q_twist);
192 
193  EXPECT_TRUE(q.isApprox(q_swing * q_twist, 1.e-10));
194 
195  q.set(0., 0., 0., -1);
196  q.swingTwistDecomposition(Vector3d(1., 0., 0.), q_swing, q_twist);
197 
198  EXPECT_TRUE(q.isApprox(q_swing * q_twist, 1.e-10));
199 }
200 
201 int main(int argc, char** argv)
202 {
203  ::testing::InitGoogleTest(&argc, argv);
204  return RUN_ALL_TESTS();
205 }
RobotDynamics::Math::Quaternion::w
EIGEN_STRONG_INLINE double & w()
Definition: Quaternion.h:106
main
int main(int argc, char **argv)
Definition: QuaternionTests.cpp:201
RobotDynamics::Math::Quaternion::swingTwistDecomposition
void swingTwistDecomposition(const Vector3d &twist_axis, Quaternion &swing, Quaternion &twist)
Decompose a quaternion into a swing then twist quaternion where the twist is about the given axis...
Definition: Quaternion.h:316
unit_test_utils::TEST_PREC
const double TEST_PREC
Definition: UnitTestUtils.hpp:22
RobotDynamics::Math::Quaternion::vec
EIGEN_STRONG_INLINE Vector3d vec() const
Get vector part.
Definition: Quaternion.h:128
unit_test_utils::checkVector4dEpsilonClose
static bool checkVector4dEpsilonClose(const RobotDynamics::Math::Vector4d &v1, const RobotDynamics::Math::Vector4d &v2, const double epsilon)
Definition: UnitTestUtils.hpp:275
RobotDynamics::Math::SpatialTransform
Compact representation of spatial transformations.
Definition: rdl_eigenmath.h:337
RobotDynamics::Math::Quaternion::y
EIGEN_STRONG_INLINE double & y()
Definition: Quaternion.h:86
RobotDynamics::Math::Quaternion::sanitize
void sanitize()
sanitize the quaternion by negating each element if the w element is less than zero ...
Definition: Quaternion.h:150
RobotDynamics::Math::Quaternion
Quaternion that are used for singularity free joints.
Definition: Quaternion.h:25
TEST_F
TEST_F(QuaternionFixture, Sanitize)
Definition: QuaternionTests.cpp:21
RobotDynamics::Math::Quaternion::x
EIGEN_STRONG_INLINE double & x()
Definition: Quaternion.h:76
unit_test_utils::checkVector3dEpsilonClose
static bool checkVector3dEpsilonClose(const RobotDynamics::Math::Vector3d &v1, const RobotDynamics::Math::Vector3d &v2, const double epsilon)
Definition: UnitTestUtils.hpp:256
RobotDynamics::Math::Quaternion::slerp
Quaternion slerp(double alpha, const Quaternion &quat) const
From Wikipedia: In computer graphics, Slerp is shorthand for spherical linear interpolation, introduced by Ken Shoemake in the context of quaternion interpolation for the purpose of animating 3D rotation. It refers to constant-speed motion along a unit-radius great circle arc, given the ends and an interpolation parameter between 0 and 1.
Definition: Quaternion.h:258
RobotDynamics::Math::Quaternion::z
EIGEN_STRONG_INLINE double & z()
Definition: Quaternion.h:96
RobotDynamics::Math::Quaternion::conjugate
Quaternion conjugate() const
Definition: Quaternion.h:285
RobotDynamics::Math::Quaternion::set
void set(double x, double y, double z, double w)
Definition: Quaternion.h:116
RobotDynamics::Math::AxisAngle
Eigen::AngleAxisd AxisAngle
Definition: rdl_eigenmath.h:27
QuaternionFixture::f
Eigen::IOFormat f
Definition: QuaternionTests.cpp:18
RobotDynamics::Math::Xrot
SpatialTransform Xrot(double angle_rad, const Vector3d &axis)
Get spatial transform from angle and axis.
Definition: SpatialAlgebraOperators.h:250
RobotDynamics::Math::toAxisAngle
static AxisAngle toAxisAngle(Quaternion q)
Get axis angle representation of a quaternion.
Definition: SpatialAlgebraOperators.h:132

rdl_dynamics
Author(s):
autogenerated on Tue Apr 20 2021 02:25:27