108 lines
7.0 KiB
Python
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()
|