From 12bd089479820cee9d3d524460fc1c62f9350817 Mon Sep 17 00:00:00 2001 From: Hancheol Choi Date: Sat, 24 Dec 2022 11:09:11 +0900 Subject: [PATCH] Fix trlog exceptional condition For the rotation part of trlog, the iseye condition does not catch all conditions where the rotation value is near-zero. I actually got divided-by-zero exception in the general case part while doing some simulation. There are many ways to avoid divided-by-zero exception when we calculate skw = ... / math.sin(theta). But, I recommend you to fix iseye routine. Instead of iseye routine, we can simply check if trace(R) = 1 + 2cos(theta=0) = 3 --- spatialmath/base/transforms3d.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/spatialmath/base/transforms3d.py b/spatialmath/base/transforms3d.py index e5857758..020a5f54 100644 --- a/spatialmath/base/transforms3d.py +++ b/spatialmath/base/transforms3d.py @@ -1221,13 +1221,14 @@ def trlog(T, check=True, twist=False, tol=10): elif isrot(T, check=check): # deal with rotation matrix R = T - if smb.iseye(R): + trace_R = np.trace(R) + if abs(trace_R - 3) < tol * _eps: # matrix is identity if twist: return np.zeros((3,)) else: return np.zeros((3, 3)) - elif abs(np.trace(R) + 1) < tol * _eps: + elif abs(trace_R + 1) < tol * _eps: # check for trace = -1 # rotation by +/- pi, +/- 3pi etc. diagonal = R.diagonal()