From 3f4e64cc383971c31a7618f4d69416c16b7b29f1 Mon Sep 17 00:00:00 2001 From: Carlos Mastalli Date: Thu, 31 Aug 2023 09:49:31 +0100 Subject: [PATCH] [bugfix] Casting 1e-2 in SO log function --- src/multibody/liegroup/special-orthogonal.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/multibody/liegroup/special-orthogonal.hpp b/src/multibody/liegroup/special-orthogonal.hpp index ad6cd825ca..9884de93b3 100644 --- a/src/multibody/liegroup/special-orthogonal.hpp +++ b/src/multibody/liegroup/special-orthogonal.hpp @@ -71,7 +71,7 @@ namespace pinocchio if_then_else(internal::LT, tr, Scalar(-2), if_then_else(internal::GE, R (1, 0), Scalar(0), PI_value, -PI_value), // then - if_then_else(internal::GT, tr, Scalar(2) - 1e-2, + if_then_else(internal::GT, tr, Scalar(2) - Scalar(1e-2), asin((R(1,0) - R(0,1)) / Scalar(2)), // then if_then_else(internal::GE, R (1, 0), Scalar(0), acos(tr/Scalar(2)), // then