diff --git a/spatialmath/base/transforms2d.py b/spatialmath/base/transforms2d.py index 6aad235c..664a9d6c 100644 --- a/spatialmath/base/transforms2d.py +++ b/spatialmath/base/transforms2d.py @@ -549,7 +549,7 @@ def trlog2( else: st = T[1, 0] ct = T[0, 0] - theta = math.atan(st / ct) + theta = math.atan2(st, ct) if abs(theta) < tol * _eps: tr = T[:2, 2].flatten() else: @@ -564,7 +564,7 @@ def trlog2( elif isrot2(T, check=check, tol=tol): # SO(2) rotation matrix - theta = math.atan(T[1, 0] / T[0, 0]) + theta = math.atan2(T[1, 0], T[0, 0]) if twist: return theta else: diff --git a/tests/base/test_transforms2d.py b/tests/base/test_transforms2d.py index bf578b22..fcd939bd 100755 --- a/tests/base/test_transforms2d.py +++ b/tests/base/test_transforms2d.py @@ -12,7 +12,7 @@ import unittest from math import pi import math -from scipy.linalg import logm +from scipy.linalg import expm, logm import pytest import sys @@ -88,6 +88,25 @@ def test_trlog2(self): nt.assert_array_almost_equal(trlog2(T, twist=True), vexa(logm(T))) + def test_trlog2_quadrants(self): + for c, s in ( + (1, 0), + (math.sqrt(3) / 2, 0.5), + (-0.5, math.sqrt(3) / 2), + (-0.5, -math.sqrt(3) / 2), + (-1, 0), + (0, 1), + (0, -1), + ): + R = np.array([[c, -s], [s, c]]) + for T in (R, rt2tr(R, [1, 2])): + for twist in (False, True): + with self.subTest(c=c, s=s, shape=T.shape, twist=twist): + S = trlog2(T, twist=twist) + if twist: + S = skew(S) if T.shape == (2, 2) else skewa(S) + nt.assert_allclose(expm(S), T, atol=1e-12, rtol=0) + def test_trexp2(self): R = trexp2(skew(0.5)) nt.assert_array_almost_equal(R, rot2(0.5))