"""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