From db0ca55b618ac4534498ac4e9642259850cee6a5 Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@laas.fr>
Date: Tue, 26 Nov 2019 13:13:45 +0100
Subject: [PATCH] [Tests] so3Linear : correctly tests the angular velocity when
constructed from matrix
---
tests/Main.cpp | 26 ++++++++++++++++----------
1 file changed, 16 insertions(+), 10 deletions(-)
diff --git a/tests/Main.cpp b/tests/Main.cpp
index c9a6a8a..787cd73 100644
--- a/tests/Main.cpp
+++ b/tests/Main.cpp
@@ -1623,42 +1623,44 @@ void polynomialFromBoundaryConditions(bool& error) {
void so3LinearTest(bool& error) {
quaternion_t q0(1, 0, 0, 0);
quaternion_t q1(0.7071, 0.7071, 0, 0);
- SO3Linear_t so3Traj(q0, q1, 0., 1.5);
+ const double tMin = 0.;
+ const double tMax = 1.5;
+ SO3Linear_t so3Traj(q0, q1, tMin, tMax);
- if (so3Traj.min() != 0) {
+ if (so3Traj.min() != tMin) {
error = true;
std::cout << "Min bound not respected" << std::endl;
}
- if (so3Traj.max() != 1.5) {
+ if (so3Traj.max() != tMax) {
error = true;
std::cout << "Max bound not respected" << std::endl;
}
- if (!so3Traj.computeAsQuaternion(0).isApprox(q0)) {
+ if (!so3Traj.computeAsQuaternion(tMin).isApprox(q0)) {
error = true;
std::cout << "evaluate at t=0 is not the init quaternion" << std::endl;
}
- if (so3Traj(0) != q0.toRotationMatrix()) {
+ if (so3Traj(tMin) != q0.toRotationMatrix()) {
error = true;
std::cout << "evaluate at t=0 is not the init rotation" << std::endl;
}
- if (!so3Traj.computeAsQuaternion(1.5).isApprox(q1)) {
+ if (!so3Traj.computeAsQuaternion(tMax).isApprox(q1)) {
error = true;
std::cout << "evaluate at t=max is not the final quaternion" << std::endl;
}
- if (so3Traj(1.5) != q1.toRotationMatrix()) {
+ if (so3Traj(tMax) != q1.toRotationMatrix()) {
error = true;
std::cout << "evaluate at t=max is not the final rotation" << std::endl;
}
// check derivatives :
- if (so3Traj.derivate(0, 1) != so3Traj.derivate(1., 1)) {
+ if (so3Traj.derivate(tMin, 1) != so3Traj.derivate(1., 1)) {
error = true;
std::cout << "first order derivative should be constant." << std::endl;
}
- if (so3Traj.derivate(0, 2) != point_t::Zero(3)) {
+ if (so3Traj.derivate(tMin, 2) != point3_t::Zero(3)) {
error = true;
std::cout << "second order derivative should be null" << std::endl;
}
- point3_t angular_vel = so3Traj.derivate(0, 1);
+ point3_t angular_vel = so3Traj.derivate(tMin, 1);
if(angular_vel[1] != 0. || angular_vel[2] != 0){
error = true;
std::cout << "Angular velocity around y and z axis should be null" << std::endl;
@@ -1682,6 +1684,10 @@ void so3LinearTest(bool& error) {
std::cout << "SO3Linear: calling derivate with order = 0 should raise an invalid_argument error" << std::endl;
} catch (std::invalid_argument e) {
}
+
+ SO3Linear_t so3TrajMatrix(q0.toRotationMatrix(), q1.toRotationMatrix(), tMin, tMax);
+ std::string errmsg("SO3Linear built from quaternion or from matrix are not identical.");
+ CompareCurves(so3Traj,so3TrajMatrix,errmsg,error,1e-3);
}
void SO3serializationTest(bool &error){
--
GitLab