fix: retain planning feedback and add acceptance probes

2026-09-22 00:47:11 +08:00
parent f9d8feb6f0
commit 91bcd92d6b
12 changed files with 871 additions and 10 deletions
@@ -0,0 +1,151 @@
+#!/usr/bin/env python3
+"""CPU physics fixture for delivery evidence; not robot or NAV/VLA acceptance.
+
+Requires pybullet==3.2.7 for the CLI; the independent predicate uses stdlib.
+Objects fall under gravity into generic trays. No robot, grasp controller,
+perception model, ROS endpoint or production VerifyState server is exercised.
+"""
+import argparse
+import json
+import math
+from pathlib import Path
+import time
+
+
+def verify_delivery(frames, bounds, body_id, destination_id):
+    """Require 0.25 simulated seconds of fresh, continuous, released stability.
+
+    Simulator truth is privileged test evidence, not a production sensor.
+    Bounds describe the tray interior. Whole AABB containment is conservative.
+    """
+    def reject(reason):
+        return dict(verified=False, reason=reason)
+    try:
+        if not frames:
+            return reject('missing_evidence')
+        if (len(bounds) != 2 or any(len(v) != 3 for v in bounds) or
+                not all(math.isfinite(x) for v in bounds for x in v) or
+                any(bounds[0][i] >= bounds[1][i] for i in range(3))):
+            return reject('invalid_bounds')
+        previous = None
+        for frame in frames:
+            stamp = frame['time']
+            if not math.isfinite(stamp) or (previous is not None and stamp <= previous):
+                return reject('invalid_time')
+            previous = stamp
+        end = frames[-1]['time']
+        selected = [f for f in frames if f['time'] >= end - .25 - 1e-9]
+        if len(selected) < 2 or end - selected[0]['time'] < .25 - 1e-9:
+            return reject('insufficient_stability_window')
+        previous = None
+        for frame in selected:
+            stamp = frame['time']
+            if not math.isfinite(stamp) or (previous is not None and
+                    not 0 < stamp - previous <= .02):
+                return reject('discontinuous_time')
+            previous = stamp
+            if frame['body_id'] != body_id or frame['destination_id'] != destination_id:
+                return reject('identity_mismatch')
+            if frame['held'] is not False:
+                return reject('not_released')
+            if frame['floor_contact'] is not True:
+                return reject('no_destination_support')
+            speeds = [frame['linear_speed'], frame['angular_speed']]
+            if not all(math.isfinite(x) and x >= 0 for x in speeds):
+                return reject('invalid_velocity')
+            if speeds[0] > .02 or speeds[1] > .1:
+                return reject('not_stationary')
+            box = frame['aabb']
+            if (len(box) != 2 or any(len(v) != 3 for v in box) or
+                    not all(math.isfinite(x) for v in box for x in v) or
+                    any(box[0][i] > box[1][i] for i in range(3))):
+                return reject('invalid_geometry')
+            if any(box[0][i] < bounds[0][i] - .001 or
+                   box[1][i] > bounds[1][i] + .001 for i in range(3)):
+                return reject('outside_destination')
+        return dict(verified=True, reason='released_supported_contained_stable')
+    except (KeyError, IndexError, TypeError, ValueError, OverflowError):
+        return reject('invalid_evidence')
+
+
+def run_probe():
+    import pybullet as p
+    from importlib.metadata import version
+    started = time.monotonic()
+    client = p.connect(p.DIRECT)
+    cases = []
+    try:
+        # Reset per case so no earlier object's state can satisfy later proof.
+        for name, xyz, steps, hold, velocity, expected in (
+            ('settled_in_correct_tray', (0, 0, .25), 480, False, None, True),
+            ('settled_in_wrong_tray', (.7, 0, .25), 480, False, None, False),
+            ('outside_both_trays', (1.3, 0, .25), 480, False, None, False),
+            ('brief_transit_above_tray', (0, 0, .25), 30, False, None, False),
+            ('held_inside_tray', (0, 0, .10), 480, True, None, False),
+            ('moving_inside_tray', (0, 0, .10), 480, False, (.3, 0, 0), False),
+        ):
+            p.resetSimulation(physicsClientId=client)
+            p.setGravity(0, 0, -9.81, physicsClientId=client)
+            p.setTimeStep(1 / 240, physicsClientId=client)
+            p.setPhysicsEngineParameter(numSolverIterations=100, physicsClientId=client)
+            def box(half, pos, mass=0):
+                shape = p.createCollisionShape(p.GEOM_BOX, halfExtents=half, physicsClientId=client)
+                return p.createMultiBody(baseMass=mass, baseCollisionShapeIndex=shape,
+                                         basePosition=pos, physicsClientId=client)
+            box([2, 2, .02], [0, 0, -.04])
+            floors = []
+            for x in (0., .7):
+                floors.append(box([.21, .21, .01], [x, 0, -.01]))
+                for sign in (-1, 1):
+                    box([.01, .21, .15], [x + sign * .21, 0, .15])
+                    box([.21, .01, .15], [x, sign * .21, .15])
+            obj = box([.025, .025, .025], xyz, .1)
+            if hold:
+                p.createConstraint(obj, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0],
+                                   [0, 0, 0], xyz, physicsClientId=client)
+            frames = []
+            for step in range(steps):
+                # The negative moving fixture applies velocity only; no pose
+                # teleport or manufactured result is used as measured evidence.
+                if velocity and step >= steps - 60:
+                    p.resetBaseVelocity(obj, velocity, physicsClientId=client)
+                p.stepSimulation(physicsClientId=client)
+                lin, ang = p.getBaseVelocity(obj, physicsClientId=client)
+                held = any(p.getConstraintInfo(p.getConstraintUniqueId(i, physicsClientId=client),
+                           physicsClientId=client)[0] == obj
+                           for i in range(p.getNumConstraints(physicsClientId=client)))
+                frames.append(dict(time=(step + 1) / 240, body_id=obj,
+                    destination_id=floors[0], aabb=p.getAABB(obj, physicsClientId=client),
+                    linear_speed=math.sqrt(sum(x*x for x in lin)),
+                    angular_speed=math.sqrt(sum(x*x for x in ang)), held=held,
+                    floor_contact=bool(p.getContactPoints(obj, floors[0], physicsClientId=client))))
+            verdict = verify_delivery(frames, [[-.2, -.2, 0], [.2, .2, .3]], obj, floors[0])
+            cases.append(dict(case=name, expected_verified=expected, verdict=verdict,
+                              passed=verdict['verified'] == expected,
+                              simulated_seconds=steps / 240, evidence_window=frames[-61:]))
+        return dict(scope='generic_delivery_predicate_physics_fixture',
+                    simulator='PyBullet', version=version('pybullet'),
+                    renderer='DIRECT', wall_seconds=time.monotonic()-started,
+                    passed=all(c['passed'] for c in cases), cases=cases,
+                    target_robot_acceptance=False, isaac_sim_executed=False,
+                    production_verify_state_integrated=False,
+                    grasp_navigation_perception_exercised=False)
+    finally:
+        p.disconnect(physicsClientId=client)
+
+
+def main():
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument('--output', type=Path, required=True)
+    args = parser.parse_args()
+    result = run_probe()
+    args.output.parent.mkdir(parents=True, exist_ok=True)
+    temporary = args.output.with_suffix(args.output.suffix + '.tmp')
+    temporary.write_text(json.dumps(result, indent=2, allow_nan=False) + '\n')
+    temporary.replace(args.output)
+    print(json.dumps({k: v for k, v in result.items() if k != 'cases'}))
+    raise SystemExit(0 if result['passed'] else 1)
+
+
+if __name__ == '__main__':
+    main()