106 lines
6.3 KiB
Python
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()
|