104 lines
5.8 KiB
Python
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()
|