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

91 lines
5.5 KiB
Python

"""Saved-data diagnostic: raise fingertip 10cm, then hover; no hardware API."""
import json
import argparse
import tempfile
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
from scipy.optimize import least_squares
from offline_table_guard import ROOT
from search_cup_poses_offline import PoseGuard
from check_index_board_hover import hand_geometry
from left_index_pointing_pose import pointing_target
from g20_left_lookup import LeftLookup
from audit_finite_table_path import JsonHandGeometry
from finite_table_constraint import NearEdgeSlab
from table_edge_constraint import check_joint_linear_segment
def read(path):
return json.loads((ROOT/path).read_text())
def main():
parser=argparse.ArgumentParser(description=__doc__)
parser.add_argument('--ik-only',action='store_true',help='Offline only: omit table checks; never authorizes motion')
parser.add_argument('--orientation-tolerance-deg',type=float,default=1.)
args=parser.parse_args()
if not np.isfinite(args.orientation_tolerance_deg) or not 1 <= args.orientation_tolerance_deg <= 15:
parser.error('orientation tolerance must be between 1 and 15 degrees')
source='data/elbow_raise_preflight_b68edg6f/report.json'
q0=np.array(read(source)['raw_arm_samples'][-1]['state']['joint_state']['position'])
raw=pointing_target(read('data/left_hand_open_q10r1mak/result.json')['final_feedback'],LeftLookup())
tip,_,_=hand_geometry(raw)
mount=read('config/left_hand_mount.json')
measured=read('data/measured_near_edge__v4ivh45/report.json')
guard=PoseGuard(measured['top_normal'],measured['top_point'])
# solve() then targets the actual index tip, rather than the hand origin.
guard.hand_center=np.array(mount['translation_L7_to_hand_m'])+np.array(mount['rotation_Arm_Tip_to_hand'])@tip
t0=guard.fk(q0);rotation=t0[:3,:3]
start=t0[:3,3]+rotation@guard.hand_center
target=np.array(read('data/index_board_hover_j4jxrlcz/report.json')['target_base_m'])
geometry=None if args.ik_only else JsonHandGeometry(raw)
table=NearEdgeSlab(measured['top_normal'],measured['top_point'],measured['outward_base'],measured['near_edge_base_m'],.02,uncertainty_m=0,verified=True,source='saved nominal diagnostic')
def clearance(q):
return min((max(table.bounds(v['points']))[0],name) for name,v in geometry.sample(q).items())
path=[q0];rows=[];status='ik_waypoints_found'
lifted=start+[0,0,.10]
for stage,a,b in [('lift_tip_10cm',start,lifted),('transit_to_hover',lifted,target)]:
for fraction in np.linspace(0,1,21)[1:]:
point=a+(b-a)*fraction
q=guard.solve(point,rotation,[path[-1],q0])
if q is None and args.orientation_tolerance_deg>1:
lower=np.array([c[3] for c in guard.chain]);upper=np.array([c[4] for c in guard.chain])
tolerance=np.deg2rad(args.orientation_tolerance_deg)
def residual(joints):
tf=guard.fk(joints)
delta=Rotation.from_matrix(rotation.T@tf[:3,:3]).as_rotvec()
angle=np.linalg.norm(delta)
excess=delta*max(0.,1-(tolerance-1e-4)/max(angle,1e-12))
return np.r_[tf[:3,3]+tf[:3,:3]@guard.hand_center-point,.2*excess,1e-5*(joints-path[-1])]
fit=least_squares(residual,path[-1],bounds=(lower,upper),max_nfev=500,ftol=1e-10,xtol=1e-10,gtol=1e-10)
tf=guard.fk(fit.x)
if np.linalg.norm(tf[:3,3]+tf[:3,:3]@guard.hand_center-point)<.002 and Rotation.from_matrix(rotation.T@tf[:3,:3]).magnitude()<=tolerance:
q=fit.x
if q is None:
rows.append(dict(stage=stage,fraction=float(fraction),status='local_ik_not_found',target=point.tolist()))
status='local_ik_not_found';break
tf=guard.fk(q)
row=dict(stage=stage,fraction=float(fraction),tip_error_m=float(np.linalg.norm(tf[:3,3]+tf[:3,:3]@guard.hand_center-point)),orientation_error_deg=float(np.rad2deg(Rotation.from_matrix(rotation.T@tf[:3,:3]).magnitude())))
if args.ik_only:
row['table_status']='not_checked_offline_ik_only'
rows.append(row);path.append(q)
continue
row['end_clearance']=clearance(q)
try:
row['table_check']=check_joint_linear_segment(path[-1],q,clearance,reach_bound_m=geometry.reach)
row['table_status']='nominal_segment_pass'
except (RuntimeError,ValueError) as exc:
row['table_status']='not_verified';row['reason']=str(exc)
rows.append(row);path.append(q)
if status=='local_ik_not_found':break
out=Path(tempfile.mkdtemp(prefix='index_keep_pose_',dir=ROOT/'data'))/'report.json'
report=dict(executable=False,motion_sent=False,status=status,source=source,target_base_m=target.tolist(),start_tip_base_m=start.tolist(),lift_tip_target_m=lifted.tolist(),planned_hand_raw=raw,waypoints=[q.tolist() for q in path],segments=rows,limitations=['Saved states, not live start.','Fixed pointing shape; hand transition not verified here.','10cm is fingertip translation, not elbow rise.','Nominal table geometry, no measured uncertainty bound.','Normal margin check does not certify or model initial contact escape.','No self-collision, controller interpolation or execution verification.'])
out.write_text(json.dumps(report,indent=2))
print(status,'IK segments',len(path)-1,'table passes',sum(r.get('table_status')=='nominal_segment_pass' for r in rows))
print(out)
if __name__=='__main__':
main()