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

55 lines
2.6 KiB
Python

"""Read-only left-ring IK audit. Never sends arm or hand movement commands."""
import json
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation as Rot
from real_arm import RealArm
ROOT = Path(__file__).resolve().parents[1]
def main():
cap = ROOT / 'data/left_ring_current'
info = json.loads((ROOT / 'scene/a7l20_scene.json').read_text())
q = info['hands']['left']['mount_quat_wxyz']
sim_mount = Rot.from_quat([q[1], q[2], q[3], q[0]]).as_matrix()
arm = RealArm('left', table_z=-0.2471)
report = {'executed': False, 'table_z_m': -0.2471,
'mount_assumption': 'current flange pose is the user comparison reference; +45 deg about base Z',
'candidates': []}
try:
state = arm.read()
F = state['R'].as_matrix()
# Express the user's top-view rotation in the reference flange frame.
mount = F.T @ Rot.from_euler('z', 45, degrees=True).as_matrix() @ F @ sim_mount
report['reference_flange_quat_xyzw'] = state['quat']
report['mount_rotation'] = mount.tolist()
for path in sorted(cap.glob('grasp_l20_ring_*.json')):
g = json.loads(path.read_text())
q = g['wrist_quat_wxyz']
hand_R = Rot.from_quat([q[1], q[2], q[3], q[0]]).as_matrix()
flange_R = Rot.from_matrix(hand_R @ mount.T)
pos = np.array(g['wrist_pos'])
approach = np.array(g['approach']); approach /= np.linalg.norm(approach)
pre = pos - 0.10 * approach
jp = arm.ik(pre, flange_R, seed=state['joints'])
jg = arm.ik(pos, flange_R, seed=jp if jp is not None else state['joints'])
item = {'source': path.name, 'pregrasp_base': pre.tolist(), 'grasp_base': pos.tolist(),
'pregrasp_ik': jp is not None, 'grasp_ik': jg is not None,
'thumb_height_above_measured_table_m': g['metrics']['thumb_h_cm']/100 - (-0.265) + report['table_z_m']}
# Height in the candidate is relative to -0.265 m.
item['thumb_height_above_measured_table_m'] = g['metrics']['thumb_h_cm']/100 + (-0.265) - report['table_z_m']
if jp is not None:
item['pregrasp_joints'] = jp.tolist()
fk = arm.fk(jp)
item['fk_error_m'] = None if fk is None else float(np.linalg.norm(fk-pre))
report['candidates'].append(item)
print(json.dumps(item, ensure_ascii=False), flush=True)
finally:
arm.close()
(cap / 'left_plan_audit.json').write_text(json.dumps(report, indent=2))
if __name__ == '__main__':
main()