Add assert_exponential_coordinates_equal

doc/source/api.rst

@@ -328,6 +328,7 @@ Exponential Coordinates
    ~exponential_coordinates_from_transform
    ~exponential_coordinates_from_screw_axis
    ~exponential_coordinates_from_transform_log
+   ~assert_exponential_coordinates_equal
 
 Screw Matrix
 ------------

pytransform3d/test/test_transformations.py

@@ -252,6 +252,8 @@ def test_norm_exponential_coordinates():
         Stheta_only_translation)
     assert_array_almost_equal(
         Stheta_only_translation, Stheta_only_translation2)
+    pt.assert_exponential_coordinates_equal(
+        Stheta_only_translation, Stheta_only_translation2)
 
     rng = np.random.default_rng(381)
 
@@ -265,15 +267,18 @@ def test_norm_exponential_coordinates():
         assert_array_almost_equal(
             pt.transform_from_exponential_coordinates(Stheta),
             pt.transform_from_exponential_coordinates(Stheta2))
-        assert_array_almost_equal(pt.norm_exponential_coordinates(Stheta),
-                                  pt.norm_exponential_coordinates(Stheta2))
+        assert_array_almost_equal(
+            pt.norm_exponential_coordinates(Stheta),
+            pt.norm_exponential_coordinates(Stheta2))
+        pt.assert_exponential_coordinates_equal(Stheta, Stheta2)
 
     for _ in range(10):
         Stheta = rng.standard_normal(size=6)
         # ensure that theta is not within [-pi, pi]
         Stheta[rng.integers(0, 3)] += np.pi + rng.random()
         Stheta_norm = pt.norm_exponential_coordinates(Stheta)
         assert not np.all(Stheta == Stheta_norm)
+        pt.assert_exponential_coordinates_equal(Stheta, Stheta_norm)
 
         A2B = pt.transform_from_exponential_coordinates(Stheta)
         Stheta2 = pt.exponential_coordinates_from_transform(A2B)

pytransform3d/transformations/__init__.py

@@ -36,8 +36,9 @@
     random_transform, random_screw_axis, random_exponential_coordinates)
 from ._plot import plot_transform, plot_screw
 from ._testing import (
-    assert_transform, assert_screw_parameters_equal,
-    assert_unit_dual_quaternion_equal, assert_unit_dual_quaternion)
+    assert_transform, assert_exponential_coordinates_equal,
+    assert_screw_parameters_equal, assert_unit_dual_quaternion_equal,
+    assert_unit_dual_quaternion)
 from ._jacobians import (
     left_jacobian_SE3, left_jacobian_SE3_series, left_jacobian_SE3_inv,
     left_jacobian_SE3_inv_series)
@@ -74,8 +75,9 @@
     "dq_q_conj", "dq_conj", "concatenate_dual_quaternions",
     "dual_quaternion_sclerp", "dual_quaternion_power", "dq_prod_vector",
     "plot_transform", "plot_screw",
-    "assert_transform", "assert_screw_parameters_equal",
-    "assert_unit_dual_quaternion_equal", "assert_unit_dual_quaternion",
+    "assert_transform", "assert_exponential_coordinates_equal",
+    "assert_screw_parameters_equal", "assert_unit_dual_quaternion_equal",
+    "assert_unit_dual_quaternion",
     "left_jacobian_SE3", "left_jacobian_SE3_series", "left_jacobian_SE3_inv",
     "left_jacobian_SE3_inv_series"
 ]

pytransform3d/transformations/_testing.py

@@ -4,6 +4,7 @@
 from ..rotations import assert_rotation_matrix, norm_angle, eps
 from ._dual_quaternion_operations import (
     dq_q_conj, concatenate_dual_quaternions)
+from ._conversions import norm_exponential_coordinates
 
 
 def assert_transform(A2B, *args, **kwargs):
@@ -30,6 +31,36 @@ def assert_transform(A2B, *args, **kwargs):
                               *args, **kwargs)
 
 
+def assert_exponential_coordinates_equal(Stheta1, Stheta2):
+    """Raise an assertion if exp. coordinates are not approximately equal.
+
+    Parameters
+    ----------
+    Stheta1 : array-like, shape (6,)
+        Exponential coordinates of transformation:
+        S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,
+        where the first 3 components are related to rotation and the last 3
+        components are related to translation.
+
+    Stheta2 : array-like, shape (6,)
+        Exponential coordinates of transformation:
+        S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,
+        where the first 3 components are related to rotation and the last 3
+        components are related to translation.
+
+    args : tuple
+        Positional arguments that will be passed to
+        `assert_array_almost_equal`
+
+    kwargs : dict
+        Positional arguments that will be passed to
+        `assert_array_almost_equal`
+    """
+    Stheta1 = norm_exponential_coordinates(Stheta1)
+    Stheta2 = norm_exponential_coordinates(Stheta2)
+    assert_array_almost_equal(Stheta1, Stheta2)
+
+
 def assert_unit_dual_quaternion(dq, *args, **kwargs):
     """Raise an assertion if the dual quaternion does not have unit norm.
 

pytransform3d/transformations/_testing.pyi

@@ -5,6 +5,9 @@ import numpy.typing as npt
 def assert_transform(A2B: npt.ArrayLike, *args, **kwargs): ...
 
 
+def assert_exponential_coordinates_equal(Stheta1: npt.ArrayLike, Stheta2: npt.ArrayLike): ...
+
+
 def assert_unit_dual_quaternion(dq: npt.ArrayLike, *args, **kwargs): ...