feat: add Python checker parity (BodyContact/PositionDelta/OrderedStep)

tests/test_body_contact.py

@@ -0,0 +1,130 @@
+"""
+Unit tests for ``check_body_contact`` (Python side of BodyContactChecker).
+
+The JS side will get its own test file using vitest/jest — see
+``tests/test_body_contact.js`` (to be added) in AxisWebInfra.
+
+Run with::
+
+    python -m pytest tests/test_body_contact.py -v
+"""
+
+from __future__ import annotations
+
+import pytest
+
+from util.validate_offline_trajectory import check_body_contact
+
+
+def _state(positions):
+    return {
+        "object_positions": positions,
+        "object_orientations": {k: [0, 0, 0, 1] for k in positions},
+    }
+
+
+# --------------------------------------------------------------------------- #
+# 1. Bodies in true AABB overlap -> success                                    #
+# --------------------------------------------------------------------------- #
+
+def test_bodies_in_aabb_overlap_returns_true():
+    state = _state({
+        "akita_black_bowl_1_main": [0.0, 0.0, 0.05],
+        "plate_1_main":             [0.01, 0.0, 0.05],  # 1 cm apart, both small
+    })
+    assert check_body_contact(state, {
+        "bodyAName": "akita_black_bowl_1_main",
+        "bodyBName": "plate_1_main",
+    }) is True
+
+
+# --------------------------------------------------------------------------- #
+# 2. Bodies far apart -> failure                                              #
+# --------------------------------------------------------------------------- #
+
+def test_bodies_far_apart_returns_false():
+    state = _state({
+        "akita_black_bowl_1_main": [0.0, 0.0, 0.05],
+        "plate_1_main":             [0.50, 0.50, 0.05],  # 70 cm away
+    })
+    assert check_body_contact(state, {
+        "bodyAName": "akita_black_bowl_1_main",
+        "bodyBName": "plate_1_main",
+    }) is False
+
+
+# --------------------------------------------------------------------------- #
+# 3. max_contact_distance inflates the threshold                              #
+# --------------------------------------------------------------------------- #
+
+def test_max_contact_distance_inflates_threshold():
+    state = _state({
+        "akita_black_bowl_1_main": [0.0, 0.0, 0.05],
+        "plate_1_main":             [0.05, 0.0, 0.05],  # 5 cm apart
+    })
+    # Without inflation: 5 cm > 2.5 cm default half-extent x 2 = 5 cm, exactly at boundary
+    # so it should still be in contact (aabb overlap is <=, not <).
+    assert check_body_contact(state, {
+        "bodyAName": "akita_black_bowl_1_main",
+        "bodyBName": "plate_1_main",
+    }) is True
+
+    # With explicit half-extent reduction they don't touch without inflation
+    state2 = _state({
+        "akita_black_bowl_1_main": [0.0, 0.0, 0.05],
+        "plate_1_main":             [0.04, 0.0, 0.05],  # 4 cm apart, half-extents 1 cm each
+    })
+    # Without max_contact_distance, gap = 4 - 1 - 1 = 2 cm -> not touching
+    assert check_body_contact(state2, {
+        "bodyAName": "akita_black_bowl_1_main",
+        "bodyBName": "plate_1_main",
+        "default_half_extent": 0.01,
+    }) is False
+    # With 0.03 m max_contact_distance, gap 2 cm < 3 cm -> touching
+    assert check_body_contact(state2, {
+        "bodyAName": "akita_black_bowl_1_main",
+        "bodyBName": "plate_1_main",
+        "default_half_extent": 0.01,
+        "maxContactDistance": 0.03,
+    }) is True
+
+
+# --------------------------------------------------------------------------- #
+# 4. Missing body -> failure (graceful)                                       #
+# --------------------------------------------------------------------------- #
+
+def test_missing_body_returns_false():
+    state = _state({"akita_black_bowl_1_main": [0.0, 0.0, 0.05]})
+    assert check_body_contact(state, {
+        "bodyAName": "akita_black_bowl_1_main",
+        "bodyBName": "plate_1_main",  # not in state
+    }) is False
+
+
+# --------------------------------------------------------------------------- #
+# 5. Same body on both sides -> rejected at construction time                 #
+# --------------------------------------------------------------------------- #
+
+def test_same_body_name_is_rejected():
+    state = _state({"akita_black_bowl_1_main": [0.0, 0.0, 0.05]})
+    assert check_body_contact(state, {
+        "bodyAName": "akita_black_bowl_1_main",
+        "bodyBName": "akita_black_bowl_1_main",
+    }) is False
+
+
+# --------------------------------------------------------------------------- #
+# 6. snake_case option names also accepted (legacy compat)                    #
+# --------------------------------------------------------------------------- #
+
+def test_snake_case_options_accepted():
+    state = _state({
+        "akita_black_bowl_1_main": [0.0, 0.0, 0.05],
+        "plate_1_main":             [0.0, 0.0, 0.05],  # same position
+    })
+    assert check_body_contact(state, {
+        "body_a_name": "akita_black_bowl_1_main",
+        "body_b_name": "plate_1_main",
+        "max_contact_distance": 0.0,
+        "min_contact_count": 1,
+    }) is True

