Files
a7l20-hug-sim/scripts/verify_pointing_sequence.py
T

106 lines
6.3 KiB
Python

"""Offline sampled pointing/arm-path diagnostics, never an execution certificate."""
import json
import xml.etree.ElementTree as ET
import itertools
import numpy as np
import mujoco
from offline_table_guard import ROOT
from audit_table_edge_start import EdgePoseGuard
from table_edge_constraint import TableEdgeConstraint
from g20_left_lookup import LeftLookup
from left_index_pointing_pose import pointing_target
def main():
cap=ROOT/'data/start_evidence_20260927T053837893105Z'
plane=json.loads((cap/'index_hover_validation.json').read_text())
plans=json.loads((ROOT/'data/index_board_hover_j4jxrlcz/report.json').read_text())
state=json.loads((cap/'arm_snapshot.json').read_text())['states']['left']
q=np.array(state['joint_state']['position'])
raw=json.loads((ROOT/'data/left_hand_open_q10r1mak/result.json').read_text())['final_feedback']
lookup=LeftLookup();target=pointing_target(raw,lookup)
path=ROOT.parent/'Linkerbot_URDF_cal/L20/LEFT/linkerhand_g20_left.urdf'
xml=ET.parse(path).getroot()
extension=xml.find('mujoco')
if extension is None:extension=ET.SubElement(xml,'mujoco')
compiler=extension.find('compiler')
if compiler is None:compiler=ET.SubElement(extension,'compiler')
# Geometry/FK only, never integrate. Repair invalid CAD inertia in memory.
compiler.set('balanceinertia','true')
for mesh in xml.findall('.//mesh'):
filename=mesh.get('filename')
if filename:mesh.set('filename',str((path.parent/filename).resolve()))
compiler.set('strippath','false')
model=mujoco.MjModel.from_xml_string(ET.tostring(xml,encoding='unicode'))
data=mujoco.MjData(model)
vertices={}
for i in range(model.ngeom):
if model.geom_type[i]!=mujoco.mjtGeom.mjGEOM_MESH:raise ValueError('Unsupported geometry')
mid=model.geom_dataid[i];start=model.mesh_vertadr[mid];count=model.mesh_vertnum[mid]
vertices[i]=model.mesh_vert[start:start+count]
def group(i):
name=model.body(model.geom_bodyid[i]).name
return name.split('_')[0]
pairs=[(i,j) for i,j in itertools.combinations(range(model.ngeom),2)
if group(i)!=group(j) and group(i) in ('thumb','index','middle','ring','pinky')
and group(j) in ('thumb','index','middle','ring','pinky')]
def set_hand(rows):
for name,value in lookup.describe(rows).items():
data.qpos[model.joint(name).qposadr[0]]=value['table_angle_rad']
mujoco.mj_forward(model,data)
return np.concatenate([v@data.geom_xmat[i].reshape(3,3).T+data.geom_xpos[i] for i,v in vertices.items()])
mount=json.loads((ROOT/'config/left_hand_mount.json').read_text())
guard=EdgePoseGuard(plane['table_normal'],plane['table_point_m'])
guard.mount=np.array(mount['rotation_Arm_Tip_to_hand'])
# Same observed edge reconstructed from the saved pixel endpoints and plane.
camera=json.loads((cap/'camera.json').read_text());K=np.array(camera['K']);T=np.array(camera['T_world_cam_opencv'])
uv=np.array(plane['edge_pixels']);rays=np.c_[(uv[:,0]-K[0,2])/K[0,0],(uv[:,1]-K[1,2])/K[1,1],np.ones(2)]@T[:3,:3].T
n=np.array(plane['table_normal']);p=np.array(plane['table_point_m'])
ep=T[:3,3]+rays*(((p-T[:3,3])@n)/(rays@n))[:,None]
inward=np.cross(n,ep[1]-ep[0]);inward/=np.linalg.norm(inward)
if (np.array(plans['target_base_m'])-ep.mean(axis=0))@inward<0:inward=-inward
guard.edge=TableEdgeConstraint(n,p,inward,ep.mean(axis=0),diagnostic=True,uncertainty_m=.025)
guard.observed_y=plane['observed_edge_y_m']
closure=[];fromto=np.empty(6)
for fraction in np.linspace(0,1,321):
rows=np.rint((1-fraction)*np.array(raw)+fraction*np.array(target)).astype(int).tolist()
guard.cloud=set_hand(rows)
worst=(.1,None)
for i,j in pairs:
distance=float(mujoco.mj_geomDistance(model,data,i,j,.1,fromto))
if distance<worst[0]:worst=(distance,[model.body(model.geom_bodyid[k]).name for k in (i,j)])
transform=guard.fk(q)
world=(guard.cloud@guard.mount.T+guard.hand_center)@transform[:3,:3].T+transform[:3,3]
closure.append(dict(fraction=float(fraction),interfinger_convex_gap_m=worst[0],pair=worst[1],
hand_infinite_plane_gap_m=float(((world-p)@n).min())-.025,
full_body_clearance=guard.clearance(q)))
guard.cloud=set_hand(target)
paths=[]
for index,candidate in enumerate(plans['candidates']):
end=np.array(candidate['joints_rad']);samples=[]
for f in np.linspace(0,1,201):
gap,body=guard.clearance((1-f)*q+f*end)
samples.append(dict(fraction=float(f),gap_m=gap,body=body))
try:certificate=guard.check_segment(q,end)
except (RuntimeError,ValueError) as exc:certificate={'rejected':str(exc)}
paths.append(dict(candidate=index,minimum=min(samples,key=lambda s:s['gap_m']),
endpoint=samples[-1],samples=samples,continuous_table_check=certificate))
report=dict(executable=False,motion_sent=False,lookup=lookup.provenance(),planned_hand_raw=target,
closure_samples=closure,arm_paths=paths,
inter_finger_pairs_checked=len(pairs),
worst_interfinger=min(closure,key=lambda c:c['interfinger_convex_gap_m']),
hand_geometry_scope='Calibrated URDF geometry, all21 JSON angles; in-memory balanceinertia; no dynamics.',
blockers=['Current start outside observed edge coverage; negative conservative clearance unresolved.',
'Inter-finger convex checks exclude same-finger, palm, arm, adapter and other-arm contacts.',
'321 hand and201 arm samples do not certify unsampled motion or asynchronous finger tracking.',
'Saved hand feedback is not live/synchronized; physical closure feedback still untested.',
'Other scene obstacles and real controller interpolation unverified.'])
(cap/'pointing_sequence_validation.json').write_text(json.dumps(report,indent=2))
print('Closure samples:',len(closure),'interfinger pairs:',len(pairs))
print('Worst interfinger:',report['worst_interfinger'])
print('Full sampled arm paths:',len(paths),'continuous table passes:',sum('rejected' not in r['continuous_table_check'] for r in paths))
print(cap/'pointing_sequence_validation.json')
if __name__=='__main__':main()