airo-spatial-algebra 2025.4.0__tar.gz

airo_spatial_algebra-2025.4.0/PKG-INFO

@@ -0,0 +1,14 @@
+Metadata-Version: 2.4
+Name: airo_spatial_algebra
+Version: 2025.4.0
+Summary: Code for working with SE3 poses, transforms... for robotic manipulation at the Ghent University AI and Robotics Lab
+Author: Thomas Lips
+Author-email: thomas.lips@ugent.be
+Requires-Dist: numpy<2.0
+Requires-Dist: scipy
+Requires-Dist: spatialmath-python
+Requires-Dist: airo-typing==2025.4.0
+Dynamic: author
+Dynamic: author-email
+Dynamic: requires-dist
+Dynamic: summary

airo_spatial_algebra-2025.4.0/README.md

@@ -0,0 +1,7 @@
+# airo-spatial-algebra
+
+This package provides functionality to work with SE3 poses, transforms etc. The heavy lifting is done by Peter Corke's [spatial-math](https://github.com/petercorke/spatialmath-python) package. We simply wrap a subset of its features to make it more verbose and self-explanatory.
+
+The package contains an `SE3Container` class for converting between different representations of SE3 poses (and hence SO3 rotations as well).
+Furthermore a few common operations on points and poses (such as changing the frame in which they are represented) are provided for convenience.
+

airo_spatial_algebra-2025.4.0/airo_spatial_algebra/__init__.py

@@ -0,0 +1,4 @@
+from airo_spatial_algebra.operations import transform_points
+from airo_spatial_algebra.se3 import SE3Container
+
+__all__ = ["SE3Container", "transform_points"]

airo_spatial_algebra-2025.4.0/airo_spatial_algebra/operations.py

@@ -0,0 +1,82 @@
+"""
+This file contains some operations on points and poses.
+
+It also defines a helper class for a collection of points, but this is only used internally
+to allow the user to directly interact with np.arrays to reduce friction.
+
+i.e. the user can  call <transform>(points) where points is just a numpy array,
+    instead of having to first convert the points to a specific format.
+"""
+
+import numpy as np
+from airo_typing import HomogeneousMatrixType, Vector3DArrayType, Vectors3DType
+
+
+class _HomogeneousPoints:
+    """Helper class to facilitate multiplicating 4x4 matrices with one or more 3D points.
+    This class internally handles the addition / removal of a dimension to the points.
+    """
+
+    # TODO: extend to generic dimensions (1D,2D,3D).
+    def __init__(self, points: Vectors3DType):
+        if not self.is_valid_points_type(points):
+            raise ValueError(f"Invalid argument for {_HomogeneousPoints.__name__}.__init__ ")
+
+        points = _HomogeneousPoints.ensure_array_2d(points)
+        self._homogeneous_points = np.concatenate([points, np.ones((points.shape[0], 1), dtype=np.float32)], axis=1)
+
+    @staticmethod
+    def is_valid_points_type(points: Vectors3DType) -> bool:
+        if len(points.shape) == 1:
+            if len(points) == 3:
+                return True
+        elif len(points.shape) == 2:
+            if points.shape[1] == 3:
+                return True
+        return False
+
+    @staticmethod
+    def ensure_array_2d(points: Vectors3DType) -> Vector3DArrayType:
+        """If points is a single shape (3,) point, then it is reshaped to (1,3)."""
+        if len(points.shape) == 1:
+            if len(points) != 3:
+                raise ValueError("points has only one dimension, but it's length is not 3")
+            points = points.reshape((1, 3))
+        return points
+
+    @property
+    def homogeneous_points(self) -> np.ndarray:
+        """Nx4 matrix representing the homogeneous points"""
+        return self._homogeneous_points
+
+    @property
+    def points(self) -> Vectors3DType:
+        """Nx3 matrix representing the points"""
+        # normalize points (for safety, should never be necessary with affine transforms)
+        # but we've had bugs of this type with projection operations, so better safe than sorry?
+        scalars = self._homogeneous_points[:, 3][:, np.newaxis]
+        points = self.homogeneous_points[:, :3] / scalars
+        # TODO: if the original poitns was (1,3) matrix, then the resulting points would be a (3,) vector.
+        #  Is this desirable? and if not, how to avoid it?
+        if points.shape[0] == 1:
+            # single point -> create vector from 1x3 matrix
+            return points[0]
+        else:
+            return points
+
+    def apply_transform(self, homogeneous_transform_matrix: HomogeneousMatrixType) -> None:
+        self._homogeneous_points = (homogeneous_transform_matrix @ self.homogeneous_points.transpose()).transpose()
+
+
+def transform_points(homogeneous_transform_matrix: HomogeneousMatrixType, points: Vectors3DType) -> Vectors3DType:
+    """Applies a transform to a (set of) point(s).
+
+    Args:
+        homogeneous_transform_matrix (HomogeneousMatrixType): _description_
+        points (PointsType): _description_
+    Returns:
+        PointsType: (3,) vector or (N,3) matrix.
+    """
+    homogeneous_points = _HomogeneousPoints(points)
+    homogeneous_points.apply_transform(homogeneous_transform_matrix)
+    return homogeneous_points.points