tests/test_ordered_step.py

@@ -0,0 +1,112 @@
+"""Unit tests for check_ordered_step."""
+
+from __future__ import annotations
+
+import pytest
+
+from util.validate import check_ordered_step
+
+
+# A tiny fake "dispatcher" that returns cfg["pass"] for demonstration/testing.
+def _dispatch(state, cfg):
+    # cfg: {"type": ..., "pass": bool}
+    return bool(cfg.get("pass", False))
+
+
+# 1. All steps pass -> True
+def test_all_steps_pass():
+    states = [{"t": 0}, {"t": 1}, {"t": 2}]
+    opts = {
+        "steps": [
+            {"type": "A", "pass": True},
+            {"type": "B", "pass": True},
+            {"type": "C", "pass": True},
+        ],
+        "dispatch": _dispatch,
+    }
+    assert check_ordered_step(states, opts) is True
+
+
+# 2. A step never passes -> False (but earlier ones pass)
+def test_later_step_fails():
+    states = [{"t": 0}, {"t": 1}, {"t": 2}]
+    opts = {
+        "steps": [
+            {"type": "A", "pass": True},
+            {"type": "B", "pass": False},  # never satisfied
+            {"type": "C", "pass": True},
+        ],
+        "dispatch": _dispatch,
+    }
+    assert check_ordered_step(states, opts) is False
+
+
+# 3. Steps pass on different frames -> still True (in order)
+def test_steps_pass_on_different_frames():
+    # Frame 0 satisfies step A, frame 1 satisfies B, frame 2 satisfies C.
+    # The dispatcher here keys off the frame index via state["t"].
+    def dispatch(state, cfg):
+        t = state["t"]
+        if cfg["type"] == "A":
+            return t >= 0
+        if cfg["type"] == "B":
+            return t >= 1
+        if cfg["type"] == "C":
+            return t >= 2
+        return False
+
+    states = [{"t": 0}, {"t": 1}, {"t": 2}]
+    opts = {
+        "steps": [{"type": "A"}, {"type": "B"}, {"type": "C"}],
+        "dispatch": dispatch,
+    }
+    assert check_ordered_step(states, opts) is True
+
+
+# 4. Order matters: A must be satisfied before B advances
+def test_order_matters():
+    # B is satisfiable from frame 0, but A only becomes true at frame 2.
+    # Strict ordering means we wait for A first; since A never passes until
+    # frame 2 and there are only 3 frames, B may not have time. Let's use
+    # enough frames so B can pass after A.
+    def dispatch(state, cfg):
+        t = state["t"]
+        if cfg["type"] == "A":
+            return t >= 2  # A satisfied at t=2
+        if cfg["type"] == "B":
+            return t >= 3  # B satisfied at t=3
+        return False
+
+    # Only 2 frames (t=0,1): A never satisfied -> False
+    states = [{"t": 0}, {"t": 1}]
+    opts = {
+        "steps": [{"type": "A"}, {"type": "B"}],
+        "dispatch": dispatch,
+    }
+    assert check_ordered_step(states, opts) is False
+
+    # 4 frames (t=0..3): A at t=2, B at t=3 -> True
+    states2 = [{"t": 0}, {"t": 1}, {"t": 2}, {"t": 3}]
+    opts2 = {
+        "steps": [{"type": "A"}, {"type": "B"}],
+        "dispatch": dispatch,
+    }
+    assert check_ordered_step(states2, opts2) is True
+
+
+# 5. Empty steps -> False
+def test_empty_steps_returns_false():
+    assert check_ordered_step([{"t": 0}], {"steps": [], "dispatch": _dispatch}) is False
+    assert check_ordered_step([{"t": 0}], {"dispatch": _dispatch}) is False
+
+
+# 6. Missing dispatcher -> False (graceful)
+def test_missing_dispatcher_returns_false():
+    opts = {"steps": [{"type": "A"}]}
+    assert check_ordered_step([{"t": 0}], opts) is False
+
+
+# 7. No frames -> False (can't complete any step)
+def test_no_frames_returns_false():
+    opts = {"steps": [{"type": "A", "pass": True}], "dispatch": _dispatch}
+    assert check_ordered_step([], opts) is False

tests/test_position_delta.py

