From 570413c5c52006bb5d4febe97d9982a16cf674dc Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Tue, 6 Sep 2016 10:47:26 +0200
Subject: [PATCH] [Doc] Add developer doc to
JointModelFreeFlyer::integrate_impl
---
src/multibody/joint/joint-free-flyer.hpp | 2 ++
1 file changed, 2 insertions(+)
diff --git a/src/multibody/joint/joint-free-flyer.hpp b/src/multibody/joint/joint-free-flyer.hpp
index a6d9b99ac..488a80b9a 100644
--- a/src/multibody/joint/joint-free-flyer.hpp
+++ b/src/multibody/joint/joint-free-flyer.hpp
@@ -276,6 +276,8 @@ namespace se3
res.head<3>() = M1.translation();
QuaternionMap_t res_quat(res.tail<4>().data());
res_quat = M1.rotation();
+ // Norm of qs might be epsilon-different to 1, so M1.rotation might be epsilon-different to a rotation matrix.
+ // It is then safer to re-normalized after converting M1.rotation to quaternion.
firstOrderNormalize(res_quat);
return res;
--
GitLab