55 lines
2.6 KiB
Python
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()
|