airo_spatial_algebra-2025.4.0/airo_spatial_algebra/se3.py

@@ -0,0 +1,214 @@
+from __future__ import annotations
+
+from typing import Optional  # use class as type for class methods
+
+import numpy as np
+from airo_typing import (
+    AxisAngleType,
+    EulerAnglesType,
+    HomogeneousMatrixType,
+    QuaternionType,
+    RotationMatrixType,
+    RotationVectorType,
+    Vector3DType,
+)
+from scipy.spatial.transform import Rotation
+from spatialmath import SE3, UnitQuaternion
+from spatialmath.base import trnorm
+
+
+class SE3Container:
+    """A container class for SE3 elements. These elements are used to represent the 3D Pose of an element A in a frame B,
+    or stated differently the transform from frame B to frame A.
+
+    Conventions:
+    translations are in meters,rotations in radians.
+    quaternions are scalar-last and normalized.
+    euler angles are the angles of consecutive rotations around the original X-Y-Z axis (in that order).
+
+    Note that ther exist many different types of euler angels that differ in the order of axes,
+    and in whether they rotate around the original and the new axes. We chose this convention as it is the most common in robotics
+    and also easy to reason about. use the Scipy.transform.Rotation class if you need to convert from/to other formats.
+
+    This is a wrapper around the SE3 class of Peter Corke's Spatial Math Library: https://petercorke.github.io/spatialmath-python/
+    The scope if this class is not to perform arbitrary calculations on SE3 elements,
+    it is merely a 'simplified and more readable' wrapper
+    that facilitates creating/retrieving position and/or orientations in various formats.
+
+    If you need support for calculations and/or more ways to create SE3 elements, use Peter Corke's Spatial Math Library directly.
+    You can decide this on the fly as you can always access the SE3 attribute of this class or instantiate this class from an SE3 object
+    """
+
+    def __init__(self, se3: SE3) -> None:  # type: ignore
+        self.se3 = se3
+
+    @classmethod
+    def random(cls) -> SE3Container:
+        """A random SE3 element with translations in the [-1,1]^3 cube."""
+        return cls(SE3.Rand())
+
+    @classmethod
+    def from_translation(cls, translation: Vector3DType) -> SE3Container:
+        """creates a translation-only SE3 element"""
+        return cls(SE3.Trans(translation.tolist()))
+
+    @classmethod
+    def from_homogeneous_matrix(cls, matrix: HomogeneousMatrixType) -> SE3Container:
+        _assert_is_se3_matrix(matrix)
+        return cls(SE3(matrix))
+
+    @classmethod
+    def from_rotation_matrix_and_translation(
+        cls, rotation_matrix: RotationMatrixType, translation: Optional[Vector3DType] = None
+    ) -> SE3Container:
+        _assert_is_so3_matrix(rotation_matrix)
+        return cls(SE3.Rt(rotation_matrix, translation))
+
+    @classmethod
+    def from_rotation_vector_and_translation(
+        cls, rotation_vector: RotationVectorType, translation: Optional[Vector3DType] = None
+    ) -> SE3Container:
+        return cls(SE3.Rt(Rotation.from_rotvec(rotation_vector).as_matrix(), translation))
+
+    @classmethod
+    def from_quaternion_and_translation(
+        cls, quaternion: QuaternionType, translation: Optional[Vector3DType] = None
+    ) -> SE3Container:
+        q = UnitQuaternion(quaternion[3], quaternion[:3])  # scalar-first in math lib
+        return cls(SE3.Rt(q.R, translation))
+
+    @classmethod
+    def from_euler_angles_and_translation(
+        cls, euler_angels: EulerAnglesType, translation: Optional[Vector3DType] = None
+    ) -> SE3Container:
+        # convert from extrinsic XYZ to rotmatrix
+        # bc SE3.Eul does not accept translation
+        rot_matrix = Rotation.from_euler("xyz", euler_angels, degrees=False).as_matrix()
+        return cls.from_rotation_matrix_and_translation(rot_matrix, translation)
+
+    @classmethod
+    def from_orthogonal_base_vectors_and_translation(
+        cls,
+        x_axis: Vector3DType,
+        y_axis: Vector3DType,
+        z_axis: Vector3DType,
+        translation: Optional[Vector3DType] = None,
+    ) -> SE3Container:
+        # create orientation matrix with base vectors as columns
+        orientation_matrix = np.zeros((3, 3))
+        for i, axis in enumerate([x_axis, y_axis, z_axis]):
+            orientation_matrix[:, i] = axis / np.linalg.norm(axis)
+
+        _assert_is_so3_matrix(orientation_matrix)
+
+        return cls(SE3.Rt(orientation_matrix, translation))
+
+    @property
+    def orientation_as_quaternion(self) -> QuaternionType:
+        angle, vec = self.se3.angvec()
+        scalar_first_quaternion = UnitQuaternion.AngVec(angle, vec).A
+        return self.scalar_first_quaternion_to_scalar_last(scalar_first_quaternion)
+
+    @property
+    def orientation_as_euler_angles(self) -> EulerAnglesType:
+        zyx_ordered_angles = self.se3.eul()
+        # convert from intrinsic ZYZ  to extrinsic xyz
+        return Rotation.from_euler("ZYZ", zyx_ordered_angles, degrees=False).as_euler("xyz", degrees=False)
+
+    @property
+    def orientation_as_axis_angle(self) -> AxisAngleType:
+        angle, axis = self.se3.angvec()
+        return axis.astype(np.float64), float(angle)
+
+    @property
+    def orientation_as_rotation_vector(self) -> Vector3DType:
+        axis, angle = self.orientation_as_axis_angle
+        if axis is None:
+            return np.zeros(3)
+        return angle * axis
+
+    @property
+    def rotation_matrix(self) -> RotationMatrixType:
+        return self.se3.R
+
+    @property
+    def homogeneous_matrix(self) -> HomogeneousMatrixType:
+        return self.se3.A
+
+    @property
+    def translation(self) -> Vector3DType:
+        # TODO: should this be named position or translation?
+        return self.se3.t
+
+    @property
+    def x_axis(self) -> Vector3DType:
+        """also called normal vector. This is the first column of the rotation matrix"""
+        return self.se3.n
+
+    @property
+    def y_axis(self) -> Vector3DType:
+        """also colled orientation vector. This is the second column of the rotation matrix"""
+        return self.se3.o
+
+    @property
+    def z_axis(self) -> Vector3DType:
+        """also called approach vector. This is the third column of the rotation matrix"""
+        return self.se3.a
+
+    def __str__(self) -> str:
+        return str(f"SE3 -> \n {self.homogeneous_matrix}")
+
+    @staticmethod
+    def scalar_first_quaternion_to_scalar_last(scalar_first_quaternion: np.ndarray) -> QuaternionType:
+        scalar_last_quaternion = np.roll(scalar_first_quaternion, -1)
+        return scalar_last_quaternion
+
+    @staticmethod
+    def scalar_last_quaternion_to_scalar_first(scalar_last_quaternion: QuaternionType) -> np.ndarray:
+        scalar_first_quaternion = np.roll(scalar_last_quaternion, 1)
+        return scalar_first_quaternion
+
+
+def normalize_so3_matrix(matrix: np.ndarray) -> np.ndarray:
+    """normalize an SO3 matrix (i.e. a rotation matrix) to be orthogonal and have determinant 1 (right-handed coordinate system)
+    see https://en.wikipedia.org/wiki/3D_rotation_group
+
+    Can be used to fix numerical issues with rotation matrices
+
+    will make sure x,y,z are unit vectors, then
+    will construct new x vector as y cross z, then construct new y vector as z cross x, so that x,y,z are orthogonal
+
+    """
+    assert matrix.shape == (3, 3), "matrix is not a 3x3 matrix"
+    return trnorm(matrix)
+
+
+def _assert_is_so3_matrix(matrix: np.ndarray) -> None:
+    """check if matrix is a valid SO3 matrix
+    this requires the matrix to be orthogonal (base vectors are perpendicular) and have determinant 1 (right-handed coordinate system)
+    see https://en.wikipedia.org/wiki/3D_rotation_group
+
+    This function will raise a ValueError if the matrix is not valid
+
+    """
+    if matrix.shape != (3, 3):
+        raise ValueError("matrix is not a 3x3 matrix")
+    if not np.allclose(matrix @ matrix.T, np.eye(3)):
+        raise ValueError(
+            "matrix is not orthnormal, i.e. its base vectors are not perpendicular. If you are sure this is a numerical issue, use normalize_so3_matrix()"
+        )
+    if not np.allclose(np.linalg.det(matrix), 1):
+        raise ValueError("matrix does not have determinant 1 (not right-handed)")
+
+
+def _assert_is_se3_matrix(matrix: np.ndarray) -> None:
+    """check if matrix is a valid SE3 matrix (i.e. a valid pose)
+    this requires the rotation part to be a valid SO3 matrix and the translation part to be a 3D vector
+
+    This function will raise a ValueError if the matrix is not valid
+    """
+    if matrix.shape != (4, 4):
+        raise ValueError("matrix is not a 4x4 matrix")
+    if not np.allclose(matrix[3, :], np.array([0, 0, 0, 1])):
+        raise ValueError("last row of matrix is not [0,0,0,1]")
+    _assert_is_so3_matrix(matrix[:3, :3])

