405 lines
14 KiB
C++
405 lines
14 KiB
C++
/*
|
|
* Copyright (C) 2012 Open Source Robotics Foundation
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*
|
|
*/
|
|
|
|
#include <gtest/gtest.h>
|
|
|
|
#include "gazebo/math/Helpers.hh"
|
|
#include "gazebo/math/Quaternion.hh"
|
|
|
|
using namespace gazebo;
|
|
|
|
class QuaternionTest : public ::testing::Test { };
|
|
|
|
//////////////////////////////////////////////////
|
|
TEST_F(QuaternionTest, Quaternion)
|
|
{
|
|
{
|
|
math::Quaternion q;
|
|
EXPECT_TRUE(math::equal(q.w, 1.0));
|
|
EXPECT_TRUE(math::equal(q.x, 0.0));
|
|
EXPECT_TRUE(math::equal(q.y, 0.0));
|
|
EXPECT_TRUE(math::equal(q.z, 0.0));
|
|
}
|
|
|
|
{
|
|
math::Quaternion q(1, 2, 3, 4);
|
|
EXPECT_TRUE(math::equal(q.w, 1.0));
|
|
EXPECT_TRUE(math::equal(q.x, 2.0));
|
|
EXPECT_TRUE(math::equal(q.y, 3.0));
|
|
EXPECT_TRUE(math::equal(q.z, 4.0));
|
|
}
|
|
|
|
{
|
|
math::Quaternion q(0, 1, 2);
|
|
EXPECT_TRUE(q == math::Quaternion(math::Vector3(0, 1, 2)));
|
|
}
|
|
|
|
math::Quaternion q1(math::Vector3(0, 0, 1), M_PI);
|
|
EXPECT_TRUE(math::equal(q1.x, 0.0));
|
|
EXPECT_TRUE(math::equal(q1.y, 0.0));
|
|
EXPECT_TRUE(math::equal(q1.z, 1.0));
|
|
EXPECT_TRUE(math::equal(q1.w, 0.0));
|
|
|
|
math::Quaternion q(q1);
|
|
EXPECT_TRUE(q == q1);
|
|
|
|
q.SetToIdentity();
|
|
EXPECT_TRUE(math::equal(q.w, 1.0));
|
|
EXPECT_TRUE(math::equal(q.x, 0.0));
|
|
EXPECT_TRUE(math::equal(q.y, 0.0));
|
|
EXPECT_TRUE(math::equal(q.z, 0.0));
|
|
|
|
q = math::Quaternion(M_PI*0.1, M_PI*0.5, M_PI);
|
|
EXPECT_TRUE(q == math::Quaternion(0.110616, -0.698401, 0.110616, 0.698401));
|
|
|
|
EXPECT_TRUE(q.GetLog() ==
|
|
math::Quaternion(0, -1.02593, 0.162491, 1.02593));
|
|
|
|
EXPECT_TRUE(q.GetExp() ==
|
|
math::Quaternion(0.545456, -0.588972, 0.093284, 0.588972));
|
|
|
|
q1 = q;
|
|
q1.w = 2.0;
|
|
EXPECT_TRUE(q1.GetLog() ==
|
|
math::Quaternion(0, -0.698401, 0.110616, 0.698401));
|
|
|
|
q1.x = 0.000000001;
|
|
q1.y = 0.0;
|
|
q1.z = 0.0;
|
|
q1.w = 0.0;
|
|
EXPECT_TRUE(q1.GetExp() == math::Quaternion(1, 0, 0, 0));
|
|
|
|
q.Invert();
|
|
EXPECT_TRUE(q == math::Quaternion(0.110616, 0.698401, -0.110616, -0.698401));
|
|
|
|
q.SetFromAxis(0, 1, 0, M_PI);
|
|
EXPECT_TRUE(q == math::Quaternion(6.12303e-17, 0, 1, 0));
|
|
|
|
q.SetFromAxis(math::Vector3(1, 0, 0), M_PI);
|
|
EXPECT_TRUE(q == math::Quaternion(0, 1, 0, 0));
|
|
|
|
q.Set(1, 2, 3, 4);
|
|
EXPECT_TRUE(math::equal(q.w, 1.0));
|
|
EXPECT_TRUE(math::equal(q.x, 2.0));
|
|
EXPECT_TRUE(math::equal(q.y, 3.0));
|
|
EXPECT_TRUE(math::equal(q.z, 4.0));
|
|
|
|
q.Normalize();
|
|
EXPECT_TRUE(q == math::Quaternion(0.182574, 0.365148, 0.547723, 0.730297));
|
|
|
|
|
|
EXPECT_TRUE(math::equal(q.GetRoll(), 1.4289, 1e-3));
|
|
EXPECT_TRUE(math::equal(q.GetPitch(), -0.339837, 1e-3));
|
|
EXPECT_TRUE(math::equal(q.GetYaw(), 2.35619, 1e-3));
|
|
|
|
math::Vector3 axis;
|
|
double angle;
|
|
q.GetAsAxis(axis, angle);
|
|
EXPECT_TRUE(axis == math::Vector3(0.371391, 0.557086, 0.742781));
|
|
EXPECT_TRUE(math::equal(angle, 2.77438, 1e-3));
|
|
|
|
q.Scale(0.1);
|
|
EXPECT_TRUE(q == math::Quaternion(0.990394, 0.051354, 0.0770309, 0.102708));
|
|
|
|
q = q + math::Quaternion(0, 1, 2);
|
|
EXPECT_TRUE(q == math::Quaternion(1.46455, -0.352069, 0.336066, 0.841168));
|
|
|
|
q += q;
|
|
EXPECT_TRUE(q == math::Quaternion(2.92911, -0.704137, 0.672131, 1.68234));
|
|
|
|
q -= math::Quaternion(.4, .2, .1);
|
|
EXPECT_TRUE(q == math::Quaternion(1.95416, -0.896677, 0.56453, 1.65341));
|
|
|
|
q = q - math::Quaternion(0, 1, 2);
|
|
EXPECT_TRUE(q == math::Quaternion(1.48, -0.493254, 0.305496, 0.914947));
|
|
|
|
q *= math::Quaternion(.4, .1, .01);
|
|
EXPECT_TRUE(q == math::Quaternion(1.53584, -0.236801, 0.551841, 0.802979));
|
|
|
|
q = q * 5.0;
|
|
EXPECT_TRUE(q == math::Quaternion(7.67918, -1.184, 2.7592, 4.0149));
|
|
|
|
std::cerr << "[" << q.w << ", " << q.x << ", " << q.y << ", " << q.z << "]\n";
|
|
std::cerr << q.RotateVectorReverse(math::Vector3(1, 2, 3)) << "\n";
|
|
|
|
EXPECT_TRUE(q.RotateVectorReverse(math::Vector3(1, 2, 3)) ==
|
|
math::Vector3(-0.104115, 0.4975, 3.70697));
|
|
|
|
EXPECT_TRUE(math::equal(q.Dot(math::Quaternion(.4, .2, .1)), 7.67183, 1e-3));
|
|
|
|
EXPECT_TRUE(math::Quaternion::Squad(1.1, math::Quaternion(.1, 0, .2),
|
|
math::Quaternion(0, .3, .4), math::Quaternion(.5, .2, 1),
|
|
math::Quaternion(0, 0, 2), true) ==
|
|
math::Quaternion(0.346807, -0.0511734, -0.0494723, 0.935232));
|
|
|
|
EXPECT_TRUE(math::Quaternion::EulerToQuaternion(math::Vector3(.1, .2, .3)) ==
|
|
math::Quaternion(0.983347, 0.0342708, 0.106021, 0.143572));
|
|
|
|
q.Round(2);
|
|
EXPECT_TRUE(math::equal(-1.18, q.x));
|
|
EXPECT_TRUE(math::equal(2.76, q.y));
|
|
EXPECT_TRUE(math::equal(4.01, q.z));
|
|
EXPECT_TRUE(math::equal(7.68, q.w));
|
|
|
|
q.x = q.y = q.z = q.w = 0.0;
|
|
q.Normalize();
|
|
EXPECT_TRUE(q == math::Quaternion());
|
|
|
|
q.SetFromAxis(0, 0, 0, 0);
|
|
EXPECT_TRUE(q == math::Quaternion());
|
|
|
|
EXPECT_TRUE(math::Quaternion::EulerToQuaternion(0.1, 0.2, 0.3) ==
|
|
math::Quaternion(0.983347, 0.0342708, 0.106021, 0.143572));
|
|
|
|
q.x = q.y = q.z = q.w = 0.0;
|
|
q.GetAsAxis(axis, angle);
|
|
EXPECT_TRUE(axis == math::Vector3(1, 0, 0));
|
|
EXPECT_TRUE(math::equal(angle, 0.0, 1e-3));
|
|
{
|
|
// simple 180 rotation about yaw, should result in x and y flipping signs
|
|
q = math::Quaternion(0, 0, M_PI);
|
|
math::Vector3 v = math::Vector3(1, 2, 3);
|
|
math::Vector3 r1 = q.RotateVector(v);
|
|
math::Vector3 r2 = q.RotateVectorReverse(v);
|
|
std::cout << "[" << q.w << ", " << q.x << ", "
|
|
<< q.y << ", " << q.z << "]\n";
|
|
std::cout << " forward turns [" << v << "] to [" << r1 << "]\n";
|
|
std::cout << " reverse turns [" << v << "] to [" << r2 << "]\n";
|
|
EXPECT_TRUE(r1 == math::Vector3(-1, -2, 3));
|
|
EXPECT_TRUE(r2 == math::Vector3(-1, -2, 3));
|
|
}
|
|
|
|
{
|
|
// simple 90 rotation about yaw, should map x to y, y to -x
|
|
// simple -90 rotation about yaw, should map x to -y, y to x
|
|
q = math::Quaternion(0, 0, 0.5*M_PI);
|
|
math::Vector3 v = math::Vector3(1, 2, 3);
|
|
math::Vector3 r1 = q.RotateVector(v);
|
|
math::Vector3 r2 = q.RotateVectorReverse(v);
|
|
std::cout << "[" << q.w << ", " << q.x << ", "
|
|
<< q.y << ", " << q.z << "]\n";
|
|
std::cout << " forward turns [" << v << "] to [" << r1 << "]\n";
|
|
std::cout << " reverse turns [" << v << "] to [" << r2 << "]\n";
|
|
std::cout << " x axis [" << q.GetXAxis() << "]\n";
|
|
std::cout << " y axis [" << q.GetYAxis() << "]\n";
|
|
std::cout << " z axis [" << q.GetZAxis() << "]\n";
|
|
EXPECT_TRUE(r1 == math::Vector3(-2, 1, 3));
|
|
EXPECT_TRUE(r2 == math::Vector3(2, -1, 3));
|
|
EXPECT_TRUE(q.GetInverse().GetXAxis() == math::Vector3(0, -1, 0));
|
|
EXPECT_TRUE(q.GetInverse().GetYAxis() == math::Vector3(1, 0, 0));
|
|
EXPECT_TRUE(q.GetInverse().GetZAxis() == math::Vector3(0, 0, 1));
|
|
}
|
|
|
|
// Test RPY fixed-body-frame convention:
|
|
// Rotate each unit vector in roll and pitch
|
|
{
|
|
q = math::Quaternion(M_PI/2.0, M_PI/2.0, 0);
|
|
math::Vector3 v1(1, 0, 0);
|
|
math::Vector3 r1 = q.RotateVector(v1);
|
|
// 90 degrees about X does nothing,
|
|
// 90 degrees about Y sends point down to -Z
|
|
EXPECT_EQ(r1, math::Vector3(0, 0, -1));
|
|
|
|
math::Vector3 v2(0, 1, 0);
|
|
math::Vector3 r2 = q.RotateVector(v2);
|
|
// 90 degrees about X sends point to +Z
|
|
// 90 degrees about Y sends point to +X
|
|
EXPECT_EQ(r2, math::Vector3(1, 0, 0));
|
|
|
|
math::Vector3 v3(0, 0, 1);
|
|
math::Vector3 r3 = q.RotateVector(v3);
|
|
// 90 degrees about X sends point to -Y
|
|
// 90 degrees about Y does nothing
|
|
EXPECT_EQ(r3, math::Vector3(0, -1, 0));
|
|
}
|
|
|
|
{
|
|
// now try a harder case (axis[1,2,3], rotation[0.3*pi])
|
|
// verified with octave
|
|
q.SetFromAxis(math::Vector3(1, 2, 3), 0.3*M_PI);
|
|
std::cout << "[" << q.w << ", " << q.x << ", "
|
|
<< q.y << ", " << q.z << "]\n";
|
|
std::cout << " x [" << q.GetInverse().GetXAxis() << "]\n";
|
|
std::cout << " y [" << q.GetInverse().GetYAxis() << "]\n";
|
|
std::cout << " z [" << q.GetInverse().GetZAxis() << "]\n";
|
|
EXPECT_TRUE(q.GetInverse().GetXAxis() ==
|
|
math::Vector3(0.617229, -0.589769, 0.520770));
|
|
EXPECT_TRUE(q.GetInverse().GetYAxis() ==
|
|
math::Vector3(0.707544, 0.705561, -0.039555));
|
|
EXPECT_TRUE(q.GetInverse().GetZAxis() ==
|
|
math::Vector3(-0.344106, 0.392882, 0.852780));
|
|
|
|
// rotate about the axis of rotation should not change axis
|
|
math::Vector3 v = math::Vector3(1, 2, 3);
|
|
math::Vector3 r1 = q.RotateVector(v);
|
|
math::Vector3 r2 = q.RotateVectorReverse(v);
|
|
EXPECT_TRUE(r1 == math::Vector3(1, 2, 3));
|
|
EXPECT_TRUE(r2 == math::Vector3(1, 2, 3));
|
|
|
|
// rotate unit vectors
|
|
v = math::Vector3(0, 0, 1);
|
|
r1 = q.RotateVector(v);
|
|
r2 = q.RotateVectorReverse(v);
|
|
EXPECT_TRUE(r1 == math::Vector3(0.520770, -0.039555, 0.852780));
|
|
EXPECT_TRUE(r2 == math::Vector3(-0.34411, 0.39288, 0.85278));
|
|
v = math::Vector3(0, 1, 0);
|
|
r1 = q.RotateVector(v);
|
|
r2 = q.RotateVectorReverse(v);
|
|
EXPECT_TRUE(r1 == math::Vector3(-0.58977, 0.70556, 0.39288));
|
|
EXPECT_TRUE(r2 == math::Vector3(0.707544, 0.705561, -0.039555));
|
|
v = math::Vector3(1, 0, 0);
|
|
r1 = q.RotateVector(v);
|
|
r2 = q.RotateVectorReverse(v);
|
|
EXPECT_TRUE(r1 == math::Vector3(0.61723, 0.70754, -0.34411));
|
|
EXPECT_TRUE(r2 == math::Vector3(0.61723, -0.58977, 0.52077));
|
|
|
|
EXPECT_TRUE(-q == math::Quaternion(-0.891007, -0.121334,
|
|
-0.242668, -0.364002));
|
|
|
|
EXPECT_TRUE(q.GetAsMatrix3() == math::Matrix3(
|
|
0.617229, -0.589769, 0.52077,
|
|
0.707544, 0.705561, -0.0395554,
|
|
-0.344106, 0.392882, 0.85278));
|
|
|
|
EXPECT_TRUE(q.GetAsMatrix4() == math::Matrix4(
|
|
0.617229, -0.589769, 0.52077, 0,
|
|
0.707544, 0.705561, -0.0395554, 0,
|
|
-0.344106, 0.392882, 0.85278, 0,
|
|
0, 0, 0, 1));
|
|
}
|
|
|
|
// Test quaternion multiplication (rotation) order of application
|
|
// if qa rotates frame o to p
|
|
// qb rotates frame p to q
|
|
// qc rotates frame q to r
|
|
// qd rotates frame r to s
|
|
// then qd * qc * qb * qa rotates frame o to s
|
|
EXPECT_EQ(math::Quaternion(0, 0, 0),
|
|
math::Quaternion(0, -0.5*M_PI, 0)*
|
|
math::Quaternion(-0.5*M_PI, 0, 0)*
|
|
math::Quaternion(0, 0.5*M_PI, 0)*
|
|
math::Quaternion(0, 0, 0.5*M_PI));
|
|
EXPECT_EQ(math::Quaternion(0, 0, M_PI),
|
|
math::Quaternion(0, 0, 0.5*M_PI)*
|
|
math::Quaternion(0, 0.5*M_PI, 0)*
|
|
math::Quaternion(-0.5*M_PI, 0, 0)*
|
|
math::Quaternion(0, -0.5*M_PI, 0));
|
|
}
|
|
|
|
//////////////////////////////////////////////////
|
|
TEST_F(QuaternionTest, Integrate)
|
|
{
|
|
// Integrate by zero, expect no change
|
|
{
|
|
const math::Quaternion q(0.5, 0.5, 0.5, 0.5);
|
|
EXPECT_EQ(q, q.Integrate(math::Vector3::Zero, 1.0));
|
|
EXPECT_EQ(q, q.Integrate(math::Vector3::UnitX, 0.0));
|
|
EXPECT_EQ(q, q.Integrate(math::Vector3::UnitY, 0.0));
|
|
EXPECT_EQ(q, q.Integrate(math::Vector3::UnitZ, 0.0));
|
|
}
|
|
|
|
// Integrate along single axes,
|
|
// expect linear change in roll, pitch, yaw
|
|
{
|
|
const math::Quaternion q(1, 0, 0, 0);
|
|
math::Quaternion qRoll = q.Integrate(math::Vector3::UnitX, 1.0);
|
|
math::Quaternion qPitch = q.Integrate(math::Vector3::UnitY, 1.0);
|
|
math::Quaternion qYaw = q.Integrate(math::Vector3::UnitZ, 1.0);
|
|
EXPECT_EQ(qRoll.GetAsEuler(), math::Vector3::UnitX);
|
|
EXPECT_EQ(qPitch.GetAsEuler(), math::Vector3::UnitY);
|
|
EXPECT_EQ(qYaw.GetAsEuler(), math::Vector3::UnitZ);
|
|
}
|
|
|
|
// Integrate sequentially along single axes in order XYZ,
|
|
// expect rotations to match Euler Angles
|
|
{
|
|
const math::Quaternion q(1, 0, 0, 0);
|
|
const double angle = 0.5;
|
|
math::Quaternion qX = q.Integrate(math::Vector3::UnitX, angle);
|
|
math::Quaternion qXY = qX.Integrate(math::Vector3::UnitY, angle);
|
|
EXPECT_EQ(qXY.GetAsEuler(), angle*math::Vector3(1, 1, 0));
|
|
}
|
|
{
|
|
const math::Quaternion q(1, 0, 0, 0);
|
|
const double angle = 0.5;
|
|
math::Quaternion qX = q.Integrate(math::Vector3::UnitX, angle);
|
|
math::Quaternion qXZ = qX.Integrate(math::Vector3::UnitZ, angle);
|
|
EXPECT_EQ(qXZ.GetAsEuler(), angle*math::Vector3(1, 0, 1));
|
|
}
|
|
{
|
|
const math::Quaternion q(1, 0, 0, 0);
|
|
const double angle = 0.5;
|
|
math::Quaternion qY = q.Integrate(math::Vector3::UnitY, angle);
|
|
math::Quaternion qYZ = qY.Integrate(math::Vector3::UnitZ, angle);
|
|
EXPECT_EQ(qYZ.GetAsEuler(), angle*math::Vector3(0, 1, 1));
|
|
}
|
|
{
|
|
const math::Quaternion q(1, 0, 0, 0);
|
|
const double angle = 0.5;
|
|
math::Quaternion qX = q.Integrate(math::Vector3::UnitX, angle);
|
|
math::Quaternion qXY = qX.Integrate(math::Vector3::UnitY, angle);
|
|
math::Quaternion qXYZ = qXY.Integrate(math::Vector3::UnitZ, angle);
|
|
EXPECT_EQ(qXYZ.GetAsEuler(), angle*math::Vector3::One);
|
|
}
|
|
|
|
// Integrate sequentially along single axes in order ZYX,
|
|
// expect rotations to not match Euler Angles
|
|
{
|
|
const math::Quaternion q(1, 0, 0, 0);
|
|
const double angle = 0.5;
|
|
math::Quaternion qZ = q.Integrate(math::Vector3::UnitZ, angle);
|
|
math::Quaternion qZY = qZ.Integrate(math::Vector3::UnitY, angle);
|
|
EXPECT_NE(qZY.GetAsEuler(), angle*math::Vector3(0, 1, 1));
|
|
}
|
|
{
|
|
const math::Quaternion q(1, 0, 0, 0);
|
|
const double angle = 0.5;
|
|
math::Quaternion qZ = q.Integrate(math::Vector3::UnitZ, angle);
|
|
math::Quaternion qZX = qZ.Integrate(math::Vector3::UnitX, angle);
|
|
EXPECT_NE(qZX.GetAsEuler(), angle*math::Vector3(1, 0, 1));
|
|
}
|
|
{
|
|
const math::Quaternion q(1, 0, 0, 0);
|
|
const double angle = 0.5;
|
|
math::Quaternion qZ = q.Integrate(math::Vector3::UnitZ, angle);
|
|
math::Quaternion qZY = qZ.Integrate(math::Vector3::UnitY, angle);
|
|
math::Quaternion qZYX = qZY.Integrate(math::Vector3::UnitX, angle);
|
|
EXPECT_NE(qZYX.GetAsEuler(), angle*math::Vector3(1, 1, 1));
|
|
}
|
|
{
|
|
const math::Quaternion q(1, 0, 0, 0);
|
|
const double angle = 0.5;
|
|
math::Quaternion qY = q.Integrate(math::Vector3::UnitY, angle);
|
|
math::Quaternion qYX = qY.Integrate(math::Vector3::UnitX, angle);
|
|
EXPECT_NE(qYX.GetAsEuler(), angle*math::Vector3(1, 1, 0));
|
|
}
|
|
|
|
// Integrate a full rotation about different axes,
|
|
// expect no change.
|
|
{
|
|
const math::Quaternion q(0.5, 0.5, 0.5, 0.5);
|
|
const double fourPi = 4 * M_PI;
|
|
math::Quaternion qX = q.Integrate(math::Vector3::UnitX, fourPi);
|
|
math::Quaternion qY = q.Integrate(math::Vector3::UnitY, fourPi);
|
|
math::Quaternion qZ = q.Integrate(math::Vector3::UnitZ, fourPi);
|
|
EXPECT_EQ(q, qX);
|
|
EXPECT_EQ(q, qY);
|
|
EXPECT_EQ(q, qZ);
|
|
}
|
|
}
|
|
|