Files
a7l20-hug-sim/scripts/replan_pinky_low_4cm.py

108 lines
7.0 KiB
Python

"""Recompute 4cm plan using user-resolved pinky-low mount; offline only."""
import json
import hashlib
import xml.etree.ElementTree as ET
import numpy as np
from scipy.spatial.transform import Rotation as R
from search_cup_poses_offline import ROOT,PoseGuard,hand_cloud
from table_contact_escape import ContactEscapeChecker,plan_escape
from table_edge_constraint import TableEdgeConstraint
from table_escape_geometry import EscapeGeometry
def main():
cap=ROOT/'data/left_cup_current';mountfile=ROOT/'config/left_hand_mount.json'
mount=json.loads(mountfile.read_text());M=np.array(mount['rotation_Arm_Tip_to_hand'])
if not np.allclose(M.T@M,np.eye(3)) or np.linalg.det(M)<0 or not np.allclose(M[:,2],[0,0,-1]):
raise ValueError('Invalid fixed mounting rotation')
old=json.loads((cap/'cup_yaw_plan.json').read_text())['solutions'][0];g=old['grasp']
# New observed table plane; no automatic transfer of old scene PASS claims.
edge=json.loads((ROOT/'data/table_edge_check_20260924_01/start_edge_audit.json').read_text())['edge_evidence']
guard=PoseGuard(edge['top_normal_base'],edge['top_point_base_m']);guard.mount=M
guard.hand_center=np.array(mount['translation_L7_to_hand_m'])
reference=guard.fk(mount['reference_arm_joints_rad'])[:3,:3]@M
palm_normal=reference@np.array(mount['palm_normal_local'])
pinky_direction=reference@np.array(mount['pinky_side_local'])
thumb_direction=reference@np.array(mount['thumb_side_local'])
# LEFT is mirrored: pinky=+Y_hand, thumb=-Y_hand.
if not (palm_normal[2]<-.7 and pinky_direction[2]<-.7 and thumb_direction[2]>.7):
raise ValueError('Reference must be palm down, LEFT pinky low and thumb high')
if not np.isclose(np.degrees(np.arccos(abs(palm_normal[2]))),45):
raise ValueError('Reference palm-plane tilt must be 45 degrees')
report={'executable':False,'hardware_connected':False,'motion_sent':False,
'mount_source':str(mountfile),'mount_sha256':hashlib.sha256(mountfile.read_bytes()).hexdigest(),
'reference_check':{'palm_normal_base':palm_normal.tolist(),
'pinky_side_height_sign':float(pinky_direction[2]),
'thumb_side_height_sign':float(thumb_direction[2]),
'left_hand_pinky_side_local':mount['pinky_side_local'],
'status':'pinky_low_45deg_pass'},
'table_source':'data/table_edge_check_20260924_01/start_edge_audit.json',
'object_source':'OLD saved cup hypothesis; not a fresh cup pose validation',
'stages':{},'candidate_raise_m':.04}
p=np.array(g['wrist_pos'])+[0,0,.04]
rot=R.from_quat(np.roll(g['wrist_quat_wxyz'],-1)).as_matrix()@M.T
snapshot=sorted(cap.glob('arm_snapshot_*.json'))[-1]
current=np.array(json.loads(snapshot.read_text())['states']['left']['joint_state']['position'])
midpoint=np.array([(j[3]+j[4])/2 for j in guard.chain])
seeds=[current,np.array(old['grasp_joints']),np.array(old['pregrasp_joints']),midpoint,np.zeros(7)]
try:
q=guard.solve(p,rot,seeds)
if q is None:raise RuntimeError('Corrected-mount local grasp IK not found from 5 seeds')
report['grasp_joints']=q.tolist();print('Corrected mount grasp IK found',flush=True)
direction=np.array(g['approach']);direction/=np.linalg.norm(direction)
guard.cloud=hand_cloud(g['hand_pre']);qp=None
for distance in (.10,.07,.05,.03):
candidate=guard.solve(p-distance*direction,rot,[q]+seeds)
if candidate is None:continue
try:result=guard.check_segment(candidate,q)
except (ValueError,RuntimeError):continue
qp=candidate;report['approach_distance_m']=distance
report['stages']['approach']=result;break
if qp is None:raise RuntimeError('No checked pregrasp IK/path at 10/7/5/3cm')
report['pregrasp_joints']=qp.tolist();print('Corrected mount approach checked',flush=True)
delta={k:abs(g['hand_qpos'][k]-v) for k,v in g['hand_pre'].items()}
for j in ET.parse(ROOT/'assets/l20_left_mj.urdf').getroot().findall('joint'):
m=j.find('mimic')
if m is not None:delta[j.get('name')]=abs(float(m.get('multiplier',1)))*delta[m.get('joint')]
padding=guard.radius*sum(delta.values())/200;minimum=float('inf')
for t in np.linspace(0,1,101):
guard.cloud=hand_cloud({k:(1-t)*v+t*g['hand_qpos'][k] for k,v in g['hand_pre'].items()})
gap,body=guard.clearance(q);minimum=min(minimum,gap)
if gap<guard.margin+padding:raise RuntimeError(f'Closure tabletop rejection: {body}')
report['stages']['closure']={'minimum_gap_m':minimum,'interval_bound_m':padding,'samples':101}
print('Corrected mount closure checked',flush=True)
lift=[];seed=q
for height in (.01,.02,.03,.04,.05):
nxt=guard.solve(p+[0,0,height],rot,[seed,q]+seeds)
if nxt is None:raise RuntimeError(f'Lift local IK not found at {height}m')
result=guard.check_segment(seed,nxt);lift.append(dict(height_m=height,joints=nxt.tolist(),**result));seed=nxt
print('Corrected mount lift checked',height,flush=True)
report['stages']['lift']=lift;report['local_table_check']='pass'
# Start-contact phase is distinct from normal 20mm transit. It never
# ignores negative initial geometry or treats a planned hand as measured.
start_geometry=EscapeGeometry(g['hand_pre'])
start_table=TableEdgeConstraint(edge['top_normal_base'],edge['top_point_base_m'],
edge['inward_normal_base'],edge['edge_point_base_m'],diagnostic=True,
uncertainty_m=edge['uncertainty_m'])
escape_checker=ContactEscapeChecker(start_geometry,start_table,observed_y=edge['observed_y_range_m'])
escape=plan_escape(escape_checker,current)
escape['hand_assumption']='saved planned hand_pre, not measured current hand'
report['conditional_start_escape']=escape
if 'verification' in escape:
cleared=np.asarray(escape['waypoints_rad'][-1])
try:report['conditional_start_transit']=escape_checker.check_normal_segment(cleared,qp)
except (ValueError,RuntimeError) as e:report['conditional_start_transit']={'status':'blocked','reason':str(e)}
else:
report['conditional_start_transit']={'status':'blocked','reason':escape['status']}
except (ValueError,RuntimeError) as e:
report['local_table_check']='blocked';report['reason']=str(e);print(str(e),flush=True)
report['limitations']=['Mount appearance/orientation accepted by user; not a precision metrology certificate.',
'Local IK failure is not global unreachability.',
'Old cup contact hypothesis must be regenerated from new frame before execution.',
'No verified initial transit/actual hand state/self-collision/other-arm/cup collision or controller trajectory.',
'Table check uses observed plane without calibrated total-error bound. Real motion gate remains locked.']
out=cap/'cup_4cm_pinky_low_verification.json';out.write_text(json.dumps(report,indent=2));print(out,flush=True)
if __name__=='__main__':main()