152 lines
7.6 KiB
Python
152 lines
7.6 KiB
Python
|
#!/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()
|