40 lines
2.9 KiB
Python
40 lines
2.9 KiB
Python
"""Explicit offline smoothing ablation, preserving calibration and moving wrist."""
|
|
import json,shutil
|
|
import numpy as np
|
|
import mujoco
|
|
from scipy.ndimage import gaussian_filter1d
|
|
from scipy.spatial.transform import Rotation
|
|
from l20_calibrated import ROOT,Calibration
|
|
base=ROOT/'output/l20_2047635068/jitter_audit';out=base/'temporal';out.mkdir(exist_ok=True)
|
|
m=dict(np.load(base/'corrected/motion.npz'));cal=Calibration();names=list(m['joint_names'])
|
|
sigma=2.5
|
|
a=gaussian_filter1d(m['active_qpos'],sigma,axis=0)
|
|
q=[]
|
|
for row in a:
|
|
d=dict(zip(cal.active,row))
|
|
for n in cal.passive:d[n]=cal.passive_value(n,d[cal.curves[n][0]])[0]
|
|
q.append([d[n] for n in names])
|
|
m['qpos']=np.array(q);m['active_qpos']=a
|
|
m['wrist_pos_unsmoothed']=m['wrist_pos'].copy();m['wrist_quat_unsmoothed_wxyz']=m['wrist_quat_wxyz'].copy()
|
|
m['wrist_pos']=gaussian_filter1d(m['wrist_pos'],sigma,axis=0)
|
|
rot=Rotation.from_quat(m['wrist_quat_wxyz'][:,[1,2,3,0]])
|
|
relative=(rot[0].inv()*rot).as_rotvec();assert np.linalg.norm(relative,axis=1).max()<np.pi/2
|
|
m['wrist_quat_wxyz']=(rot[0]*Rotation.from_rotvec(gaussian_filter1d(relative,sigma,axis=0))).as_quat()[:,[3,0,1,2]]
|
|
for n in ['l20_moving.xml','l20_right.xml']:shutil.copy2(base/'corrected'/n,out/n)
|
|
model=mujoco.MjModel.from_xml_path(str(out/'l20_moving.xml'));data=mujoco.MjData(model)
|
|
addr=[model.jnt_qposadr[model.joint(str(n)).id] for n in names];sites=[model.site(f'landmark_{i:02d}').id for i in range(21)]
|
|
actual=[]
|
|
for q in m['qpos']:
|
|
data.qpos[addr]=q;mujoco.mj_forward(model,data);actual.append(data.site_xpos[sites].copy())
|
|
m['actual']=np.array(actual);m['command_u8']=np.array([cal.command(dict(zip(names,q))) for q in m['qpos']]);m['decoded_qpos']=np.array([cal.decode(c,names) for c in m['command_u8']])
|
|
m['offline_smoothing_sigma_frames']=sigma
|
|
np.savez_compressed(out/'motion.npz',**m)
|
|
np.savetxt(out/'trajectory.csv',np.c_[m['time'],m['wrist_pos'],m['wrist_quat_wxyz'],m['qpos']],delimiter=',',header=','.join(['time_s','wrist_x','wrist_y','wrist_z','qw','qx','qy','qz']+names),comments='')
|
|
limits=np.array([cal.urdf_limits[n] for n in names]);r=dict(frames=len(a),offline_sigma_frames=sigma,uses_future_frames=True,
|
|
tip_error_mean_mm=float(np.linalg.norm(m['actual'][:,[4,8,12,16,20]]-m['targets'][:,[4,8,12,16,20]],axis=-1).mean()*1000),
|
|
max_joint_step_deg=float(np.abs(np.diff(m['qpos'],axis=0)).max()*180/np.pi),joint_second_difference_rms_deg=float(np.sqrt(np.mean(np.diff(m['qpos'],n=2,axis=0)**2))*180/np.pi),
|
|
wrist_second_difference_rms_mm=float(np.sqrt(np.mean(np.diff(m['wrist_pos'],n=2,axis=0)**2))*1000),wrist_max_change_from_source_mm=float(np.linalg.norm(m['wrist_pos']-m['wrist_pos_unsmoothed'],axis=1).max()*1000),
|
|
wrist_range_m=np.ptp(m['wrist_pos'],axis=0).tolist(),limit_violations=int(np.sum((m['qpos']<limits[:,0]-1e-8)|(m['qpos']>limits[:,1]+1e-8))),
|
|
boundary='Offline hand-only smoothing ablation, not contact or hardware validation')
|
|
(out/'validation.json').write_text(json.dumps(r,indent=2));print(json.dumps(r,indent=2))
|