Skip to content

Commit a787a87

Browse files
committed
handle theta=0 case
1 parent 5735a6f commit a787a87

File tree

1 file changed

+6
-5
lines changed

1 file changed

+6
-5
lines changed

spatialmath/base/transforms2d.py

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -392,12 +392,13 @@ def trlog2(T, check=True, twist=False, tol=10):
392392
st = T[1,0]
393393
ct = T[0,0]
394394
theta = math.atan(st / ct)
395-
396-
V = np.array([[st, -(1-ct)], [1-ct, st]])
397-
tr = (np.linalg.inv(V) @ T[:2, 2]) * theta
398-
print(tr)
395+
if abs(theta) < tol * _eps:
396+
tr = T[:2, 2].flatten()
397+
else:
398+
V = np.array([[st, -(1-ct)], [1-ct, st]])
399+
tr = (np.linalg.inv(V) @ T[:2, 2]) * theta
399400
if twist:
400-
return np.array([tr, theta])
401+
return np.hstack([tr, theta])
401402
else:
402403
return np.block([
403404
[smb.skew(theta), tr[:, np.newaxis]],

0 commit comments

Comments
 (0)