Pose and Transform

.coveragerc

@@ -0,0 +1,3 @@
+[run]
+relative_files = true
+omit = ./example*, ./tests/*

.github/workflows/test.yml

@@ -2,22 +2,40 @@ name: Run Tests
 
 on: 
     push:
+    pull_request:
     workflow_dispatch:
 
 jobs:
   test:
-    name: Run Tests 🧪
+    name: Run Tests with coverage
     runs-on: ubuntu-latest
-
+    permissions:
+      contents: read
+      pull-requests: write
     steps:
-    - uses: actions/checkout@v4
-      with:
-        persist-credentials: false
-    - name: Set up Python
-      uses: actions/setup-python@v5
-      with:
-        python-version: "3.x"
-    - name: Install dependencies
-      run: python3 -m pip install .[test]
-    - name: Run tests
-      run: python3 -m pytest tests/*.py
+      - name: Checkout code
+        uses: actions/checkout@v4
+
+      - name: Set up Python
+        uses: actions/setup-python@v5
+        with:
+          python-version: '3.10'
+
+      - name: Install dependencies
+        run: |
+          python -m pip install --upgrade pip
+          pip install -e '.[test]'
+
+      - name: Run tests with coverage
+        run: |
+          python -m pytest --cov-report=xml tests/
+
+      - name: Comment coverage on PR
+        if: github.event_name == 'pull_request'
+        uses: py-cov-action/python-coverage-comment-action@v3
+        with:
+          GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
+          MINIMUM_GREEN: 80
+          MINIMUM_ORANGE: 70
+          ANNOTATE_MISSING_LINES: true
+          ANNOTATION_TYPE: notice

.gitignore

@@ -1,5 +1,10 @@
-*venv/
+# Generated 
 __pycache__/
 dist/
 
-.env
+# Environment
+*venv/
+.env
+
+# Output
+.coverage

.vscode/settings.json

@@ -0,0 +1,11 @@
+{
+    "python.testing.unittestArgs": [
+        "-v",
+        "-s",
+        "./tests",
+        "-p",
+        "*test*.py"
+    ],
+    "python.testing.pytestEnabled": false,
+    "python.testing.unittestEnabled": true
+}

README.md

@@ -1,21 +1,60 @@
-# Description
-FrameTransforms is a lightweight, native Python pacakge to simplify frame transformations. It supports:
+# frame-transforms
+`frame-transforms` is a lightweight, thread-safe, Python-native package to simplify frame transforms in robotics. With it, you can manage and translate between coordinate frames with ease. It features:
 
-1. Registration and update of relative coordinate frames.
-2. Automatic computation of transitive transformations.
-3. Multithreaded access.
+1. Automatic computation of transitive transforms.
+2. Registration and update of relative coordinate frames.
+3. An intuitive, object-oriented API. 
+
+Though in beta, the library is extensively tested.
+
+This package was inspired by the interface of `posetree` and shares much of its functionality but offers a more batteries-included experience. Similarly to [posetree](https://github.com/robobenjie/posetree?tab=readme-ov-file#philosophy-of-transforms-poses-and-frames)'s nomenclature, `Pose` is a location and orientation in space, whereas `Transform` is an action that describes the change in position and orientation to get from one `Pose` to another.
+
+## Installation
+
+```bash
+pip install frame-transforms
+```
 
 ## Application
 Consider a simple robot consisting of a mobile base and a camera mounted on a gimbal. 
 
-The camera detects an obstacle in its coordinate frame. Where is it in world frame?
+The camera detects an object in its coordinate frame. Where is it in world frame?
 
 ```python
-registry.update(Frame.WORLD, Frame.BASE, base_pose)
-registry.update(Frame.BASE, Frame.CAMERA, camera_pose)
+# Setup
+registry.update(Frame.WORLD, Frame.BASE, base_transform)
+registry.update(Frame.BASE, Frame.CAMERA, camera_transform)
+
+# Define the Pose
+object_pose = Pose(
+    Transform(
+        np.array([1, 0, 0]),
+        Rotation.from_euler("xyz", [0, 0, 0], degrees=True),
+    ),
+    parent_frame=Frame.CAMERA,
+    registry=registry,
+)
 
-# Locations are in homogenous coordinates
-obstacle_in_world = registry.get_transform(Frame.CAMERA, Frame.WORLD) @ obstacle_in_camera
+# Get the position and orientation of the object in world frame
+position_in_world = object_pose.get_position(Frame.WORLD)
+orientation_in_world = object_pose.get_orientation(Frame.WORLD)
 ```
 
-# [Examples](https://github.com/MinhxNguyen7/FrameTransforms/blob/main/example.py)
+## ROS Integration
+
+Simply wrap the `Register` in a ROS node, subscribing to pose updates and publishing/service-calling the poses. The `Register` is thread-safe, so callbacks are simple.
+
+## Alternatives
+- `tf2`: Heavyweight, requires ROS.
+- `posetree`: Requires `PoseTree` subclass implementation.
+
+# [Examples](https://github.com/MinhxNguyen7/FrameTransforms/blob/main/example.py)
+
+# Development Setup
+
+- Clone and `cd` into this repository.
+- Set up virtual environment.
+  - `python -m venv venv`
+  - `source venv/bin/activate` (Linux/Mac) or `venv\Scripts\activate` (Windows)
+- Install package with dev and test dependencies
+  - `pip install -e '.[dev,test]'`

example.py

@@ -3,7 +3,12 @@
 import numpy as np
 from scipy.spatial.transform import Rotation
 
-from frame_transforms import Registry, make_3d_transformation, InvaidTransformationError
+from frame_transforms import (
+    Pose,
+    Transform,
+    Registry,
+    InvalidTransformError,
+)
 
 
 class Frame(Enum):
@@ -21,82 +26,111 @@ def make_example_registry():
 
     # Add transformations between frames
 
-    world_to_base_transform = make_3d_transformation(
+    world_to_base = Transform(
         np.array([0, 1, 0]), Rotation.from_euler("xyz", [0, 0, 0], degrees=True)
     )
-    registry.add_transform(Frame.WORLD, Frame.BASE, world_to_base_transform)
+    registry.add_transform(Frame.WORLD, Frame.BASE, world_to_base)
 
-    base_to_camera_transform = make_3d_transformation(
+    base_to_camera = Transform(
         np.array([0, 0, 1]), Rotation.from_euler("xyz", [0, 90, 0], degrees=True)
     )
-    registry.add_transform(Frame.BASE, Frame.CAMERA, base_to_camera_transform)
+    registry.add_transform(Frame.BASE, Frame.CAMERA, base_to_camera)
 
     return registry
 
 
 def add_cycle_example():
     """
-    Demonstrates adding a transformation that would create a cycle in the registry,
-    raising an InvaidTransformationError.
+    Demonstrates adding a transform that would create a cycle in the registry,
+    raising an InvalidTransformError.
     """
     registry = make_example_registry()
 
-    # Attempt to add a transformation that creates a cycle
+    # Attempt to add a transform that creates a cycle
     try:
         registry.add_transform(Frame.CAMERA, Frame.WORLD, np.zeros(4))
-    except InvaidTransformationError:
+    except InvalidTransformError:
         print(
-            "Caught invalid transformation because there is already a path between CAMERA and WORLD."
+            "Caught invalid transform because there is already a path between CAMERA and WORLD."
         )
 
 
-def transitive_transformation_example():
+def transitive_transform_example():
     """
-    Demonstrates getting a transformation from one frame to another through an intermediate frame.
+    Demonstrates getting a transform from one frame to another through an intermediate frame.
     """
     registry = make_example_registry()
 
-    expected = make_3d_transformation(
+    expected = Transform(
         np.array([0, 1, 1]), Rotation.from_euler("xyz", [0, 90, 0], degrees=True)
     )
     actual = registry.get_transform(Frame.WORLD, Frame.CAMERA)
-    assert np.allclose(actual[:3], expected[:3]), "Position mismatch"
-    assert np.allclose(actual[3:], expected[3:]), "Rotation mismatch"
-    print("Transformation from WORLD to CAMERA is correct.")
+    assert np.allclose(actual.translation, expected.translation), "Position mismatch"
+    assert np.allclose(
+        actual.rotation.as_matrix(), expected.rotation.as_matrix()
+    ), "Rotation mismatch"
+    print("Transform from WORLD to CAMERA is correct.")
 
 
-def update_transformation_example():
+def update_transform_example():
     """
-    Demonstrates updating an existing transformation in the registry,
+    Demonstrates updating an existing transform in the registry,
     specifically, moving the base on which the camera sits.
     """
     registry = make_example_registry()
 
-    # Update the transformation from WORLD to BASE
-    new_transform = make_3d_transformation(
+    # Update the transform from WORLD to BASE
+    new_transform = Transform(
         np.array([0, 2, 0]), Rotation.from_euler("xyz", [0, 0, 0], degrees=True)
     )
     registry.update(Frame.WORLD, Frame.BASE, new_transform)
 
-    # Attempt to add instead of update the transformation
+    # Attempt to add instead of update the transform
     try:
         registry.add_transform(Frame.WORLD, Frame.BASE, new_transform)
-    except InvaidTransformationError:
+    except InvalidTransformError:
         print(
-            "Caught invalid transformation because both frames already exist in the registry."
+            "Caught invalid transform because both frames already exist in the registry."
         )
 
-    # Check the updated transformation
-    expected = make_3d_transformation(
+    # Check the updated transform
+    expected = Transform(
         np.array([0, 2, 1]), Rotation.from_euler("xyz", [0, 90, 0], degrees=True)
     )
     actual = registry.get_transform(Frame.WORLD, Frame.CAMERA)
-    assert np.allclose(actual[:3], expected[:3]), "Position mismatch after update"
-    assert np.allclose(actual[3:], expected[3:]), "Rotation mismatch after update"
-    print("Transformation from WORLD to CAMERA updated correctly.")
+    assert np.allclose(
+        actual.translation, expected.translation
+    ), "Position mismatch after update"
+    assert np.allclose(
+        actual.rotation.as_matrix(), expected.rotation.as_matrix()
+    ), "Rotation mismatch after update"
+    print("Transform from WORLD to CAMERA updated correctly.")
+
+
+def pose_example():
+    """
+    Demonstrates creating and using a Pose object.
+    """
+    registry = make_example_registry()
+
+    object_pose = Pose(
+        Transform(
+            np.array([1, 0, 0]),
+            Rotation.from_euler("xyz", [0, 0, 0], degrees=True),
+        ),
+        parent_frame=Frame.CAMERA,
+        registry=registry,
+    )
+
+    position_in_world = object_pose.get_position(Frame.WORLD)
+    print("Object position in WORLD frame:", position_in_world)
+
+    orientation_in_world = object_pose.get_orientation(Frame.WORLD)
+    print("Object orientation in WORLD frame:", orientation_in_world.as_matrix())
 
 
 if __name__ == "__main__":
     add_cycle_example()
-    transitive_transformation_example()
-    update_transformation_example()
+    transitive_transform_example()
+    update_transform_example()
+    pose_example()

frame_transforms/__init__.py

@@ -1,5 +1,7 @@
-from .registry import (
+from .transforms import (
+    Pose as Pose,
+    Transform as Transform,
     Registry as Registry,
-    InvaidTransformationError as InvaidTransformationError,
+    InvalidTransformError as InvalidTransformError,
 )
 from .utils import make_3d_transformation as make_3d_transformation