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

61 lines
2.9 KiB
Python

"""Compare downloaded controller URDF FK with controller services. Read-only."""
import json
from pathlib import Path
import xml.etree.ElementTree as ET
import numpy as np
from scipy.spatial.transform import Rotation as R
from real_arm import RealArm
ROOT=Path(__file__).resolve().parents[1]
def urdf_fk(joints, side):
root=ET.parse(ROOT/'assets/controller_model/lingzuv2_description.urdf').getroot()
transforms={'base_link':np.eye(4)};pending=list(root.findall('joint'))
while pending:
progressed=False
for joint in pending[:]:
parent=joint.find('parent').get('link');child=joint.find('child').get('link')
if parent not in transforms:continue
origin=joint.find('origin');T=np.eye(4)
if origin is not None:
T[:3,3]=np.fromstring(origin.get('xyz','0 0 0'),sep=' ')
T[:3,:3]=R.from_euler('xyz',np.fromstring(origin.get('rpy','0 0 0'),sep=' ')).as_matrix()
name=joint.get('name');letter='L' if side=='left' else 'R'
if name in [f'{letter}{i}_JOINT' for i in range(1,8)]:
axis=np.fromstring(joint.find('axis').get('xyz'),sep=' ')
T[:3,:3]=T[:3,:3]@R.from_rotvec(axis*joints[int(name[1])-1]).as_matrix()
transforms[child]=transforms[parent]@T;pending.remove(joint);progressed=True
if not progressed:raise RuntimeError('Unresolved URDF tree')
return transforms[('L' if side=='left' else 'R')+'7_Link']
def main():
import argparse
ap=argparse.ArgumentParser();ap.add_argument('--side',choices=['left','right'],required=True);args=ap.parse_args()
results=[]
for side in (args.side,):
arm=RealArm(side)
try:
state=arm.read()
for idx in (-1,1,3,5):
q=state['joints'].copy()
if idx>=0:q[idx]+=.1
response=arm._call('fk','lbot_arm_interfaces/srv/ForwardKinematics',{'joints':q.tolist()})
if not response.get('success'):raise RuntimeError('Controller FK failed')
T=urdf_fk(q,side)
# Web L7/R7 origin -> controller Arm_Tip, verified across configurations.
T[:3,3] += T[:3,:3] @ np.array([-.02325,0,-.052])
p=np.array([response['position'][k] for k in 'xyz'])
rotation=R.from_euler('xyz',[response['euler'][k] for k in 'xyz'])
row={'side':side,'perturbed_joint':idx,'position_error_mm':float(np.linalg.norm(T[:3,3]-p)*1000),
'angle_error_deg':float((R.from_matrix(T[:3,:3]).inv()*rotation).magnitude()*180/np.pi)}
results.append(row);print(row,flush=True)
finally:arm.close()
(ROOT/f'assets/controller_model/fk_audit_{args.side}.json').write_text(json.dumps(results,indent=2))
if any(r['position_error_mm']>.1 or r['angle_error_deg']>.01 for r in results):
raise SystemExit('FK alignment failed')
if __name__=='__main__':main()