airo_spatial_algebra-2025.4.0/airo_spatial_algebra.egg-info/PKG-INFO

@@ -0,0 +1,14 @@
+Metadata-Version: 2.4
+Name: airo_spatial_algebra
+Version: 2025.4.0
+Summary: Code for working with SE3 poses, transforms... for robotic manipulation at the Ghent University AI and Robotics Lab
+Author: Thomas Lips
+Author-email: thomas.lips@ugent.be
+Requires-Dist: numpy<2.0
+Requires-Dist: scipy
+Requires-Dist: spatialmath-python
+Requires-Dist: airo-typing==2025.4.0
+Dynamic: author
+Dynamic: author-email
+Dynamic: requires-dist
+Dynamic: summary

airo_spatial_algebra-2025.4.0/airo_spatial_algebra.egg-info/SOURCES.txt

@@ -0,0 +1,13 @@
+README.md
+setup.py
+airo_spatial_algebra/__init__.py
+airo_spatial_algebra/operations.py
+airo_spatial_algebra/py.typed
+airo_spatial_algebra/se3.py
+airo_spatial_algebra.egg-info/PKG-INFO
+airo_spatial_algebra.egg-info/SOURCES.txt
+airo_spatial_algebra.egg-info/dependency_links.txt
+airo_spatial_algebra.egg-info/requires.txt
+airo_spatial_algebra.egg-info/top_level.txt
+test/test_operations.py
+test/test_se3_container.py

