From a21c4db0321bd81a4db323a531d75860de0739c2 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Thu, 1 Sep 2016 11:06:21 +0200
Subject: [PATCH] [C++] Normalize (using 1-order DL) integration for FreeFlyer,
Spherical and Unbounded rotations
---
src/multibody/joint/joint-free-flyer.hpp | 1 +
src/multibody/joint/joint-revolute-unbounded.hpp | 9 +++++----
src/multibody/joint/joint-spherical.hpp | 1 +
3 files changed, 7 insertions(+), 4 deletions(-)
diff --git a/src/multibody/joint/joint-free-flyer.hpp b/src/multibody/joint/joint-free-flyer.hpp
index 01e493cb3..a880de1fe 100644
--- a/src/multibody/joint/joint-free-flyer.hpp
+++ b/src/multibody/joint/joint-free-flyer.hpp
@@ -276,6 +276,7 @@ namespace se3
res.head<3>() = M1.translation();
QuaternionMap_t res_quat(res.tail<4>().data());
res_quat = M1.rotation();
+ firstOrderNormalize(res_quat);
return res;
}
diff --git a/src/multibody/joint/joint-revolute-unbounded.hpp b/src/multibody/joint/joint-revolute-unbounded.hpp
index 3cd481f48..ec27b7249 100644
--- a/src/multibody/joint/joint-revolute-unbounded.hpp
+++ b/src/multibody/joint/joint-revolute-unbounded.hpp
@@ -160,10 +160,11 @@ namespace se3
double cosOmega,sinOmega; SINCOS(omega, &sinOmega, &cosOmega);
// TODO check the cost of atan2 vs SINCOS
- ConfigVector_t result;
- result <<
- cosOmega * ca - sinOmega * sa,
- sinOmega * ca + cosOmega * sa;
+ ConfigVector_t result (cosOmega * ca - sinOmega * sa,
+ sinOmega * ca + cosOmega * sa);
+ const double norm2 = q.squaredNorm();
+ result *= (3 - norm2) / 2;
+
return result;
}
diff --git a/src/multibody/joint/joint-spherical.hpp b/src/multibody/joint/joint-spherical.hpp
index 59bd5f65b..08d6f720f 100644
--- a/src/multibody/joint/joint-spherical.hpp
+++ b/src/multibody/joint/joint-spherical.hpp
@@ -336,6 +336,7 @@ namespace se3
Motion_t::Quaternion_t pOmega(se3::exp3(q_dot));
Motion_t::Quaternion_t quaternion_result(q*pOmega);
+ firstOrderNormalize(quaternion_result);
return quaternion_result.coeffs();
}
--
GitLab