47 lines
5.3 KiB
Python
47 lines
5.3 KiB
Python
"""Audit saved transforms and IK, without hardware access."""
|
|
from pathlib import Path
|
|
from project_paths import configured_path
|
|
import json,xml.etree.ElementTree as ET
|
|
import numpy as np,mujoco
|
|
from scipy.spatial.transform import Rotation as R
|
|
from scipy.optimize import least_squares
|
|
ROOT=Path(__file__).resolve().parents[1]
|
|
|
|
def pose(p,r):
|
|
t=np.eye(4);t[:3,:3]=r;t[:3,3]=p;return t
|
|
|
|
def main():
|
|
m=mujoco.MjModel.from_xml_path(str(ROOT/'scene/cup_workcell.xml'));d=mujoco.MjData(m);mujoco.mj_resetDataKeyframe(m,d,m.key('snapshot').id);mujoco.mj_forward(m,d)
|
|
base=pose(d.body('base_link').xpos,d.body('base_link').xmat.reshape(3,3));mount=json.loads((ROOT/'config/left_hand_mount.json').read_text());M=np.array(mount['rotation_Arm_Tip_to_hand']);tiphand=pose(mount['translation_Arm_Tip_to_hand_m'],M);l7hand=pose(mount['translation_L7_to_hand_m'],M)
|
|
h=pose(d.body('left_hand_hand_base_link').xpos,d.body('left_hand_hand_base_link').xmat.reshape(3,3));tip=pose(d.site('left_Arm_Tip').xpos,d.site('left_Arm_Tip').xmat.reshape(3,3));l7=pose(d.body('L7_Link').xpos,d.body('L7_Link').xmat.reshape(3,3))
|
|
rep={'hardware_connected':False,'base_world_translation_m':base[:3,3].tolist(),'mount':{'tip_to_hand_matrix_error':float(abs(np.linalg.inv(tip)@h-tiphand).max()),'L7_to_hand_matrix_error':float(abs(np.linalg.inv(l7)@h-l7hand).max())}}
|
|
meta=json.loads((ROOT/'scene/measured_layout.json').read_text());snap=json.loads((ROOT / meta['snapshot_file']).read_text());rep['controller_fk']={}
|
|
for side in ['left','right']:
|
|
p=snap['states'][side]['pose'];expected=pose([p['position'][x] for x in 'xyz'],R.from_quat([p['orientation'][x] for x in 'xyzw']).as_matrix());site=d.site(side+'_Arm_Tip');actual=np.linalg.inv(base)@pose(site.xpos,site.xmat.reshape(3,3));rep['controller_fk'][side]={'position_mm':float(np.linalg.norm(actual[:3,3]-expected[:3,3])*1000),'rotation_deg':float(np.degrees(R.from_matrix(expected[:3,:3].T@actual[:3,:3]).magnitude()))}
|
|
cap=ROOT/'data/cup_sim_capture_20260928';cam=json.loads((cap/'camera.json').read_text());T=np.array(cam['T_world_cam_opencv']);npz=np.load(cap/'hug_cup.npz');rep['hug_camera_to_world']={'landmarks_max_error_m':float(abs(npz['landmarks_cam']@T[:3,:3].T+T[:3,3]-npz['landmarks_world']).max()),'wrist_matrix_error':float(abs(T@npz['T_cam_wrist']-npz['T_world_wrist']).max()),'exported_camera_matrix_error':float(abs(T-npz['T_world_cam']).max())}
|
|
# Compare all common hand-joint transforms/axes; virtual fingertip additions are allowed.
|
|
paths=[ROOT/'assets/l20_left_mj.urdf',configured_path('REFERENCE_LEFT_URDF', 'external/reference/l20_left_with_tips.urdf', required=True)];trees=[ET.parse(p).getroot() for p in paths];maps=[{j.get('name'):j for j in t.findall('joint')} for t in trees];diff=[]
|
|
for name,a in maps[0].items():
|
|
b=maps[1].get(name)
|
|
if b is None:diff.append([name,'missing']);continue
|
|
for tag in ['parent','child','origin','axis']:
|
|
x,y=a.find(tag),b.find(tag)
|
|
if (None if x is None else x.attrib)!=(None if y is None else y.attrib):diff.append([name,tag,None if x is None else x.attrib,None if y is None else y.attrib])
|
|
rep['hand_urdf_joint_differences']=diff
|
|
idx=np.array([m.joint(f'L{i}_JOINT').qposadr[0] for i in range(1,8)]);ids=[m.joint(f'L{i}_JOINT').id for i in range(1,8)];bounds=m.jnt_range[ids].T;qall=d.qpos.copy();qstart=qall[idx].copy();handid=m.body('left_hand_hand_base_link').id
|
|
def solve(target,rot,seed,limits=True):
|
|
def residual(q):
|
|
d.qpos[:]=qall;d.qpos[idx]=q;mujoco.mj_forward(m,d)
|
|
return np.r_[d.xpos[handid]-target,.15*R.from_matrix(rot.T@d.xmat[handid].reshape(3,3)).as_rotvec()]
|
|
lo,hi=bounds if limits else (np.full(7,-2*np.pi),np.full(7,2*np.pi))
|
|
fit=least_squares(residual,np.clip(seed,lo+1e-8,hi-1e-8),bounds=(lo,hi),max_nfev=500,ftol=1e-11,xtol=1e-11,gtol=1e-11)
|
|
e=residual(fit.x);return {'position_error_mm':float(np.linalg.norm(e[:3])*1000),'rotation_error_deg':float(np.degrees(np.linalg.norm(e[3:])/.15)),'q':fit.x.tolist(),'cost':float(np.linalg.norm(e)),'near_limit_joints':[f'L{i+1}' for i in range(7) if min(fit.x[i]-lo[i],hi[i]-fit.x[i])<.001]}
|
|
rep['known_reachable_pose_test']=solve(h[:3,3],h[:3,:3],qstart+np.array([.04,-.04,.04,-.04,.04,-.04,.04]))
|
|
rep['candidates']=[];rng=np.random.default_rng(12)
|
|
for file in sorted(cap.glob('grasp_l20_cup_*.json')):
|
|
g=json.loads(file.read_text());Q=R.from_quat(np.roll(g['wrist_quat_wxyz'],-1)).as_matrix();approach=np.array(g['approach']);target=np.array(g['wrist_pos'])-.07*approach;H=pose(target,Q);tiptarget=H@np.linalg.inv(tiphand);l7target=H@np.linalg.inv(l7hand)
|
|
row={'file':file.name,'hand_target_world_m':target.tolist(),'hand_target_base_m':(np.linalg.inv(base)@H)[:3,3].tolist(),'tip_target_base_m':(np.linalg.inv(base)@tiptarget)[:3,3].tolist(),'roundtrip_error':float(abs(tiptarget@tiphand-H).max()),'approach_dot_palm_x':float(approach@Q[:,0]),'approach_norm':float(np.linalg.norm(approach))}
|
|
seeds=[qstart,*[rng.uniform(bounds[0],bounds[1]) for _ in range(12)]];res=[solve(target,Q,q) for q in seeds];row['bounded_best']=min(res,key=lambda x:x['cost']);row['unbounded_diagnostic']=solve(target,Q,np.array(row['bounded_best']['q']),False);row['grasp_without_7cm_retreat']=solve(np.array(g['wrist_pos']),Q,np.array(row['bounded_best']['q']));rep['candidates'].append(row)
|
|
out=ROOT/'data/cup_coordinate_audit_20260928/report.json';out.write_text(json.dumps(rep,indent=2));print(json.dumps(rep,indent=2))
|
|
if __name__=='__main__':main()
|