airo_spatial_algebra-2025.4.0/airo_spatial_algebra.egg-info/dependency_links.txt

@@ -0,0 +1 @@
+

airo_spatial_algebra-2025.4.0/airo_spatial_algebra.egg-info/requires.txt

@@ -0,0 +1,4 @@
+numpy<2.0
+scipy
+spatialmath-python
+airo-typing==2025.4.0

airo_spatial_algebra-2025.4.0/airo_spatial_algebra.egg-info/top_level.txt

@@ -0,0 +1 @@
+airo_spatial_algebra

airo_spatial_algebra-2025.4.0/setup.cfg

@@ -0,0 +1,4 @@
+[egg_info]
+tag_build = 
+tag_date = 0
+

airo_spatial_algebra-2025.4.0/setup.py

@@ -0,0 +1,17 @@
+import pathlib
+
+import setuptools
+
+root_folder = pathlib.Path(__file__).parents[1]
+setuptools.setup(
+    name="airo_spatial_algebra",
+    version="2025.4.0",
+    description="Code for working with SE3 poses, transforms... for robotic manipulation at the Ghent University AI and Robotics Lab",
+    author="Thomas Lips",
+    author_email="thomas.lips@ugent.be",
+    install_requires=["numpy<2.0", "scipy", "spatialmath-python", "airo-typing==2025.4.0"],
+    packages=setuptools.find_packages(exclude=["test"]),
+    # include py.typed to declare type information is available, see
+    # https://mypy.readthedocs.io/en/stable/installed_packages.html#making-pep-561-compatible-packages
+    package_data={"airo_spatial_algebra": ["py.typed"]},
+)

