diff --git a/.github/workflows/test-package.yml b/.github/workflows/test-package.yml index 2736e07..7dedc7a 100644 --- a/.github/workflows/test-package.yml +++ b/.github/workflows/test-package.yml @@ -10,7 +10,7 @@ jobs: strategy: fail-fast: false matrix: - python-version: ['3.8', '3.9', '3.10'] + python-version: ['3.9', '3.10', '3.11', '3.12', '3.13'] steps: - uses: actions/checkout@v2 - name: Set up Python ${{matrix.python-version}} diff --git a/pymlg/numpy/se3.py b/pymlg/numpy/se3.py index ea4b79c..e296946 100644 --- a/pymlg/numpy/se3.py +++ b/pymlg/numpy/se3.py @@ -165,6 +165,14 @@ def odot(b): X[0:3, 0:3] = SO3.odot(b[0:3]) X[0:3, 3:6] = b[3] * np.identity(3) return X + + @staticmethod + def ocircle(b): + b = np.array(b).ravel() + X = np.zeros((6, 4)) + X[3:6, -1] = b[0:3] + X[0:3, :3] = SO3.odot(b[0:3]) + return X @staticmethod def adjoint(T): diff --git a/tests/numpy/test_se3.py b/tests/numpy/test_se3.py new file mode 100644 index 0000000..cd25fdf --- /dev/null +++ b/tests/numpy/test_se3.py @@ -0,0 +1,24 @@ +""" +Perform any other group-specific tests that are not part of the standard tests. +""" + +from pymlg import SE3 as G +import numpy as np + +def test_odot(): + p = np.array([0.1, 0.2, 0.3, 0.4]) + x = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6]) + X = G.odot(p) + assert X.shape == (4, 6) + assert np.allclose(G.wedge(x) @ p, X @ x) + +def test_ocircle(): + p = np.array([0.1, 0.2, 0.3, 0.4]) + x = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6]) + X = G.ocircle(p) + assert X.shape == (6, 4) + assert np.allclose(p.T @ G.wedge(x), x.T @ X) + +if __name__ == "__main__": + test_odot() + test_ocircle()