Skip to content

Commit 12bd089

Browse files
authored
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
1 parent bef0c9e commit 12bd089

File tree

1 file changed

+3
-2
lines changed

1 file changed

+3
-2
lines changed

spatialmath/base/transforms3d.py

+3-2
Original file line numberDiff line numberDiff line change
@@ -1221,13 +1221,14 @@ def trlog(T, check=True, twist=False, tol=10):
12211221
elif isrot(T, check=check):
12221222
# deal with rotation matrix
12231223
R = T
1224-
if smb.iseye(R):
1224+
trace_R = np.trace(R)
1225+
if abs(trace_R - 3) < tol * _eps:
12251226
# matrix is identity
12261227
if twist:
12271228
return np.zeros((3,))
12281229
else:
12291230
return np.zeros((3, 3))
1230-
elif abs(np.trace(R) + 1) < tol * _eps:
1231+
elif abs(trace_R + 1) < tol * _eps:
12311232
# check for trace = -1
12321233
# rotation by +/- pi, +/- 3pi etc.
12331234
diagonal = R.diagonal()

0 commit comments

Comments
 (0)