"""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()