38 lines
4.3 KiB
Python
38 lines
4.3 KiB
Python
"""Measure jitter by pipeline stage and verify objective/gradient consistency."""
|
|
import json
|
|
from pathlib import Path
|
|
import numpy as np
|
|
from scipy.spatial.transform import Rotation
|
|
from l20_calibrated import ROOT,Calibration,CalibratedAdaptor,palm_basis
|
|
from l20_consistent_optimizer import ConsistentPositionOptimizer
|
|
from dex_retargeting.retargeting_config import RetargetingConfig
|
|
|
|
base=ROOT/'output/l20_2047635068';out=base/'jitter_audit'
|
|
old=np.load(base/'motion.npz');new=np.load(out/'corrected/motion.npz');contact=np.load(base/'bottle_grasp/registered_motion.npz');human=np.load(ROOT/'output/dex_2047635068/human_joints.npz')
|
|
def stats(a,scale=1):
|
|
a=a*scale;v=np.diff(a,axis=0);acc=np.diff(a,n=2,axis=0)
|
|
return dict(max_component_step=float(np.abs(v).max()),rms_component_step=float(np.sqrt(np.mean(v*v))),rms_second_difference=float(np.sqrt(np.mean(acc*acc))),p99_component_step=float(np.percentile(np.abs(v),99)))
|
|
r={'units':'Position in mm; joint and rotation angles in degrees; differences are per frame at 30 fps', 'stages':{}}
|
|
for name,a,scale in [('human_root_local_joints',human['joints'],1000),('scaled_robot_targets',old['targets'],1000),('old_robot_fk',old['actual'],1000),('corrected_robot_fk',new['actual'],1000),('old_raw_joint',old['qpos_raw'],180/np.pi),('old_filtered_joint',old['qpos'],180/np.pi),('corrected_raw_joint',new['qpos_raw'],180/np.pi),('corrected_filtered_joint',new['qpos'],180/np.pi),('old_contact_joint',contact['qpos'],180/np.pi),('source_wrist',old['wrist_pos'],1000),('contact_wrist',contact['wrist_pos'],1000)]:r['stages'][name]=stats(a,scale)
|
|
r['target_arrays_identical']=bool(np.array_equal(old['targets'],new['targets']))
|
|
r['wrist_arrays_identical']=bool(np.array_equal(old['wrist_pos'],new['wrist_pos']) and np.array_equal(old['wrist_quat_wxyz'],new['wrist_quat_wxyz']))
|
|
r['tip_errors_mm']={key:float(np.linalg.norm(m['actual'][:,[4,8,12,16,20]]-m['targets'][:,[4,8,12,16,20]],axis=-1).mean()*1000) for key,m in [('old',old),('corrected',new)]}
|
|
steps=np.max(np.abs(np.diff(contact['qpos'],axis=0)),axis=1);ix=np.argsort(steps)[-10:][::-1]
|
|
r['largest_contact_jumps']=[dict(from_frame=int(t),to_frame=int(t+1),time_s=float((t+1)/30),joint=str(contact['joint_names'][np.argmax(np.abs(contact['qpos'][t+1]-contact['qpos'][t]))]),joint_step_deg=float(steps[t]*180/np.pi),source_target_max_step_mm=float(np.linalg.norm(np.diff(old['targets'][t:t+2],axis=0)[0],axis=-1).max()*1000),source_joint_max_step_deg=float(np.max(np.abs(old['qpos'][t+1]-old['qpos'][t]))*180/np.pi)) for t in ix]
|
|
basis=Rotation.from_matrix(np.array([palm_basis(p) for p in human['joints']]))
|
|
root=Rotation.from_rotvec(human['root_orient'])
|
|
r['palm_basis_max_step_deg']=float(np.rad2deg((basis[:-1].inv()*basis[1:]).magnitude().max()))
|
|
r['source_root_max_step_deg']=float(np.rad2deg((root[:-1].inv()*root[1:]).magnitude().max()))
|
|
# Check gradient away from the previous iterate, where the omitted penalty is observable.
|
|
cfg=json.loads((out/'corrected/dex_config.json').read_text());rt=RetargetingConfig.from_dict(cfg).build();cal=Calibration();ad=CalibratedAdaptor(rt.optimizer.robot,cal);rt.optimizer.set_kinematic_adaptor(ad)
|
|
names=list(new['joint_names']);last=np.array([new['qpos'][666,names.index(n)] for n in cal.active]);bounds=np.array([cal.bounds[n] for n in cal.active]);x=np.clip(last+.03,bounds[:,0]+.001,bounds[:,1]-.001)
|
|
def check(op):
|
|
fun=op.get_objective_function(new['targets'][666],np.array([]),last);g=np.zeros(16);fun(x,g);fd=[];eps=1e-5
|
|
for j in range(16):
|
|
d=np.eye(16)[j]*eps;fd.append((fun(x+d,np.array([]))-fun(x-d,np.array([])))/(2*eps))
|
|
return float(np.max(np.abs(g-fd)))
|
|
r['objective_gradient_max_error_before']=check(rt.optimizer);rt.optimizer.__class__=ConsistentPositionOptimizer;r['objective_gradient_max_error_after']=check(rt.optimizer)
|
|
r['corrected_joint_second_difference_reduction_pct']=100*(1-r['stages']['corrected_filtered_joint']['rms_second_difference']/r['stages']['old_filtered_joint']['rms_second_difference'])
|
|
r['holding_phase_comparison']={key:dict(holding_max_joint_step_deg=float(np.abs(np.diff(m['qpos'][160:],axis=0)).max()*180/np.pi),holding_second_diff_rms_deg=float(np.sqrt(np.mean(np.diff(m['qpos'][160:],n=2,axis=0)**2))*180/np.pi)) for key,m in [('original',old),('corrected',new),('contact',contact)]}
|
|
(out/'audit.json').write_text(json.dumps(r,indent=2));print(json.dumps(r,indent=2))
|