airo_spatial_algebra-2025.4.0/test/test_operations.py

@@ -0,0 +1,39 @@
+import numpy as np
+import pytest
+from airo_spatial_algebra.operations import _HomogeneousPoints, transform_points
+from airo_spatial_algebra.se3 import SE3Container
+
+
+def test_helper_class_creation():
+    point = np.array([1.0, 2, 3])
+    hpoints = _HomogeneousPoints(point)
+    assert hpoints._homogeneous_points.shape == (1, 4)
+    assert hpoints._homogeneous_points[0, -1] == 1.0
+
+    homogeneous_point = np.array([1, 2, 3, 1])
+    with pytest.raises(ValueError):
+        _HomogeneousPoints(homogeneous_point)
+
+    points = np.arange(6).reshape(2, 3)
+    hpoints = _HomogeneousPoints(points)
+    assert hpoints._homogeneous_points.shape == (2, 4)
+    # check that the scale is 1.0
+    assert hpoints._homogeneous_points[0, -1] == 1.0
+
+    wronglyshaped_points = np.arange(6).reshape(3, 2)
+    with pytest.raises(ValueError):
+        _HomogeneousPoints(wronglyshaped_points)
+
+
+@pytest.mark.parametrize("points", [np.arange(6).astype(np.float32).reshape(2, 3), np.array([1.0, 2, 3])])
+def test_helper_class_properties(points):
+    hpoints = _HomogeneousPoints(points)
+    assert np.isclose(points, hpoints.points).all()
+    assert hpoints.homogeneous_points.shape == (points.size // 3, 4)
+
+
+def test_transform_points():
+    points = np.arange(6).astype(np.float32).reshape(2, 3)
+    transform = SE3Container.random()
+    transformed_points = transform_points(transform.homogeneous_matrix, points)
+    assert np.isclose(transformed_points[0], transform.rotation_matrix @ points[0] + transform.translation).all()

airo_spatial_algebra-2025.4.0/test/test_se3_container.py

@@ -0,0 +1,132 @@
+import numpy as np
+import pytest
+from airo_spatial_algebra import SE3Container
+from airo_spatial_algebra.se3 import _assert_is_so3_matrix, normalize_so3_matrix
+from scipy.spatial.transform import Rotation
+from spatialmath import SE3, SO3
+
+
+@pytest.fixture(autouse=True)
+def seed():
+    """fixture that will run before each test in this module to make them deterministic.
+    Random poses etc are generated with spatialmath,
+    which usesnp.random"""
+    np.random.seed(2022)
+
+
+def test_from_translation_and_homogeneous():
+    pose = np.eye(4)
+    translation = np.array([1, 2, 3.0])
+    pose[:3, 3] = translation
+    se3 = SE3Container.from_translation(translation)
+    assert np.isclose(se3.homogeneous_matrix, pose).all()
+
+
+def test_from_hom_matrix():
+    pose = SE3.Rand().A
+    se3 = SE3Container.from_homogeneous_matrix(pose)
+    assert np.isclose(se3.homogeneous_matrix, pose).all()
+
+
+def test_from_bad_hom_matrix():
+    pose = SE3.Rand().A
+    pose[3, 3] += 0.01
+    with pytest.raises(ValueError):
+        SE3Container.from_homogeneous_matrix(pose)
+
+
+def test_from_rot_mat():
+    # no translation
+    rot_matrix = SO3.Rand().R
+    pose = np.eye(4)
+    pose[:3, :3] = rot_matrix
+    se3 = SE3Container.from_rotation_matrix_and_translation(rot_matrix)
+    assert np.isclose(se3.homogeneous_matrix, pose).all()
+
+    # with translation
+    trans = np.array([1, 2, 3.2])
+    pose[:3, 3] = trans
+    se3 = SE3Container.from_rotation_matrix_and_translation(rot_matrix, trans)
+    assert np.isclose(se3.homogeneous_matrix, pose).all()
+
+
+def test_from_bad_rot_mat():
+    rot_matrix = SO3.Rand().R
+    rot_matrix[0, 0] += 0.01
+    with pytest.raises(ValueError):
+        SE3Container.from_rotation_matrix_and_translation(rot_matrix)
+
+
+def test_from_base_vectors():
+    rot_matrix = np.eye(3)
+    se3 = SE3Container.from_orthogonal_base_vectors_and_translation(
+        rot_matrix[:, 0], rot_matrix[:, 1], rot_matrix[:, 2], np.array([1, 2, 3])
+    )
+    assert np.isclose(se3.rotation_matrix, rot_matrix).all()
+
+
+def test_from_rotation_vector():
+    angle, axis = SO3.Rand().angvec()
+    rot_vec = angle * axis
+    se3 = SE3Container.from_rotation_vector_and_translation(rot_vec)
+    assert np.isclose(se3.orientation_as_rotation_vector, rot_vec).all()
+
+
+def test_quaternions():
+    quat = [0, 0, 0, 1.0]  # no rotation scalar-last
+    se3 = SE3Container.from_quaternion_and_translation(quat)
+    assert np.isclose(se3.rotation_matrix, np.eye(3)).all()
+    assert np.isclose(se3.translation, np.zeros(3)).all()
+    assert np.isclose(quat, se3.orientation_as_quaternion).all()
+
+
+def test_euler():
+    euler = [np.pi, np.pi / 4, np.pi / 5]  # 90 degs of X
+    rot_matrix = Rotation.from_euler("xyz", euler).as_matrix()
+    se3 = SE3Container.from_euler_angles_and_translation(euler)
+    assert np.isclose(se3.rotation_matrix, rot_matrix).all()
+    assert np.isclose(se3.orientation_as_euler_angles, euler).all()
+
+
+def test_get_axes_functions():
+    se3 = SE3Container.random()
+    hom_matrix = se3.homogeneous_matrix
+    for i, atrr in enumerate([se3.x_axis, se3.y_axis, se3.z_axis]):
+        assert np.isclose(atrr, hom_matrix[:3, i]).all()
+
+
+def test_repr():
+    se3 = SE3Container.random()
+    print(se3)
+
+
+def test_all_orientation_reprs_are_equivalent():
+    se3 = SE3Container.random()
+    rot_matrix = se3.rotation_matrix
+    assert np.isclose(Rotation.from_rotvec(se3.orientation_as_rotation_vector).as_matrix(), rot_matrix).all()
+    assert np.isclose(Rotation.from_euler("xyz", se3.orientation_as_euler_angles).as_matrix(), rot_matrix).all()
+    assert np.isclose(Rotation.from_quat(se3.orientation_as_quaternion).as_matrix(), rot_matrix).all()
+
+
+def test_quaternion_scalar_conversion():
+    se3 = SE3Container.random()
+    quat = se3.orientation_as_quaternion
+    assert np.isclose(
+        quat,
+        SE3Container.scalar_first_quaternion_to_scalar_last(SE3Container.scalar_last_quaternion_to_scalar_first(quat)),
+    ).all()
+
+
+def test_axis_angle_dtypes():
+    se3 = SE3Container.from_homogeneous_matrix(np.identity(4))
+    axis, angle = se3.orientation_as_axis_angle
+    assert isinstance(angle, float)
+    assert isinstance(axis, np.ndarray)
+    assert axis.dtype == np.float64
+
+
+def test_normalize_so3():
+    so3 = SO3.Rand().R
+    so3[0, 0] += 0.01
+    normalized_so3 = normalize_so3_matrix(so3)
+    _assert_is_so3_matrix(normalized_so3)