From 12bd089479820cee9d3d524460fc1c62f9350817 Mon Sep 17 00:00:00 2001
From: Hancheol Choi <babchol@gmail.com>
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()