@@ -0,0 +1,128 @@
+"""Unit tests for check_position_delta."""
+
+from __future__ import annotations
+
+import pytest
+
+from util.validate_offline_trajectory import check_position_delta
+
+
+def _state(positions, initial_position=None):
+    s = {
+        "object_positions": positions,
+        "object_orientations": {k: [0, 0, 0, 1] for k in positions},
+    }
+    if initial_position is not None:
+        s["initial_position"] = initial_position
+    return s
+
+
+# 1. No movement, no bound -> False
+def test_no_movement_no_bound_returns_false():
+    state = _state({"bowl": [0.0, 0.0, 0.05]})
+    assert check_position_delta(state, {"sampleBodyName": "bowl"}) is False
+
+
+# 2. Movement on X matches minDeltaX -> True (baseline passed explicitly)
+def test_x_min_delta_passed():
+    state = _state({"bowl": [0.02, 0.0, 0.05]})  # +2 cm
+    assert check_position_delta(state, {
+        "sampleBodyName": "bowl",
+        "axes": ["x"],
+        "minDeltaX": 0.018,  # 18 mm
+        "initialPosition": [0.0, 0.0, 0.05],  # baseline = origin
+    }) is True
+
+
+# 3. Movement on X below minDeltaX -> False
+def test_x_below_min_returns_false():
+    state = _state({"bowl": [0.01, 0.0, 0.05]})  # +1 cm, below 18 mm
+    assert check_position_delta(state, {
+        "sampleBodyName": "bowl",
+        "axes": ["x"],
+        "minDeltaX": 0.018,
+    }) is False
+
+
+# 4. Negative movement matches maxDeltaX -> True
+def test_x_max_delta_passed():
+    state = _state({"bowl": [-0.025, 0.0, 0.05]})  # -2.5 cm
+    assert check_position_delta(state, {
+        "sampleBodyName": "bowl",
+        "axes": ["x"],
+        "maxDeltaX": -0.018,
+        "initialPosition": [0.0, 0.0, 0.05],  # baseline = origin
+    }) is True
+
+
+# 5. Multiple axes (OR semantics) -> True if any matches
+def test_multi_axis_or_semantics():
+    state = _state({"bowl": [0.05, 0.0, 0.05]})  # X moved, Y not
+    assert check_position_delta(state, {
+        "sampleBodyName": "bowl",
+        "axes": ["x", "y"],
+        "minDeltaX": 0.018,
+        "minDeltaY": 0.5,  # not satisfied
+        "initialPosition": [0.0, 0.0, 0.05],
+    }) is True
+
+
+# 6. Missing body -> False (graceful)
+def test_missing_body_returns_false():
+    state = _state({})
+    assert check_position_delta(state, {"sampleBodyName": "bowl"}) is False
+
+
+# 7. Explicit initialPosition
+def test_explicit_initial_position():
+    state = _state({"bowl": [0.020, 0.0, 0.05]})
+    assert check_position_delta(state, {
+        "sampleBodyName": "bowl",
+        "axes": ["x"],
+        "minDeltaX": 0.015,
+        "initialPosition": [0.000, 0.0, 0.05],  # baseline = origin
+    }) is True
+    # Same final position, but baseline shifted: now no movement
+    state2 = _state({"bowl": [0.020, 0.0, 0.05]})
+    assert check_position_delta(state2, {
+        "sampleBodyName": "bowl",
+        "axes": ["x"],
+        "minDeltaX": 0.015,
+        "initialPosition": [0.010, 0.0, 0.05],  # baseline closer
+    }) is False
+
+
+# 8. Z axis only
+def test_z_axis_only():
+    state = _state({"bowl": [0.0, 0.0, 0.10]})
+    assert check_position_delta(state, {
+        "sampleBodyName": "bowl",
+        "axes": ["z"],
+        "minDeltaZ": 0.05,
+        "initialPosition": [0.0, 0.0, 0.05],
+    }) is True
+
+
+# 9. Empty axes list -> False
+def test_empty_axes_returns_false():
+    state = _state({"bowl": [0.0, 0.0, 0.05]})
+    assert check_position_delta(state, {
+        "sampleBodyName": "bowl",
+        "axes": [],
+    }) is False
+
+
+# 10. Invalid axis in list -> filtered out; if all invalid -> False
+def test_invalid_axis_filtered():
+    state = _state({"bowl": [0.02, 0.0, 0.05]})
+    # 'foo' is invalid; 'x' is valid
+    assert check_position_delta(state, {
+        "sampleBodyName": "bowl",
+        "axes": ["foo", "x"],
+        "minDeltaX": 0.018,
+    }) is True
+    # All invalid -> False
+    assert check_position_delta(state, {
+        "sampleBodyName": "bowl",
+        "axes": ["foo", "bar"],
+    }) is False