feat(pi05): compose relative poses in SE3

tests/processor/test_pi05_state_from_action.py

@@ -14,6 +14,8 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+from math import pi
+
 import numpy as np
 import pytest
 import torch
@@ -114,6 +116,26 @@ def test_state_history_can_be_relative_with_absolute_gripper():
     )
 
 
+def test_state_history_can_use_se3_composition_with_absolute_gripper():
+    state_history = torch.tensor(
+        [[[0.0, 1.0, 0.0, 0.0, 0.0, pi / 2, 0.2], [0.0, 0.0, 0.0, 0.0, 0.0, pi / 2, 0.4]]]
+    )
+    step = Pi05FlattenStateHistoryProcessorStep(
+        history_steps=2,
+        max_state_dim=14,
+        relative=True,
+        exclude_joints=["gripper"],
+        state_names=["x", "y", "z", "rx", "ry", "rz", "gripper_width"],
+        pose_representation="se3",
+        se3_pose_groups=[list(range(6))],
+    )
+
+    result = step(_transition(torch.zeros(1, 2, 7), state_history))
+
+    expected = torch.tensor([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.4]])
+    torch.testing.assert_close(result[TransitionKey.OBSERVATION][OBS_STATE], expected, atol=1e-6, rtol=1e-6)
+
+
 def test_inference_state_history_is_rolled_and_reset():
     step = Pi05StateFromActionProcessorStep(enabled=True, history_steps=2)
 
@@ -173,3 +195,33 @@ def test_relative_state_history_stats_match_processor_representation():
 
     expected = np.asarray([[0.0, 0.1, 0.0, 0.1], [-1.0, 0.1, 0.0, 0.2], [-2.0, 0.2, 0.0, 0.3]])
     np.testing.assert_allclose(stats["mean"], expected.mean(axis=0))
+
+
+def test_se3_relative_action_stats_use_reference_frame():
+    actions = np.asarray(
+        [
+            [0.0, 0.0, 0.0, 0.0, 0.0, pi / 2, 0.2],
+            [0.0, 1.0, 0.0, 0.0, 0.0, pi / 2, 0.3],
+        ],
+        dtype=np.float32,
+    )
+    dataset = {"action": actions, "episode_index": np.zeros(2, dtype=np.int64)}
+    features = {
+        "action": {
+            "shape": [7],
+            "names": ["x", "y", "z", "rx", "ry", "rz", "gripper_width"],
+        }
+    }
+
+    stats = compute_relative_action_stats(
+        dataset,
+        features,
+        chunk_size=2,
+        exclude_joints=["gripper"],
+        state_from_action=True,
+        pose_representation="se3",
+        se3_pose_groups=[list(range(6))],
+    )
+
+    np.testing.assert_allclose(stats["mean"][:3], [0.5, 0.0, 0.0], atol=1e-6)
+    np.testing.assert_allclose(stats["mean"][6], 0.25, atol=1e-6)

tests/processor/test_se3_relative_actions.py

@@ -0,0 +1,70 @@
+import math
+
+import pytest
+import torch
+
+from lerobot.processor.relative_action_processor import (
+    to_absolute_actions,
+    to_absolute_se3_pose,
+    to_relative_actions,
+    to_relative_se3_pose,
+)
+
+POSE_GROUP = [list(range(6))]
+
+
+def test_se3_translation_is_expressed_in_reference_frame():
+    reference = torch.tensor([[1.0, 2.0, 3.0, 0.0, 0.0, math.pi / 2]])
+    target = torch.tensor([[1.0, 3.0, 3.0, 0.0, 0.0, math.pi / 2]])
+
+    relative = to_relative_se3_pose(target, reference)
+
+    torch.testing.assert_close(relative, torch.tensor([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]]), atol=1e-6, rtol=1e-6)
+
+
+def test_se3_pose_roundtrip_for_batched_chunks():
+    torch.manual_seed(0)
+    reference = torch.randn(4, 6)
+    reference[:, 3:] *= 0.8
+    target = torch.randn(4, 11, 6)
+    target[..., 3:] *= 0.8
+
+    relative = to_relative_se3_pose(target, reference.unsqueeze(1))
+    recovered = to_absolute_se3_pose(relative, reference.unsqueeze(1))
+
+    torch.testing.assert_close(recovered, target, atol=2e-5, rtol=2e-5)
+
+
+def test_mixed_se3_pose_and_absolute_gripper_roundtrip():
+    reference = torch.tensor([[0.2, -0.1, 0.4, 0.1, 0.2, -0.3, 0.06]])
+    target = torch.tensor([[[0.3, 0.2, 0.5, -0.2, 0.1, 0.4, 0.03], [0.1, -0.3, 0.2, 0.5, -0.1, 0.2, 0.05]]])
+    mask = [True, True, True, True, True, True, False]
+
+    relative = to_relative_actions(
+        target,
+        reference,
+        mask,
+        pose_representation="se3",
+        se3_pose_groups=POSE_GROUP,
+    )
+    recovered = to_absolute_actions(
+        relative,
+        reference,
+        mask,
+        pose_representation="se3",
+        se3_pose_groups=POSE_GROUP,
+    )
+
+    torch.testing.assert_close(relative[..., 6], target[..., 6])
+    torch.testing.assert_close(recovered, target, atol=2e-5, rtol=2e-5)
+
+
+def test_se3_pose_group_cannot_be_partially_relative():
+    with pytest.raises(ValueError, match="wholly relative or wholly absolute"):
+        to_relative_actions(
+            torch.zeros(1, 7),
+            torch.zeros(1, 7),
+            [True, True, True, False, False, False, False],
+            pose_representation="se3",
+            se3_pose_groups=POSE_GROUP,
+        )