diff --git a/tests/test_body_contact.py b/tests/test_body_contact.py new file mode 100644 index 0000000..4ed04fd --- /dev/null +++ b/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 diff --git a/tests/test_ordered_step.py b/tests/test_ordered_step.py new file mode 100644 index 0000000..cc3afbd --- /dev/null +++ b/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 diff --git a/tests/test_position_delta.py b/tests/test_position_delta.py new file mode 100644 index 0000000..9ed24d8 --- /dev/null +++ b/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 diff --git a/util/check_body_contact.py b/util/check_body_contact.py new file mode 100644 index 0000000..062c663 --- /dev/null +++ b/util/check_body_contact.py @@ -0,0 +1,145 @@ +""" +Body contact checker for offline trajectory validation. + +This is the Python counterpart to +``AxisWebInfra/src/components/mujoco-framework-next/checkers/BodyContactChecker.js`` +and MUST stay in sync with it. + +The two implementations are contractually linked: the JS side runs in the +browser during teleoperation, the Python side runs offline on saved +trajectories, and they MUST agree on the success condition. Any future +change to the algorithm must be made in both repos. + +Public API +---------- + +``check_body_contact(state, opts) -> bool`` + +Where ``state`` is a single trajectory step dict of the form +``{"object_positions": {...}, "object_orientations": {...}, ...}`` +and ``opts`` is a dict with the same keys as the Hub task JSON: + + { + "bodyAName": "akita_black_bowl_1_main", + "bodyBName": "plate_1_main", + "maxContactDistance": 0.02, # meters; default 0 + "minContactCount": 1 # default 1 + } + +Because the Python side doesn't have the live MuJoCo contact array, we +approximate contact using a per-body axis-aligned bounding box (AABB) +intersection test on the body's position. This is a deliberate trade-off: + + - It is cheap (no MuJoCo needed; runs on any saved trajectory). + - It is conservative: it may say "in contact" when the actual MuJoCo + contact would say "not in contact" (because AABBs over-approximate the + body shape), but never the other way around. This is the safe direction + for a "is the task done" check. + - It supports ``maxContactDistance > 0`` for soft-contact semantics by + inflating the AABBs by the given distance before testing. + +If you need true geometric contact in offline analysis, see +``AxisDataCleaning/util/contact_util.py`` (a future work item) which will +load the MuJoCo model and replay contacts. +""" + +from __future__ import annotations + +from typing import Any, Dict, List, Optional, Tuple + +# Default AABB extents (half-sizes) for objects that don't have a registered +# extent. Tuned for LIBERO-style tabletop manipulation: most objects fit in +# a 5 cm cube. Override per-call via opts["default_half_extent"]. +DEFAULT_HALF_EXTENT = 0.025 + + +def _aabb_for_body( + state: Dict, + body_name: str, + default_half_extent: float, +) -> Optional[Tuple[float, float, float, float, float, float]]: + """Return the axis-aligned bounding box ``(xmin, ymin, zmin, xmax, ymax, zmax)`` + for ``body_name`` in this step, or ``None`` if the body is missing. + """ + positions = state.get("object_positions") or {} + pos = None + for key, value in positions.items(): + if key == body_name or key.endswith("/" + body_name) or key.endswith(body_name): + pos = value + break + if pos is None or not isinstance(pos, (list, tuple)) or len(pos) < 3: + return None + try: + x = float(pos[0]) + y = float(pos[1]) + z = float(pos[2]) + except (TypeError, ValueError): + return None + # If the state has explicit extents for this body, prefer them. + extents = state.get("object_extents") or {} + hx, hy, hz = default_half_extent, default_half_extent, default_half_extent + for key, value in extents.items(): + if key == body_name or key.endswith("/" + body_name) or key.endswith(body_name): + if isinstance(value, (list, tuple)) and len(value) >= 3: + hx, hy, hz = (abs(float(value[0])), abs(float(value[1])), abs(float(value[2]))) + break + return (x - hx, y - hy, z - hz, x + hx, y + hy, z + hz) + + +def _aabb_intersect_with_distance( + a: Tuple[float, float, float, float, float, float], + b: Tuple[float, float, float, float, float, float], + max_distance: float, +) -> bool: + """``True`` iff the two AABBs intersect, or the closest-point distance + between them is within ``max_distance``. Zero-distance is the standard + "AABB intersection" test. + """ + a_min = (a[0] - max_distance, a[1] - max_distance, a[2] - max_distance) + a_max = (a[3] + max_distance, a[4] + max_distance, a[5] + max_distance) + # AABB intersection with inflation == closest-point distance <= max_distance. + if a_max[0] < b[0] or a_min[0] > b[3]: + return False + if a_max[1] < b[1] or a_min[1] > b[4]: + return False + if a_max[2] < b[2] or a_min[2] > b[5]: + return False + return True + + +def check_body_contact(state: Dict, opts: Dict) -> bool: + """Return ``True`` if ``body_a`` and ``body_b`` are in (or near) contact. + + Mirrors the JS ``BodyContactChecker.check()`` semantics: + - Reads the two body names from ``opts["body_a"]`` / ``opts["bodyAName"]`` + and ``opts["body_b"]`` / ``opts["bodyBName"]``. + - Reads ``opts["max_contact_distance"]`` / ``opts["maxContactDistance"`` (default 0). + - Reads ``opts["min_contact_count"]`` / ``opts["minContactCount"]`` (default 1). + - Returns ``True`` iff at least ``min_contact_count`` AABB intersection + (or near-intersection within ``max_contact_distance``) is observed in + the current ``state``. With one AABB per body, this is equivalent + to "the two bodies overlap". + """ + body_a = opts.get("body_a") or opts.get("bodyAName") or opts.get("body_a_name") + body_b = opts.get("body_b") or opts.get("bodyBName") or opts.get("body_b_name") + if not body_a or not body_b: + return False + if body_a == body_b: + return False + + max_distance = float(opts.get("max_contact_distance", opts.get("maxContactDistance", 0.0)) or 0.0) + if max_distance < 0: + return False + + min_count = int(opts.get("min_contact_count", opts.get("minContactCount", 1)) or 1) + if min_count < 1: + min_count = 1 + + default_half_extent = float(opts.get("default_half_extent", DEFAULT_HALF_EXTENT)) + + aabb_a = _aabb_for_body(state, body_a, default_half_extent) + aabb_b = _aabb_for_body(state, body_b, default_half_extent) + if aabb_a is None or aabb_b is None: + return False + + return _aabb_intersect_with_distance(aabb_a, aabb_b, max_distance) and min_count <= 1 diff --git a/util/check_ordered_step.py b/util/check_ordered_step.py new file mode 100644 index 0000000..4878d16 --- /dev/null +++ b/util/check_ordered_step.py @@ -0,0 +1,65 @@ +""" +Ordered step checker for offline trajectory validation. + +Python counterpart to +``AxisWebInfra/src/components/mujoco-framework-next/checkers/OrderedStepChecker.js``. + +Must stay in sync with the JS implementation. + +Public API +---------- + +``check_ordered_step(steps: List[Dict], opts: Dict) -> bool`` + +Unlike the other Python checkers which are stateless per call, this one is +inherently stateful across a trajectory: a step is "completed" at some frame, +and later frames only need to satisfy the *next* unfinished step. + +The function accepts a **list of state steps** (the whole trajectory) and +walks it once, maintaining the current-step cursor, exactly like the JS +stateful checker does across `check()` calls. + +``opts`` matches the Hub task JSON schema: + + { + "steps": [ + {"type": "RelativePositionBoundsChecker", ...}, + {"type": "GripperOpenChecker", ...} + ], + "allow_reentry": false + } + +Returns ``True`` iff, after walking all frames, every step has been satisfied +in order. +""" + +from __future__ import annotations + +from typing import Any, Dict, List + + +def check_ordered_step(steps: List[Dict], opts: Dict) -> bool: + """Return True iff all sub-steps are satisfied in order across the + given trajectory ``steps`` (a list of state dicts). + """ + sub_configs = opts.get("steps") or opts.get("checkers") or [] + if not isinstance(sub_configs, list) or len(sub_configs) == 0: + return False + + # The dispatcher is provided by the caller via opts["dispatch"] so that + # we don't hard-code every checker here and keep it decoupled from the + # concrete check_* implementations in validate_offline_trajectory.py. + dispatch = opts.get("dispatch") + if not callable(dispatch): + # No dispatcher provided; we cannot evaluate sub-steps. + return False + + current = 0 + n = len(sub_configs) + for state in steps: + if current >= n: + break + cfg = sub_configs[current] + if dispatch(state, cfg): + current += 1 + return current >= n diff --git a/util/check_position_delta.py b/util/check_position_delta.py new file mode 100644 index 0000000..efb506a --- /dev/null +++ b/util/check_position_delta.py @@ -0,0 +1,130 @@ +""" +Position delta checker for offline trajectory validation. + +Python counterpart to +``AxisWebInfra/src/components/mujoco-framework-next/checkers/PositionDeltaChecker.js``. + +Must stay in sync with the JS implementation. + +Public API +---------- + +``check_position_delta(state, opts) -> bool`` + +Where ``state`` is a single trajectory step dict +(``{"object_positions": {...}, ...}``) and ``opts`` matches the Hub task +JSON schema: + + { + "sampleBodyName": "akita_black_bowl_1_main", + "axes": ["x", "y"], + "minDeltaX": 0.018, # body must have moved +18 mm on X + "maxDeltaX": -0.018, # OR -18 mm on X + "minDeltaY": 0.018, + "maxDeltaY": -0.018, + "captureRuntimeInitial": True, # default False + "initialPosition": [x, y, z] # optional explicit baseline + } + +The Python side is **stateless per call** (unlike the JS side, which keeps +a runtime baseline). The caller is responsible for passing the right +``initialPosition`` (typically the first step's position in the +trajectory) when ``captureRuntimeInitial=False``. +""" + +from __future__ import annotations + +from typing import Any, Dict, List, Optional, Tuple + +_AXIS_INDEX = {"x": 0, "y": 1, "z": 2} + + +def _get_body_position(state: Dict, body_name: str) -> Optional[Tuple[float, float, float]]: + positions = state.get("object_positions") or {} + for key, value in positions.items(): + if key == body_name or key.endswith("/" + body_name) or key.endswith(body_name): + if isinstance(value, (list, tuple)) and len(value) >= 3: + try: + return (float(value[0]), float(value[1]), float(value[2])) + except (TypeError, ValueError): + return None + return None + + +def _num(*candidates) -> Optional[float]: + for v in candidates: + if v is None: + continue + try: + n = float(v) + except (TypeError, ValueError): + continue + if n == n: # not NaN + return n + return None + + +def check_position_delta(state: Dict, opts: Dict) -> bool: + """Return ``True`` if the named body's position has moved past one of + the configured per-axis bounds relative to the baseline. + """ + body = opts.get("sample_body") or opts.get("sampleBodyName") or opts.get("sample_body_name") + if not body: + return False + + cur = _get_body_position(state, body) + if cur is None: + return False + + axes = opts.get("axes", ["x", "y"]) + if not isinstance(axes, list) or len(axes) == 0: + return False + axes = [str(a).lower() for a in axes if str(a).lower() in _AXIS_INDEX] + if not axes: + return False + + # Baseline: explicit initialPosition > captureRuntimeInitial (caller-supplied + # baseline) > state["initial_position"] (per-state) > current position + # (degenerate: delta is 0; check will only pass if a bound is 0). + baseline = None + init = opts.get("initialPosition") + if isinstance(init, (list, tuple)) and len(init) >= 3: + try: + baseline = (float(init[0]), float(init[1]), float(init[2])) + except (TypeError, ValueError): + baseline = None + if baseline is None and "initial_position" in state: + ip = state["initial_position"] + if isinstance(ip, dict): + for k in ("x", "y", "z"): + ip.setdefault(k, 0.0) + try: + baseline = (float(ip["x"]), float(ip["y"]), float(ip["z"])) + except (TypeError, ValueError, KeyError): + baseline = None + elif isinstance(ip, (list, tuple)) and len(ip) >= 3: + try: + baseline = (float(ip[0]), float(ip[1]), float(ip[2])) + except (TypeError, ValueError): + baseline = None + if baseline is None: + # No baseline available. This is a stateless, per-call function, so it + # cannot "capture the first frame" the way the stateful JS checker does + # with `captureRuntimeInitial`. Conservatively report no movement + # (delta = 0) rather than guessing a baseline that could produce a + # false positive. Callers doing offline validation MUST pass the + # trajectory's initial position via `initialPosition` (or a per-step + # `state["initial_position"]`) — exactly the baseline the JS side would + # have captured as its runtime initial. + return False + + for ax in axes: + idx = _AXIS_INDEX[ax] + delta = cur[idx] - baseline[idx] + hi = _num(opts.get("min_delta_" + ax), opts.get("minDelta" + ax.upper())) + lo = _num(opts.get("max_delta_" + ax), opts.get("maxDelta" + ax.upper())) + if hi is not None and delta >= hi: + return True + if lo is not None and delta <= lo: + return True + return False diff --git a/util/validate_offline_trajectory.py b/util/validate_offline_trajectory.py index c536879..509b25a 100644 --- a/util/validate_offline_trajectory.py +++ b/util/validate_offline_trajectory.py @@ -28,6 +28,17 @@ import sys from typing import Any, Dict, List, Optional, Tuple +# === parity checkers (PR-D) === +try: + from check_body_contact import check_body_contact + from check_position_delta import check_position_delta + from check_ordered_step import check_ordered_step +except Exception: + check_body_contact = None + check_position_delta = None + check_ordered_step = None + + # ---------- 从 state 中按名称取位姿/关节 ---------- @@ -297,6 +308,14 @@ def run_checker(state: Dict, config: Dict) -> bool: return check_joint_threshold(state, opts) if t == "BoxJointPositionChecker": return check_box_joint_position(state, opts) + if t == "BodyContactChecker" and check_body_contact is not None: + return check_body_contact(state, opts) + if t == "PositionDeltaChecker" and check_position_delta is not None: + return check_position_delta(state, opts) + if t == "OrderedStepChecker" and check_ordered_step is not None: + # OrderedStepChecker 接受一个完整轨迹(steps 列表)而不是单 state; + # 调用方需把轨迹传入 state['_trajectory']。 若不存在则返回 False。 + return check_ordered_step(state.get("_trajectory", [state]), opts) if t == "KeyPressSetChecker": return True # 未知类型视为不通过或可配置