From ec07ea0e773dcdf474e151aa99d08186a2b21ee0 Mon Sep 17 00:00:00 2001
From: cc <cc7@duck.com>
Date: Fri, 23 Oct 2026 02:11:19 -0700
Subject: [PATCH 1/2] refactor(messages): isolate generated geometry value
 operations

---
 dimos/message_utils/geometry.py      | 376 +++++++++++++++++++++++++++
 dimos/message_utils/test_geometry.py | 283 ++++++++++++++++++++
 2 files changed, 659 insertions(+)
 create mode 100644 dimos/message_utils/geometry.py
 create mode 100644 dimos/message_utils/test_geometry.py

diff --git a/dimos/message_utils/geometry.py b/dimos/message_utils/geometry.py
new file mode 100644
index 00000000000..2ab85421267
--- /dev/null
+++ b/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),
+    )
diff --git a/dimos/message_utils/test_geometry.py b/dimos/message_utils/test_geometry.py
new file mode 100644
index 00000000000..a26d0a07982
--- /dev/null
+++ b/dimos/message_utils/test_geometry.py
@@ -0,0 +1,283 @@
+# 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.
+
+# Copyright 2026 Dimensional Inc.
+# SPDX-License-Identifier: Apache-2.0
+
+import math
+
+from dimos_cdrgen.registry import decode as cdr_decode, encode as cdr_encode
+import numpy as np
+import pytest
+
+from dimos.message_utils.geometry import (
+    compose_transforms,
+    inverse_transform,
+    pose_from_matrix,
+    pose_from_transform,
+    pose_matrix,
+    quaternion_euler,
+    quaternion_from_euler,
+    transform_from_odometry,
+    transform_from_pose,
+    transform_matrix,
+    translate_pose_local,
+    yaw,
+)
+from dimos.messages.builtin_interfaces.msg import Time
+from dimos.messages.geometry_msgs.msg import (
+    Point,
+    Pose,
+    PoseStamped,
+    PoseWithCovariance,
+    Quaternion,
+    Transform,
+    TransformStamped,
+    Twist,
+    TwistWithCovariance,
+    Vector3,
+)
+from dimos.messages.nav_msgs.msg import Odometry
+from dimos.messages.std_msgs.msg import Header
+
+
+@pytest.mark.parametrize("seconds,nanoseconds", [(0, 0), (-1, 500000000), (1700000000, 123456789)])
+def test_odometry_transform_preserves_frames_stamp_and_pose(seconds, nanoseconds):
+    message = Odometry(
+        child_frame_id="robot/lidar",
+        header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""),
+        pose=PoseWithCovariance(
+            pose=Pose(
+                position=Point(x=0.0, y=0.0, z=0.0),
+                orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0),
+            ),
+            covariance=np.zeros(36, dtype=np.float64),
+        ),
+        twist=TwistWithCovariance(
+            twist=Twist(linear=Vector3(x=0.0, y=0.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=0.0)),
+            covariance=np.zeros(36, dtype=np.float64),
+        ),
+    )
+    message.header.frame_id = "robot/odom"
+    message.header.stamp = Time(sec=seconds, nanosec=nanoseconds)
+    message.pose.pose.position = Point(x=1, y=2, z=3)
+    message.pose.pose.orientation = Quaternion(z=0.6, w=0.8, x=0.0, y=0.0)
+
+    transformed = transform_from_odometry(message)
+
+    assert transformed.header.frame_id == "robot/odom"
+    assert transformed.child_frame_id == "robot/lidar"
+    assert transformed.header.stamp.sec == seconds
+    assert transformed.header.stamp.nanosec == nanoseconds
+    assert (
+        transformed.transform.translation.x,
+        transformed.transform.translation.y,
+        transformed.transform.translation.z,
+    ) == (1, 2, 3)
+    assert transformed.transform.rotation == message.pose.pose.orientation
+    transformed.header.frame_id = "changed"
+    transformed.transform.rotation.w = 0
+    assert message.header.frame_id == "robot/odom"
+    assert message.pose.pose.orientation.w == 0.8
+
+
+def _edge(parent, child, xyz, rotation, stamp=Time(sec=1700000000, nanosec=123456789)):
+    return TransformStamped(
+        header=Header(frame_id=parent, stamp=stamp),
+        child_frame_id=child,
+        transform=Transform(translation=Vector3(x=xyz[0], y=xyz[1], z=xyz[2]), rotation=rotation),
+    )
+
+
+def test_transform_composition_rotates_translation_and_preserves_exact_source_stamp():
+    first = _edge("world", "arm", (1, -1, 0), quaternion_from_euler(0, 0, math.pi / 2))
+    second = _edge(
+        "arm", "tool", (2, 3, 4), Quaternion(w=1, x=0.0, y=0.0, z=0.0), Time(sec=7, nanosec=0)
+    )
+
+    result = compose_transforms(first, second)
+
+    assert result.header == first.header
+    assert result.child_frame_id == "tool"
+    translation = result.transform.translation
+    assert (translation.x, translation.y, translation.z) == pytest.approx((-2, 1, 4))
+    assert yaw(result.transform.rotation) == pytest.approx(math.pi / 2)
+    result.header.stamp.nanosec = 0
+    result.transform.translation.x = 99
+    assert first.header.stamp.nanosec == 123456789
+    assert first.transform.translation.x == 1
+    assert second.transform.translation.x == 2
+
+
+def test_inverse_composes_to_identity_and_swaps_frames_without_losing_nanoseconds():
+    original = _edge("map", "camera", (2, 3, -4), quaternion_from_euler(0.3, -0.2, 1.1))
+    inverse = inverse_transform(original)
+    result = compose_transforms(original, inverse)
+
+    assert inverse.header.frame_id == "camera"
+    assert inverse.child_frame_id == "map"
+    assert inverse.header.stamp == original.header.stamp
+    translation = result.transform.translation
+    assert (translation.x, translation.y, translation.z) == pytest.approx((0, 0, 0), abs=1e-12)
+    rotation = result.transform.rotation
+    assert (rotation.x, rotation.y, rotation.z, abs(rotation.w)) == pytest.approx((0, 0, 0, 1))
+
+
+def test_transform_composition_rejects_disconnected_frames():
+    first = _edge("a", "b", (0, 0, 0), Quaternion(w=1, x=0.0, y=0.0, z=0.0))
+    second = _edge("c", "d", (0, 0, 0), Quaternion(w=1, x=0.0, y=0.0, z=0.0))
+    with pytest.raises(ValueError, match="Cannot compose frames"):
+        compose_transforms(first, second)
+
+
+@pytest.mark.parametrize(
+    "rotation",
+    [
+        Quaternion(w=0, x=0.0, y=0.0, z=0.0),
+        Quaternion(x=math.nan, w=1, y=0.0, z=0.0),
+        Quaternion(w=math.inf, x=0.0, y=0.0, z=0.0),
+    ],
+)
+def test_transform_math_rejects_invalid_rotations(rotation):
+    edge = _edge("a", "b", (0, 0, 0), rotation)
+    with pytest.raises(ValueError):
+        inverse_transform(edge)
+    with pytest.raises(ValueError):
+        compose_transforms(edge, _edge("b", "c", (0, 0, 0), Quaternion(w=1, x=0.0, y=0.0, z=0.0)))
+
+
+@pytest.mark.parametrize("value", [math.nan, math.inf, -math.inf])
+def test_transform_math_rejects_nonfinite_translations_and_angles(value):
+    edge = _edge("a", "b", (value, 0, 0), Quaternion(w=1, x=0.0, y=0.0, z=0.0))
+    with pytest.raises(ValueError, match="Translation components"):
+        inverse_transform(edge)
+    with pytest.raises(ValueError, match="Euler angles"):
+        quaternion_from_euler(value, 0, 0)
+
+
+def test_pose_conversion_copies_nested_fields_and_preserves_header():
+    edge = _edge("map", "robot", (1, 2, 3), Quaternion(z=0.6, w=0.8, x=0.0, y=0.0))
+    pose = pose_from_transform(edge)
+    assert pose.header == edge.header
+    assert pose.pose.position == Point(x=1, y=2, z=3)
+    assert pose.pose.orientation == edge.transform.rotation
+    pose.header.frame_id = "changed"
+    pose.pose.orientation.w = 0
+    assert edge.header.frame_id == "map"
+    assert edge.transform.rotation.w == 0.8
+
+
+def test_pose_transform_preserves_header_and_does_not_alias_input():
+    source = PoseStamped(
+        header=Header(frame_id="odom", stamp=Time(sec=-1, nanosec=987654321)),
+        pose=Pose(
+            position=Point(x=1, y=2, z=3), orientation=Quaternion(z=0.6, w=0.8, x=0.0, y=0.0)
+        ),
+    )
+    result = transform_from_pose(source, child_frame_id="robot/base")
+    decoded = cdr_decode(cdr_encode(result), TransformStamped)
+    assert decoded.header == source.header
+    assert decoded.child_frame_id == "robot/base"
+    assert decoded.transform.translation == Vector3(x=1, y=2, z=3)
+    assert decoded.transform.rotation == source.pose.orientation
+    result.header.frame_id = "changed"
+    result.transform.rotation.w = 0
+    assert source.header.frame_id == "odom"
+    assert source.pose.orientation.w == 0.8
+
+
+def test_transform_matrix_maps_rotation_and_translation_without_aliasing():
+    value = Transform(
+        translation=Vector3(x=2.0, y=-1.0, z=3.0),
+        rotation=quaternion_from_euler(0.0, 0.0, math.pi / 2),
+    )
+    matrix = transform_matrix(value)
+    np.testing.assert_allclose(matrix @ [1.0, 0.0, 0.0, 1.0], [2.0, 0.0, 3.0, 1.0], atol=1e-12)
+    value.translation.x = 10.0
+    assert matrix[0, 3] == 2.0
+
+
+def test_pose_matrix_roundtrip_preserves_rigid_motion_without_aliasing():
+    pose = Pose(
+        position=Point(x=2, y=-1, z=3),
+        orientation=quaternion_from_euler(0, 0, math.pi / 2),
+    )
+    matrix = pose_matrix(pose)
+    np.testing.assert_allclose(matrix @ [1, 0, 0, 1], [2, 0, 3, 1], atol=1e-12)
+    decoded = cdr_decode(cdr_encode(pose_from_matrix(matrix)), Pose)
+    np.testing.assert_allclose(pose_matrix(decoded), matrix, atol=1e-12)
+    matrix[0, 3] = 99
+    decoded.position.y = 42
+    assert pose.position == Point(x=2, y=-1, z=3)
+    assert decoded.position.x == 2
+
+
+@pytest.mark.parametrize(
+    "matrix",
+    [
+        np.eye(3),
+        np.full((4, 4), math.nan),
+        np.diag([1, 1, 1, 2]),
+        np.diag([2, 1, 1, 1]),
+        np.diag([-1, 1, 1, 1]),
+    ],
+)
+def test_pose_from_matrix_rejects_nonrigid_or_invalid_matrices(matrix):
+    with pytest.raises(ValueError):
+        pose_from_matrix(matrix)
+
+
+@pytest.mark.parametrize(
+    "pose",
+    [
+        Pose(
+            position=Point(x=math.inf, y=0.0, z=0.0),
+            orientation=Quaternion(w=1, x=0.0, y=0.0, z=0.0),
+        ),
+        Pose(orientation=Quaternion(w=0, x=0.0, y=0.0, z=0.0), position=Point(x=0.0, y=0.0, z=0.0)),
+        Pose(
+            orientation=Quaternion(w=math.nan, x=0.0, y=0.0, z=0.0),
+            position=Point(x=0.0, y=0.0, z=0.0),
+        ),
+    ],
+)
+def test_pose_matrix_rejects_invalid_geometry(pose):
+    with pytest.raises(ValueError):
+        pose_matrix(pose)
+
+
+def test_local_offset_uses_pose_axes_and_copies_orientation():
+    pose = Pose(
+        position=Point(x=1, y=2, z=3),
+        orientation=quaternion_from_euler(0, 0, math.pi / 2),
+    )
+    translated = translate_pose_local(pose, Vector3(x=2, z=-1, y=0.0))
+    position = translated.position
+    assert (position.x, position.y, position.z) == pytest.approx((1, 4, 2))
+    assert quaternion_euler(translated.orientation) == pytest.approx((0, 0, math.pi / 2))
+    translated.orientation.w = 0
+    assert pose.orientation.w == pytest.approx(math.sqrt(0.5))
+    assert pose.position == Point(x=1, y=2, z=3)
+
+
+@pytest.mark.parametrize("value", [math.nan, math.inf, -math.inf])
+def test_local_offset_rejects_nonfinite_displacements(value):
+    with pytest.raises(ValueError, match="Offset components"):
+        translate_pose_local(
+            Pose(
+                orientation=Quaternion(w=1, x=0.0, y=0.0, z=0.0),
+                position=Point(x=0.0, y=0.0, z=0.0),
+            ),
+            Vector3(x=value, y=0.0, z=0.0),
+        )

From f4231128132d5ff25e15f4f6399626b7c266e28f Mon Sep 17 00:00:00 2001
From: cc <cc7@duck.com>
Date: Fri, 23 Oct 2026 02:15:46 -0700
Subject: [PATCH 2/2] ci(cdr): run this adapter layer tests before SDK checks

---
 .github/workflows/cdr-native.yml | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/.github/workflows/cdr-native.yml b/.github/workflows/cdr-native.yml
index 08a5e34f6ed..86472495100 100644
--- a/.github/workflows/cdr-native.yml
+++ b/.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