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

104 lines
5.8 KiB
Python

"""Offline staged IK search. Saved data only, no executable motion output."""
import json
import argparse
import tempfile
from pathlib import Path
import numpy as np
from scipy.spatial import ConvexHull
from scipy.spatial.transform import Rotation as R, Slerp
from offline_table_guard import ROOT
from search_cup_poses_offline import PoseGuard
from check_index_board_hover import hand_geometry
from g20_left_lookup import LeftLookup
from left_index_pointing_pose import pointing_target
def main():
parser=argparse.ArgumentParser(description=__doc__)
parser.add_argument('--preflight',type=Path,help='Read arm start from a saved read-only preflight report')
args=parser.parse_args()
cap=ROOT/'data/start_evidence_20260927T053837893105Z'
plane=json.loads((cap/'index_hover_validation.json').read_text())
state=json.loads((cap/'arm_snapshot.json').read_text())['states']['left']
if args.preflight:
preflight=json.loads(args.preflight.read_text())
if preflight['start_check']['status']!='arm_start_matches_only':
raise ValueError('Read-only start check did not pass')
state=preflight['raw_arm_samples'][-1]['state']
q0=np.array(state['joint_state']['position'])
raw=json.loads((ROOT/'data/left_hand_open_q10r1mak/result.json').read_text())['final_feedback']
pointing=pointing_target(raw,LeftLookup())
_,opened,_=hand_geometry(raw);tip,closed,_=hand_geometry(pointing)
opened=opened[ConvexHull(opened).vertices];closed=closed[ConvexHull(closed).vertices]
guard=PoseGuard(plane['table_normal'],plane['table_point_m'])
mount=json.loads((ROOT/'config/left_hand_mount.json').read_text())
guard.mount=np.array(mount['rotation_Arm_Tip_to_hand'])
T0=guard.fk(q0);r0=T0[:3,:3]
wrist_offset=np.array([-.02325,0,-.052]);hand_offset=np.array(mount['translation_Arm_Tip_to_hand_m'])
wrist0=T0[:3,3]+r0@wrist_offset
target=np.array(json.loads((ROOT/'data/index_board_hover_j4jxrlcz/report.json').read_text())['target_base_m'])
def solve_wrist(wrist,rot,seed):
return guard.solve(wrist+rot@hand_offset,rot,[seed])
def path_check(path,cloud):
guard.cloud=cloud;worst=(float('inf'),'');all_checks=[]
for a,b in zip(path,path[1:]):
for t in np.linspace(0,1,31):
gap=guard.clearance(a+(b-a)*t)
if gap[0]<worst[0]:worst=gap
try:all_checks.append(guard.check_segment(a,b))
except (RuntimeError,ValueError) as exc:all_checks.append({'rejected':str(exc)})
return dict(sampled_minimum=worst,continuous_table_pass=all('rejected' not in r for r in all_checks),checks=all_checks)
rows=[]
for height in (.03,.06,.10,.15,.20):
lift=[q0];failed=False
for dz in np.linspace(0,height,7)[1:]:
q=solve_wrist(wrist0+[0,0,dz],r0,lift[-1])
if q is None:failed=True;break
lift.append(q)
if failed:
rows.append(dict(lift_m=height,status='lift_local_ik_failed'));continue
liftcheck=path_check(lift,opened)
pivot=wrist0+[0,0,height]
for yaw in range(-180,180,30):
z=np.array([np.cos(np.deg2rad(yaw)),np.sin(np.deg2rad(yaw)),0.])
x=np.array([0.,0.,-1.]);y=np.cross(z,x)
hand_rot=np.column_stack([x,y,z]);rot=hand_rot@guard.mount.T
interpolation=Slerp([0,1],R.from_matrix(np.array([r0,rot])))
turn=[lift[-1]];failed=False
for f in np.linspace(0,1,13)[1:]:
q=solve_wrist(pivot,interpolation(f).as_matrix(),turn[-1])
if q is None:failed=True;break
turn.append(q)
row=dict(lift_m=height,palm_down_yaw_deg=yaw,lift_check=liftcheck)
if failed:row['status']='rotation_local_ik_failed';rows.append(row);continue
row['rotation_check']=path_check(turn,opened)
desired_hand=target-hand_rot@tip
end=guard.solve(desired_hand,rot,[turn[-1],q0])
if end is None:row['status']='final_local_ik_failed';rows.append(row);continue
row['transit_check']=path_check([turn[-1],end],closed)
row['status']='staged_ik_found_not_validated'
row['lift_joints']=[q.tolist() for q in lift]
row['rotate_joints']=[q.tolist() for q in turn]
row['final_joints']=end.tolist()
tf=guard.fk(end)
actual_tip=tf[:3,3]+tf[:3,:3]@(guard.hand_center+guard.mount@tip)
row['tip_error_m']=float(np.linalg.norm(actual_tip-target))
row['palm_normal_base']=(tf[:3,:3]@guard.mount@np.array([1.,0.,0.])).tolist()
rows.append(row)
print('Completed lift height',height,flush=True)
report=dict(executable=False,motion_sent=False,target_m=target.tolist(),candidates=rows,
start_joints_rad=q0.tolist(),start_source=str(args.preflight) if args.preflight else str(cap/'arm_snapshot.json'),
limitations=['Start clearance unresolved; infinite plane used conservatively, no exemption.',
'Saved states are not fresh execution prerequisites.',
'Rotation holds Arm_Tip position; palm normal is hand local +X.',
'Hand closure at rotated pose not yet swept; no self/other-object checks.',
'Cartesian waypoints are diagnostic; joint-linear segments are not controller interpolation proof.',
'Local IK failures do not establish global unreachability.'])
outdir=Path(tempfile.mkdtemp(prefix='fresh_start_path_',dir=ROOT/'data')) if args.preflight else cap
output=outdir/'lift_rotate_point_validation.json';output.write_text(json.dumps(report,indent=2))
print('Staged IK solutions:',sum(r['status']=='staged_ik_found_not_validated' for r in rows))
print(output)
if __name__=='__main__':main()