refactor(messages): add geometry operations for generated values

.github/workflows/cdr-native.yml

@@ -37,7 +37,8 @@ jobs:
             dimos/message_utils/test_image_views.py \
             dimos/message_utils/test_image_compression.py \
             dimos/message_utils/test_camera_info.py \
-            dimos/message_utils/test_time.py
+            dimos/message_utils/test_time.py \
+            dimos/message_utils/test_geometry.py
       - name: Test native SDK
         run: |
           cargo test --locked --manifest-path native/rust/dimos-module/Cargo.toml

dimos/message_utils/geometry.py

@@ -0,0 +1,376 @@
+# Copyright 2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Geometry operations on generated ROS value types."""
+
+from collections.abc import Sequence
+from copy import deepcopy
+import math
+
+import numpy as np
+from numpy.typing import NDArray
+from scipy.spatial.transform import Rotation
+
+from dimos.messages.geometry_msgs.msg import (
+    Point,
+    Pose,
+    PoseStamped,
+    Quaternion,
+    Transform,
+    TransformStamped,
+    Vector3,
+    Wrench,
+)
+from dimos.messages.nav_msgs.msg import Odometry
+from dimos.messages.std_msgs.msg import Header
+
+
+def yaw(rotation: Quaternion) -> float:
+    """Return rotation about Z in radians (ROS XYZW quaternion convention)."""
+    return math.atan2(
+        2 * (rotation.w * rotation.z + rotation.x * rotation.y),
+        1 - 2 * (rotation.y * rotation.y + rotation.z * rotation.z),
+    )
+
+
+def transform_from_odometry(message: Odometry) -> TransformStamped:
+    """Copy the pose and declared frames into TF, preserving the exact source stamp."""
+    position = message.pose.pose.position
+    return TransformStamped(
+        header=deepcopy(message.header),
+        child_frame_id=message.child_frame_id,
+        transform=Transform(
+            translation=Vector3(x=position.x, y=position.y, z=position.z),
+            rotation=deepcopy(message.pose.pose.orientation),
+        ),
+    )
+
+
+def quaternion_from_euler(roll: float, pitch: float, yaw: float) -> Quaternion:
+    """Convert fixed-axis XYZ angles in radians to a normalized ROS quaternion."""
+    if not all(math.isfinite(value) for value in (roll, pitch, yaw)):
+        raise ValueError("Euler angles must be finite")
+    x, y, z, w = Rotation.from_euler("xyz", [roll, pitch, yaw]).as_quat()
+    return Quaternion(x=x, y=y, z=z, w=w)
+
+
+def _rotation(quaternion: Quaternion) -> Rotation:
+    values = [quaternion.x, quaternion.y, quaternion.z, quaternion.w]
+    if not all(math.isfinite(value) for value in values):
+        raise ValueError("Quaternion components must be finite")
+    # SciPy rejects zero norm and normalizes non-unit input.
+    return Rotation.from_quat(values)
+
+
+def _translation(transform: Transform) -> NDArray[np.float64]:
+    translation = transform.translation
+    values = np.array([translation.x, translation.y, translation.z], dtype=np.float64)
+    if not np.isfinite(values).all():
+        raise ValueError("Translation components must be finite")
+    return values
+
+
+def _transform(translation: NDArray[np.float64], rotation: Rotation) -> Transform:
+    x, y, z, w = rotation.as_quat()
+    return Transform(
+        translation=Vector3(x=translation[0], y=translation[1], z=translation[2]),
+        rotation=Quaternion(x=x, y=y, z=z, w=w),
+    )
+
+
+def transform_matrix(transform: Transform) -> NDArray[np.float64]:
+    """Copy a generated transform into a homogeneous 4-by-4 matrix."""
+    matrix = np.eye(4, dtype=np.float64)
+    matrix[:3, :3] = _rotation(transform.rotation).as_matrix()
+    matrix[:3, 3] = _translation(transform)
+    return matrix
+
+
+def compose_transforms(first: TransformStamped, second: TransformStamped) -> TransformStamped:
+    """Compose A←B and B←C, preserving A's exact source stamp in the A←C result.
+
+    Inputs are copied into a new value. The shared frame must match; callers
+    querying a time-varying chain are responsible for choosing each edge's time.
+    """
+    if first.child_frame_id != second.header.frame_id:
+        raise ValueError(
+            f"Cannot compose frames {first.child_frame_id!r} and {second.header.frame_id!r}"
+        )
+    rotation = _rotation(first.transform.rotation)
+    translation = _translation(first.transform) + rotation.apply(_translation(second.transform))
+    return TransformStamped(
+        header=deepcopy(first.header),
+        child_frame_id=second.child_frame_id,
+        transform=_transform(translation, rotation * _rotation(second.transform.rotation)),
+    )
+
+
+def inverse_transform(message: TransformStamped) -> TransformStamped:
+    """Return B←A from A←B, swapping frame names and preserving the exact stamp."""
+    rotation = _rotation(message.transform.rotation).inv()
+    return TransformStamped(
+        header=Header(stamp=deepcopy(message.header.stamp), frame_id=message.child_frame_id),
+        child_frame_id=message.header.frame_id,
+        transform=_transform(-rotation.apply(_translation(message.transform)), rotation),
+    )
+
+
+def pose_from_transform(message: TransformStamped) -> PoseStamped:
+    """Copy the child frame's pose in the parent frame into a generated PoseStamped."""
+    translation = message.transform.translation
+    return PoseStamped(
+        header=deepcopy(message.header),
+        pose=Pose(
+            position=Point(x=translation.x, y=translation.y, z=translation.z),
+            orientation=deepcopy(message.transform.rotation),
+        ),
+    )
+
+
+def transform_from_pose(message: PoseStamped, *, child_frame_id: str) -> TransformStamped:
+    """Copy a stamped pose into TF with an explicit child and unchanged source header."""
+    if not isinstance(message, PoseStamped):
+        raise TypeError("Expected a generated PoseStamped")
+    position = message.pose.position
+    return TransformStamped(
+        header=deepcopy(message.header),
+        child_frame_id=child_frame_id,
+        transform=Transform(
+            translation=Vector3(x=position.x, y=position.y, z=position.z),
+            rotation=deepcopy(message.pose.orientation),
+        ),
+    )
+
+
+def pose_matrix(pose: Pose) -> NDArray[np.float64]:
+    """Copy a generated pose into a homogeneous matrix, rejecting invalid geometry."""
+    values = np.array([pose.position.x, pose.position.y, pose.position.z], dtype=np.float64)
+    if not np.isfinite(values).all():
+        raise ValueError("Position components must be finite")
+    result = np.eye(4, dtype=np.float64)
+    result[:3, :3] = _rotation(pose.orientation).as_matrix()
+    result[:3, 3] = values
+    return result
+
+
+def quaternion_from_matrix(matrix: NDArray[np.float64]) -> Quaternion:
+    """Convert a finite proper 3-by-3 rotation matrix into a generated quaternion."""
+    values = np.asarray(matrix, dtype=np.float64)
+    if values.shape != (3, 3) or not np.isfinite(values).all():
+        raise ValueError("Rotation matrix must be finite and 3-by-3")
+    if not np.allclose(values.T @ values, np.eye(3), atol=1e-6, rtol=0.0) or not np.isclose(
+        np.linalg.det(values), 1.0, atol=1e-6, rtol=0.0
+    ):
+        raise ValueError("Rotation matrix must be orthonormal with determinant +1")
+    x, y, z, w = Rotation.from_matrix(values).as_quat()
+    return Quaternion(x=x, y=y, z=z, w=w)
+
+
+def pose_from_matrix(matrix: NDArray[np.float64]) -> Pose:
+    """Copy a finite rigid 4-by-4 transform into a generated pose."""
+    values = np.asarray(matrix, dtype=np.float64)
+    if values.shape != (4, 4) or not np.isfinite(values).all():
+        raise ValueError("Pose matrix must be finite and 4-by-4")
+    if not np.allclose(values[3], [0.0, 0.0, 0.0, 1.0], atol=1e-9, rtol=0.0):
+        raise ValueError("Pose matrix must have homogeneous last row [0, 0, 0, 1]")
+    return Pose(
+        position=Point(x=values[0, 3], y=values[1, 3], z=values[2, 3]),
+        orientation=quaternion_from_matrix(values[:3, :3]),
+    )
+
+
+def transform_from_matrix(matrix: NDArray[np.float64]) -> Transform:
+    """Copy a finite rigid homogeneous matrix into a generated transform."""
+    pose = pose_from_matrix(matrix)
+    return Transform(
+        translation=Vector3(x=pose.position.x, y=pose.position.y, z=pose.position.z),
+        rotation=deepcopy(pose.orientation),
+    )
+
+
+def quaternion_euler(rotation: Quaternion) -> tuple[float, float, float]:
+    """Return fixed-axis XYZ Euler angles in radians for a generated quaternion."""
+    roll, pitch, yaw = _rotation(rotation).as_euler("xyz")
+    return float(roll), float(pitch), float(yaw)
+
+
+def translate_pose_local(pose: Pose, offset: Vector3) -> Pose:
+    """Copy a pose displaced by an offset expressed in its local axes."""
+    matrix = pose_matrix(pose)
+    values = np.array([offset.x, offset.y, offset.z], dtype=np.float64)
+    if not np.isfinite(values).all():
+        raise ValueError("Offset components must be finite")
+    delta = matrix[:3, :3] @ values
+    return Pose(
+        position=Point(
+            x=pose.position.x + delta[0], y=pose.position.y + delta[1], z=pose.position.z + delta[2]
+        ),
+        orientation=deepcopy(pose.orientation),
+    )
+
+
+def point_distance(first: Point, second: Point) -> float:
+    """Euclidean distance between generated position values."""
+    return math.dist((first.x, first.y, first.z), (second.x, second.y, second.z))
+
+
+def quaternion_angle(first: Quaternion, second: Quaternion) -> float:
+    """Return the shortest angular separation in radians, normalizing inputs."""
+    return float((_rotation(first).inv() * _rotation(second)).magnitude())
+
+
+def wrench_from_array(values: Sequence[float] | NDArray[np.float64]) -> Wrench:
+    """Copy force in N and torque in Nm from [fx, fy, fz, tx, ty, tz]."""
+    array = np.asarray(values, dtype=np.float64)
+    if array.shape != (6,):
+        raise ValueError("Expected 6 elements [fx, fy, fz, tx, ty, tz]")
+    return Wrench(
+        force=Vector3(x=array[0], y=array[1], z=array[2]),
+        torque=Vector3(x=array[3], y=array[4], z=array[5]),
+    )
+
+
+def wrench_array(message: Wrench) -> NDArray[np.float64]:
+    """Return an independent numeric force/torque array for arithmetic."""
+    return np.array(
+        [
+            message.force.x,
+            message.force.y,
+            message.force.z,
+            message.torque.x,
+            message.torque.y,
+            message.torque.z,
+        ],
+        dtype=np.float64,
+    )
+
+
+def vector_array(value: Vector3 | Point) -> NDArray[np.float64]:
+    """Copy a generated spatial vector/point into an independent XYZ array."""
+    return np.array([value.x, value.y, value.z], dtype=np.float64)
+
+
+def vector_from_array(values: Sequence[float] | NDArray[np.float64]) -> Vector3:
+    """Copy exactly three coordinates into a generated vector."""
+    array = np.asarray(values, dtype=np.float64)
+    if array.shape != (3,):
+        raise ValueError("Expected 3 vector components")
+    return Vector3(x=array[0], y=array[1], z=array[2])
+
+
+def normalized_vector(value: Vector3) -> Vector3:
+    """Return a unit vector; zero length remains zero."""
+    array = vector_array(value)
+    length = float(np.linalg.norm(array))
+    return vector_from_array(array / length) if length else Vector3(x=0.0, y=0.0, z=0.0)
+
+
+def quaternion_array(value: Quaternion) -> NDArray[np.float64]:
+    """Copy XYZW components without normalizing their declared values."""
+    return np.array([value.x, value.y, value.z, value.w], dtype=np.float64)
+
+
+def quaternion_from_array(values: Sequence[float] | NDArray[np.float64]) -> Quaternion:
+    """Copy exactly four XYZW components into a generated quaternion."""
+    array = np.asarray(values, dtype=np.float64)
+    if array.shape != (4,):
+        raise ValueError("Quaternion requires exactly 4 components")
+    return Quaternion(x=float(array[0]), y=float(array[1]), z=float(array[2]), w=float(array[3]))
+
+
+def quaternion_product(first: Quaternion, second: Quaternion) -> Quaternion:
+    """Hamilton product, retaining non-unit magnitudes and multiplication order."""
+    if not isinstance(first, Quaternion) or not isinstance(second, Quaternion):
+        raise TypeError("Cannot multiply Quaternion with a non-quaternion value")
+    a, b = quaternion_array(first), quaternion_array(second)
+    xyz = a[3] * b[:3] + b[3] * a[:3] + np.cross(a[:3], b[:3])
+    return quaternion_from_array(np.append(xyz, a[3] * b[3] - np.dot(a[:3], b[:3])))
+
+
+def quaternion_conjugate(value: Quaternion) -> Quaternion:
+    """Negate the imaginary components, preserving the scalar component."""
+    return Quaternion(x=-value.x, y=-value.y, z=-value.z, w=value.w)
+
+
+def quaternion_inverse(value: Quaternion) -> Quaternion:
+    """Algebraic inverse, including non-unit quaternions."""
+    array = quaternion_array(value)
+    squared = float(np.dot(array, array))
+    if squared == 0:
+        raise ZeroDivisionError("Cannot invert zero quaternion")
+    return quaternion_from_array(quaternion_array(quaternion_conjugate(value)) / squared)
+
+
+def normalized_quaternion(value: Quaternion) -> Quaternion:
+    """Return unit components without changing sign; reject zero norm."""
+    array = quaternion_array(value)
+    length = float(np.linalg.norm(array))
+    if length == 0:
+        raise ZeroDivisionError("Cannot normalize zero quaternion")
+    return quaternion_from_array(array / length)
+
+
+def rotate_vector(rotation: Quaternion, value: Vector3) -> Vector3:
+    """Rotate an XYZ vector using a normalized spatial rotation."""
+    return vector_from_array(_rotation(rotation).apply(vector_array(value)))
+
+
+def point_from_array(values: Sequence[float] | NDArray[np.float64]) -> Point:
+    """Copy exactly three Cartesian coordinates into a generated point."""
+    vector = vector_from_array(values)
+    return Point(x=vector.x, y=vector.y, z=vector.z)
+
+
+def compose_poses(first: Pose, second: Pose) -> Pose:
+    """Compose parent/local poses without mutating inputs or normalizing stored quaternions."""
+    offset = _rotation(first.orientation).apply(vector_array(second.position))
+    return Pose(
+        position=point_from_array(vector_array(first.position) + offset),
+        orientation=quaternion_product(first.orientation, second.orientation),
+    )
+
+
+def compose_transform_values(first: Transform, second: Transform) -> Transform:
+    """Compose unframed spatial transforms in parent/local order."""
+    if not isinstance(first, Transform) or not isinstance(second, Transform):
+        raise TypeError("Expected generated Transform values")
+    return Transform(
+        translation=vector_from_array(
+            vector_array(first.translation)
+            + _rotation(first.rotation).apply(vector_array(second.translation))
+        ),
+        rotation=quaternion_product(first.rotation, second.rotation),
+    )
+
+
+def transform_pose_local(pose: Pose, transform: Transform) -> Pose:
+    """Apply an offset expressed in a pose's local axes."""
+    return compose_poses(
+        pose,
+        Pose(
+            position=point_from_array(vector_array(transform.translation)),
+            orientation=transform.rotation,
+        ),
+    )
+
+
+def relative_pose_transform(first: Pose, second: Pose) -> Transform:
+    """Find the local transform taking the first pose to the second."""
+    rotation = quaternion_inverse(first.orientation)
+    return Transform(
+        translation=vector_from_array(
+            _rotation(rotation).apply(vector_array(second.position) - vector_array(first.position))
+        ),
+        rotation=quaternion_product(rotation, second.orientation),
+    )