Initial snapshot: hand motion pipeline (Dyn-HaMR + dex-retargeting + SPIDER)
Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
This commit is contained in:
@@ -0,0 +1,37 @@
|
||||
"""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))
|
||||
@@ -0,0 +1,95 @@
|
||||
"""Build full video kinematic contact playback with linear URDF coupling."""
|
||||
import os
|
||||
os.environ.setdefault('MUJOCO_GL', 'osmesa')
|
||||
import json, shutil, xml.etree.ElementTree as E
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
import mujoco
|
||||
from scipy.spatial.transform import Rotation
|
||||
from scipy.ndimage import gaussian_filter1d
|
||||
from scipy.optimize import least_squares
|
||||
ROOT=Path(__file__).resolve().parents[1]
|
||||
OUT=ROOT/'output/l20_full_replay';OUT.mkdir(exist_ok=True)
|
||||
TASK=ROOT/'output/spider_l20_contact/datasets/processed/current/l20/right/bottle_hold'
|
||||
tree=E.parse(TASK/'scene_act.xml');root=tree.getroot();world=root.find('worldbody');asset=root.find('asset')
|
||||
# A playback scene has no object assistance or hidden controller.
|
||||
root.remove(root.find('actuator'))
|
||||
for item in list(world.findall('light')):world.remove(item)
|
||||
for item in list(asset.findall("texture[@type='skybox']")):asset.remove(item)
|
||||
E.SubElement(asset,'texture',name='daylight',type='skybox',builtin='gradient',rgb1='.55 .67 .8',rgb2='.88 .91 .94',width='512',height='3072')
|
||||
E.SubElement(asset,'texture',name='floor_grid',type='2d',builtin='checker',rgb1='.54 .59 .62',rgb2='.64 .69 .72',width='512',height='512')
|
||||
E.SubElement(asset,'material',name='floor_mat',texture='floor_grid',texrepeat='8 8',texuniform='true',reflectance='.08',rgba='1 1 1 1')
|
||||
world.find("geom[@name='floor']").attrib.update(material='floor_mat',rgba='1 1 1 1',size='2 2 .05',pos='0 0 0')
|
||||
E.SubElement(world,'light',name='key',pos='-.5 -.5 2',dir='.3 .3 -1',directional='true',diffuse='.45 .42 .38',ambient='.04 .05 .06',castshadow='false')
|
||||
E.SubElement(world,'light',name='fill',pos='1 1 1.5',dir='-.5 -.5 -1',directional='true',diffuse='.15 .18 .21',castshadow='false')
|
||||
root.find('visual/headlight').attrib.update(ambient='.28 .28 .28',diffuse='.25 .25 .25',specular='.15 .15 .15')
|
||||
root.find('visual/rgba').set('haze','.8 .86 .91 1')
|
||||
root.find('visual/quality').attrib.update(shadowsize='2048',offsamples='2')
|
||||
E.SubElement(root.find('visual'),'map',shadowclip='1',shadowscale='.8',zfar='10',znear='.01')
|
||||
(OUT/'assets').mkdir(exist_ok=True)
|
||||
for item in asset:
|
||||
if item.get('file'):
|
||||
src=Path(item.get('file'));dest=OUT/'assets'/(item.get('name')+src.suffix);shutil.copy2(src,dest);item.set('file',str(dest.relative_to(OUT)))
|
||||
scene=OUT/'scene.xml';tree.write(scene)
|
||||
model=mujoco.MjModel.from_xml_path(str(scene));data=mujoco.MjData(model)
|
||||
addr={model.joint(j).name:int(model.jnt_qposadr[j]) for j in range(model.njnt)}
|
||||
wn=['right_hand_'+s+'_'+a for s in ['pos','rot'] for a in 'xyz'];on=['right_object_'+s+'_'+a for s in ['pos','rot'] for a in 'xyz']
|
||||
wa=[addr[n] for n in wn];oa=[addr[n] for n in on]
|
||||
urdf=E.parse(ROOT/'third_party/l20_assets/L20/RIGHT/linkerhand_g20_right.urdf').getroot();js=[j for j in urdf.findall('joint') if j.get('type')!='fixed']
|
||||
active=[j.get('name') for j in js if j.find('mimic') is None];mimic={}
|
||||
limits={j.get('name'):np.array([float(j.find('limit').get(k)) for k in ['lower','upper']]) for j in js}
|
||||
for j in js:
|
||||
mi=j.find('mimic')
|
||||
if mi is not None:
|
||||
n=j.get('name');parent=mi.get('joint');s=float(mi.get('multiplier','1'));off=float(mi.get('offset','0'));mimic[n]=(parent,s,off)
|
||||
lo,hi=np.sort((limits[n]-off)/s);limits[parent]=np.array([max(limits[parent][0],lo),min(limits[parent][1],hi)])
|
||||
low=np.array([limits[n][0] for n in active]);high=np.array([limits[n][1] for n in active]);aa=[addr[n] for n in active]
|
||||
src=np.load(ROOT/'output/l20_2047635068/jitter_audit/temporal/motion.npz');N=len(src['qpos']);sn=list(src['joint_names']);base=np.clip(np.stack([src['qpos'][:,sn.index(n)] for n in active],1),low,high)
|
||||
wr0=Rotation.from_quat(src['wrist_quat_wxyz'][:,[1,2,3,0]]);wp0=src['wrist_pos'];rel=Rotation.from_euler('x',-np.pi/2);orr0=wr0*rel;op0=wp0+wr0.apply(np.tile([.12,-.065,.15],(N,1)))
|
||||
align=orr0[600].inv();shift=np.array([0,0,.3])-align.apply(op0[600]);wp=align.apply(wp0)+shift;wr=align*wr0;op=align.apply(op0)+shift;orr=align*orr0
|
||||
ref=np.load(TASK/'0/trajectory_kinematic_act.npz')['qpos'][0]
|
||||
offset=ref[aa]-base[600];dp=ref[wa[:3]]-wp[600];dr=(wr[600].inv()*Rotation.from_euler('XYZ',ref[wa[3:]])).as_rotvec()
|
||||
onset=160;weight=np.clip((np.arange(N)-130)/30,0,1);weight=weight*weight*(3-2*weight)
|
||||
# Estimated acquisition bracket, not a measured bottle track.
|
||||
op[:onset]=op[onset];oq=orr.as_quat();oq[:onset]=oq[onset];orr=Rotation.from_quat(oq)
|
||||
handgeoms=np.flatnonzero(model.geom_contype==1);objgeoms=np.flatnonzero(model.geom_contype==2)
|
||||
fingers=['thumb','index','middle','ring','pinky'];tips=[next(i for i,g in enumerate(handgeoms) if model.geom_bodyid[g]==model.body(f+'_distal').id) for f in fingers]
|
||||
def setpose(t,x):
|
||||
vals=dict(zip(active,np.clip(base[t]+x[:16],low,high)))
|
||||
vals.update({n:s*vals[p]+o for n,(p,s,o) in mimic.items()})
|
||||
for n,v in vals.items():data.qpos[addr[n]]=v
|
||||
data.qpos[wa]=np.r_[wp[t]+x[16:19],(wr[t]*Rotation.from_rotvec(x[19:])).as_euler('XYZ')]
|
||||
data.qpos[oa]=np.r_[op[t],orr[t].as_euler('XYZ')]
|
||||
mujoco.mj_kinematics(model,data)
|
||||
def distances():return np.array([min(mujoco.mj_geomDistance(model,data,int(g),int(o),.3,None) for o in objgeoms) for g in handgeoms])
|
||||
prev=np.r_[offset,dp,dr];fits=[];frames=np.unique(np.r_[np.arange(onset,N,2),N-1])
|
||||
for k,t in enumerate(frames):
|
||||
previous=prev.copy()
|
||||
def fun(x):
|
||||
setpose(t,x);d=distances()
|
||||
return np.r_[(d[tips]+.0002)/.004,np.maximum(-d-.0007,0)/.001,.18*x[:16],.8*(x[:16]-previous[:16]),.2*x[16:19]/.03,.15*x[19:]/.3,.5*(x[16:19]-previous[16:19])/.01,.5*(x[19:]-previous[19:])/.1]
|
||||
lb=np.r_[low-base[t],[-.065]*3,[-.55]*3];ub=np.r_[high-base[t],[.065]*3,[.55]*3]
|
||||
fit=least_squares(fun,np.clip(prev,lb+1e-9,ub-1e-9),bounds=(lb,ub),max_nfev=65,diff_step=1e-4)
|
||||
prev=fit.x;fits.append(prev.copy())
|
||||
if k%20==0:print('Contact keyframes',k,len(frames),flush=True)
|
||||
fit=np.array(fits);correction=np.stack([np.interp(np.arange(N),frames,fit[:,j]) for j in range(22)],1)
|
||||
correction=gaussian_filter1d(correction,1.5,axis=0)*weight[:,None]
|
||||
rows=[];penetration=[];gaps=[];minz=1e9
|
||||
for t in range(N):
|
||||
setpose(t,correction[t]);rows.append(data.qpos.copy());d=distances();penetration.append(max(0,-d.min()));gaps.append(d[tips])
|
||||
for g in np.r_[handgeoms,objgeoms]:
|
||||
mesh=model.geom_dataid[g]
|
||||
if model.geom_type[g]==mujoco.mjtGeom.mjGEOM_MESH:
|
||||
vertices=model.mesh_vert[model.mesh_vertadr[mesh]:model.mesh_vertadr[mesh]+model.mesh_vertnum[mesh]]
|
||||
minz=min(minz,float((vertices@data.geom_xmat[g].reshape(3,3).T+data.geom_xpos[g])[:,2].min()))
|
||||
minz=min(minz,float(op[t,2]-.16))
|
||||
q=np.array(rows);zshift=max(0,.025-minz);q[:,wa[2]]+=zshift;q[:,oa[2]]+=zshift
|
||||
q[:,wa[3:]]=np.unwrap(q[:,wa[3:]],axis=0);q[:,oa[3:]]=np.unwrap(q[:,oa[3:]],axis=0)
|
||||
center=np.mean(np.r_[q[:,wa[:3]],q[:,oa[:3]]],axis=0);center[2]+=.04
|
||||
root.find('statistic').attrib.update(center=' '.join(map(str,center)),extent='1')
|
||||
root.find('visual/global').attrib.update(azimuth='135',elevation='-22')
|
||||
E.SubElement(E.SubElement(root,'keyframe'),'key',name='video_start',qpos=' '.join(map(str,q[0])))
|
||||
tree.write(scene)
|
||||
np.savez_compressed(OUT/'motion.npz',qpos=q,fps=30,time=np.arange(N)/30,wrist_pos=q[:,wa[:3]],bottle_pos=q[:,oa[:3]],camera_center=center,correction=correction,contact_gaps_m=gaps,penetration_m=penetration)
|
||||
report=dict(frames=N,fps=30,duration_seconds=N/30,mode='kinematic prescribed-pose playback; not a full SPIDER dynamic rollout',bottle_pose='estimated registration; acquisition bracket 130-160, not video object tracking',nonlinear_coupling=False,linear_mimic=True,wrist_range_m=np.ptp(q[:,wa[:3]],axis=0).tolist(),finite=bool(np.isfinite(q).all()),max_finger_step_deg=float(np.rad2deg(np.abs(np.diff(q[:,aa],axis=0))).max()),mean_holding_fingertip_gap_mm=float(np.abs(gaps[onset:]).mean()*1000),max_holding_penetration_mm=float(np.max(penetration[onset:])*1000),max_all_penetration_mm=float(np.max(penetration)*1000),bottle_dimensions_m=[.05411,.05411,.157856],floor_z=0,ambient_lighting=True)
|
||||
(OUT/'validation.json').write_text(json.dumps(report,indent=2));print(json.dumps(report,indent=2),flush=True)
|
||||
@@ -0,0 +1,49 @@
|
||||
"""Size-constrained idealized bottle; no use of the corrupted radial profile."""
|
||||
import os
|
||||
os.environ.setdefault('MUJOCO_GL','osmesa')
|
||||
from pathlib import Path
|
||||
import json,xml.etree.ElementTree as E
|
||||
import numpy as np
|
||||
import trimesh,mujoco
|
||||
from PIL import Image
|
||||
|
||||
OUT=Path(__file__).resolve().parents[1]/'third_party/bottle_model_parametric'
|
||||
OUT.mkdir(parents=True,exist_ok=True);(OUT/'assets').mkdir(exist_ok=True)
|
||||
H=.157856;R=.05411/2;neck=.014
|
||||
# Only H and 2R are inherited dimensions. Shoulder/neck proportions are design choices.
|
||||
profile=np.array([[0,R],[.103,R],[.139,neck],[H,neck]])
|
||||
def lathe(points):
|
||||
n=128;a=np.arange(n)*2*np.pi/n
|
||||
vertices=np.concatenate([np.c_[r*np.cos(a),r*np.sin(a),np.full(n,z)] for z,r in points])
|
||||
faces=[]
|
||||
for k in range(len(points)-1):
|
||||
for i in range(n):
|
||||
j=(i+1)%n;faces.extend([[k*n+i,k*n+j,(k+1)*n+j],[k*n+i,(k+1)*n+j,(k+1)*n+i]])
|
||||
bottom=len(vertices);top=bottom+1
|
||||
vertices=np.r_[vertices,[[0,0,points[0,0]],[0,0,points[-1,0]]]]
|
||||
for i in range(n):
|
||||
j=(i+1)%n;faces.extend([[bottom,j,i],[top,(len(points)-1)*n+i,(len(points)-1)*n+j]])
|
||||
return trimesh.Trimesh(vertices=vertices,faces=faces,process=True)
|
||||
mesh=lathe(profile);mesh.export(OUT/'assets/bottle.obj');mesh.export(OUT/'bottle.stl');mesh.export(OUT/'bottle.glb')
|
||||
shoulder=lathe(profile[1:3]);shoulder.export(OUT/'assets/shoulder.obj')
|
||||
root=E.Element('mujoco',model='dimension_based_bottle');E.SubElement(root,'option',gravity='0 0 -9.81')
|
||||
asset=E.SubElement(root,'asset')
|
||||
E.SubElement(asset,'mesh',name='bottle_visual',file='assets/bottle.obj')
|
||||
E.SubElement(asset,'mesh',name='bottle_shoulder',file='assets/shoulder.obj')
|
||||
world=E.SubElement(root,'worldbody');body=E.SubElement(world,'body',name='bottle')
|
||||
E.SubElement(body,'freejoint',name='bottle_freejoint')
|
||||
mesh.density=.2308/mesh.volume
|
||||
E.SubElement(body,'inertial',mass='.2308',pos=' '.join(map(str,mesh.center_mass)),diaginertia=' '.join(map(str,np.diag(mesh.moment_inertia))))
|
||||
E.SubElement(body,'geom',type='mesh',mesh='bottle_visual',group='1',contype='0',conaffinity='0',rgba='.68 .74 .8 1')
|
||||
common=dict(group='3',friction='.6 .005 .001',condim='6')
|
||||
E.SubElement(body,'geom',type='cylinder',size=f'{R} {.103/2}',pos=f'0 0 {.103/2}',**common)
|
||||
E.SubElement(body,'geom',type='mesh',mesh='bottle_shoulder',**common)
|
||||
E.SubElement(body,'geom',type='cylinder',size=f'{neck} {(H-.139)/2}',pos=f'0 0 {(H+.139)/2}',**common)
|
||||
E.indent(root);E.ElementTree(root).write(OUT/'bottle.xml')
|
||||
np.savez(OUT/'profile.npz',h=profile[:,0],r=profile[:,1],height=H,diameter=2*R)
|
||||
model=mujoco.MjModel.from_xml_path(str(OUT/'bottle.xml'));data=mujoco.MjData(model);mujoco.mj_forward(model,data)
|
||||
renderer=mujoco.Renderer(model,height=480,width=640);cam=mujoco.MjvCamera();cam.lookat[:]=[0,0,H/2];cam.distance=.36;cam.azimuth=60;cam.elevation=-8
|
||||
opt=mujoco.MjvOption();opt.geomgroup[3]=0;model.vis.headlight.ambient[:]=.7
|
||||
renderer.update_scene(data,camera=cam,scene_option=opt);Image.fromarray(renderer.render()).save(OUT/'preview.png');renderer.close()
|
||||
report=dict(height_mm=H*1000,max_diameter_mm=2*R*1000,neck_diameter_mm=neck*2000,watertight=bool(mesh.is_watertight),winding_consistent=bool(mesh.is_winding_consistent),volume_m3=mesh.volume,mujoco_load_passed=True,collision_parts=3,extent_m=mesh.extents.tolist(),shape_source='Idealized cylinder body, tapered shoulder, cylinder neck; only height and maximum diameter inherited',mass_source='Previous XML mass 0.2308 kg retained; uniform solid inertia recomputed, not measured')
|
||||
(OUT/'validation.json').write_text(json.dumps(report,indent=2));print(json.dumps(report,indent=2))
|
||||
+359
@@ -0,0 +1,359 @@
|
||||
#!/usr/bin/env python3
|
||||
"""demo.py — HandFlow unified demo: cam overlay + orthographic view in one run.
|
||||
|
||||
Two camera modes (selected by --fix_camera):
|
||||
--fix_camera Fixed camera (e.g. DexYCB on a tripod). ViPE is skipped, c2w is
|
||||
identity, and the orthographic view assumes a level, fixed camera:
|
||||
a side orthographic view of the camera-space hand motion (the
|
||||
camera marker stays at the origin; only the hand moves).
|
||||
(default) Moving camera (e.g. HOT3D head-mounted). ViPE SLAM estimates c2w
|
||||
and the orthographic view is a world-space trajectory view: camera
|
||||
trajectory line + per-frame hand mesh.
|
||||
|
||||
Both modes write two mp4s into --output_dir:
|
||||
overlay.mp4 Clean camera-space MANO mesh overlaid onto the RGB frames.
|
||||
ortho.mp4 Orthographic (side / topdown) view of the hand (+ camera motion).
|
||||
|
||||
Intrinsics (--intrinsics):
|
||||
fx,fy,cx,cy Explicit pinhole intrinsics (recommended; e.g. the demo GT values).
|
||||
auto Let ViPE estimate intrinsics (only valid without --fix_camera).
|
||||
(unset) Generic default [600, 600, W/2, H/2].
|
||||
|
||||
Usage:
|
||||
# Fixed camera (no ViPE needed)
|
||||
python scripts/demo.py --input video.mp4 --fm_ckpt <ckpt> \
|
||||
--intrinsics fx,fy,cx,cy --fix_camera --output_dir output/demo
|
||||
|
||||
# Moving camera (ViPE estimates c2w; requires the vipe env)
|
||||
python scripts/demo.py --input video.mp4 --fm_ckpt <ckpt> \
|
||||
--intrinsics fx,fy,cx,cy --output_dir output/demo
|
||||
|
||||
# Moving camera, unknown intrinsics -> ViPE estimates both intr + c2w
|
||||
python scripts/demo.py --input video.mp4 --fm_ckpt <ckpt> \
|
||||
--intrinsics auto --output_dir output/demo
|
||||
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import gc
|
||||
import os
|
||||
import sys
|
||||
import tempfile
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import torch
|
||||
from omegaconf import OmegaConf
|
||||
from pytorch3d.transforms import axis_angle_to_quaternion, quaternion_to_axis_angle
|
||||
|
||||
_PROJECT_ROOT = Path(__file__).resolve().parent.parent
|
||||
sys.path.insert(0, str(_PROJECT_ROOT))
|
||||
|
||||
from utils.checkpoint_utils import load_denoiser_from_ckpt
|
||||
from utils.inference_utils import run_fm_inference_with_overlap, split_sequence_into_windows
|
||||
from utils.mano_utils import MANOForwardKinematics
|
||||
from utils.online_hamer import OnlineHaMeRPipeline
|
||||
from utils.vipe_worker import run_vipe_slam
|
||||
from visualization.renderer_p3d import PhongRenderer
|
||||
from visualization.video_io import read_video_frames, write_video_ffmpeg
|
||||
|
||||
|
||||
def resolve_intrinsics(arg_str, W: int, H: int):
|
||||
"""Resolve --intrinsics into (intr(4,) | None, vipe_estimate: bool).
|
||||
|
||||
None / "" / "default" -> generic [600, 600, W/2, H/2], vipe_estimate=False
|
||||
"auto" -> (None, True) (ViPE estimates; only without --fix_camera)
|
||||
"fx,fy,cx,cy" -> explicit (4,), vipe_estimate=False
|
||||
"""
|
||||
if arg_str in (None, "", "default"):
|
||||
return np.array([600.0, 600.0, W / 2.0, H / 2.0], dtype=np.float32), False
|
||||
if arg_str == "auto":
|
||||
return None, True
|
||||
parts = [float(x) for x in arg_str.split(",")]
|
||||
if len(parts) != 4:
|
||||
raise ValueError(f"--intrinsics expects 4 values fx,fy,cx,cy or 'auto', got {arg_str!r}")
|
||||
return np.array(parts, dtype=np.float32), False
|
||||
|
||||
|
||||
def extract_frames_to_dir(frames_bgr, img_dir: str) -> None:
|
||||
"""Write frames as jpg into a directory (input for ViPE SLAM)."""
|
||||
os.makedirs(img_dir, exist_ok=True)
|
||||
for i, f in enumerate(frames_bgr):
|
||||
cv2.imwrite(str(Path(img_dir) / f"{i:06d}.jpg"), f)
|
||||
|
||||
|
||||
def transform_verts_to_world(verts_cam_m: np.ndarray, c2w: np.ndarray) -> np.ndarray:
|
||||
"""verts_cam (T,N,3) meters + c2w (T,4,4) -> verts_world (T,N,3) meters."""
|
||||
T, N, _ = verts_cam_m.shape
|
||||
homog = np.concatenate([verts_cam_m, np.ones((T, N, 1), dtype=np.float32)], axis=-1)
|
||||
return np.einsum("tij,tnj->tni", c2w.astype(np.float32), homog)[..., :3].astype(np.float32)
|
||||
|
||||
|
||||
def smooth_pose_rotations(pose_seq: torch.Tensor, window: int) -> torch.Tensor:
|
||||
"""Temporally smooth MANO axis-angle joints by averaging in quaternion space."""
|
||||
if window <= 1:
|
||||
return pose_seq
|
||||
if window % 2 == 0:
|
||||
raise ValueError("--rotation_smooth_window must be an odd positive integer")
|
||||
|
||||
n_frames, pose_dim = pose_seq.shape
|
||||
if pose_dim % 3:
|
||||
raise ValueError(f"Expected axis-angle pose dimension divisible by 3, got {pose_dim}")
|
||||
quats = axis_angle_to_quaternion(pose_seq.reshape(n_frames, -1, 3))
|
||||
radius = window // 2
|
||||
weights = (radius + 1 - torch.arange(-radius, radius + 1, device=pose_seq.device).abs()).to(pose_seq)
|
||||
weights = weights / weights.sum()
|
||||
smoothed = []
|
||||
for frame_idx in range(n_frames):
|
||||
indices = torch.arange(frame_idx - radius, frame_idx + radius + 1, device=pose_seq.device)
|
||||
indices = indices.clamp(0, n_frames - 1)
|
||||
neighbors = quats[indices]
|
||||
reference = quats[frame_idx].unsqueeze(0)
|
||||
signs = torch.where((neighbors * reference).sum(dim=-1, keepdim=True) < 0, -1.0, 1.0)
|
||||
averaged = (neighbors * signs * weights[:, None, None]).sum(dim=0)
|
||||
smoothed.append(averaged / averaged.norm(dim=-1, keepdim=True).clamp_min(1e-8))
|
||||
return quaternion_to_axis_angle(torch.stack(smoothed)).reshape_as(pose_seq)
|
||||
|
||||
|
||||
def get_camera_geometry(frames_bgr, fix_camera: bool, intr, vipe_estimate: bool, T: int):
|
||||
"""Return (intr(4,), c2w(T,4,4), vipe_slam_time_s).
|
||||
|
||||
fix_camera=True: c2w = identity (no ViPE); intr is the provided/generic value.
|
||||
fix_camera=False: run ViPE — gt_intr (c2w only) when intr is known, else default
|
||||
(ViPE estimates intr + c2w).
|
||||
"""
|
||||
if fix_camera:
|
||||
if vipe_estimate:
|
||||
print("[demo] --fix_camera ignores --intrinsics auto; using generic 600,600,W/2,H/2")
|
||||
intr = np.array([600.0, 600.0, frames_bgr[0].shape[1] / 2.0,
|
||||
frames_bgr[0].shape[0] / 2.0], dtype=np.float32)
|
||||
c2w = np.broadcast_to(np.eye(4, dtype=np.float32), (T, 4, 4)).copy()
|
||||
return intr.astype(np.float32), c2w, 0.0
|
||||
|
||||
with tempfile.TemporaryDirectory() as tmp:
|
||||
img_dir = str(Path(tmp) / "images")
|
||||
extract_frames_to_dir(frames_bgr, img_dir)
|
||||
if vipe_estimate:
|
||||
print("[demo] ViPE SLAM (default, estimating intrinsics + c2w) ...")
|
||||
slam = run_vipe_slam(img_dir, tmp, variant="default")
|
||||
intr_arr = slam["intrinsics"]
|
||||
intr = intr_arr[0] if intr_arr.ndim == 2 else intr_arr
|
||||
else:
|
||||
print("[demo] ViPE SLAM (gt_intr, estimating c2w only) ...")
|
||||
slam = run_vipe_slam(img_dir, tmp, variant="gt_intr", gt_intrinsics_4d=intr)
|
||||
c2w = slam["poses"]
|
||||
print(f" c2w shape={c2w.shape}, intrinsics={intr}, slam_time={slam['slam_time_s']:.2f}s")
|
||||
return intr.astype(np.float32), c2w.astype(np.float32), float(slam["slam_time_s"])
|
||||
|
||||
|
||||
def main():
|
||||
ap = argparse.ArgumentParser(description="HandFlow demo: cam overlay + orthographic view")
|
||||
ap.add_argument("--input", required=True, help="Input video path")
|
||||
ap.add_argument("--fm_ckpt", required=True, help="HandFlow checkpoint (.pt or DeepSpeed directory)")
|
||||
ap.add_argument("--config", default="configs/inference.yaml")
|
||||
ap.add_argument("--intrinsics", default=None,
|
||||
help="fx,fy,cx,cy | auto | (unset -> default 600,600,W/2,H/2)")
|
||||
ap.add_argument("--fix_camera", action="store_true",
|
||||
help="Fixed camera: skip ViPE (c2w=identity), ortho view assumes a level fixed camera")
|
||||
ap.add_argument("--camera_npz", default=None,
|
||||
help="Reuse intrinsics and c2w from a previous result npz instead of rerunning ViPE")
|
||||
ap.add_argument("--rotation_smooth_window", type=int, default=0,
|
||||
help="Odd temporal window for quaternion-space MANO rotation smoothing (0 disables)")
|
||||
ap.add_argument("--side", default="right", choices=["right", "left"])
|
||||
ap.add_argument("--view", default=None, choices=["topdown", "side", "third_person"],
|
||||
help="Ortho view; default side for --fix_camera else third_person")
|
||||
ap.add_argument("--output_dir", default="output/demo")
|
||||
ap.add_argument("--device", default="cuda")
|
||||
ap.add_argument("--save_npz", default=None, help="Optional: path to save inference results npz")
|
||||
args = ap.parse_args()
|
||||
|
||||
device = torch.device(args.device)
|
||||
t0 = time.perf_counter()
|
||||
|
||||
# ── config ──
|
||||
cfg = OmegaConf.load(str(_PROJECT_ROOT / args.config))
|
||||
model_cfg = OmegaConf.load(str(_PROJECT_ROOT / cfg.model_yaml))
|
||||
|
||||
# ── read video ──
|
||||
print(f"[demo] Reading video {args.input} ...")
|
||||
frames_bgr, fps, (H, W) = read_video_frames(args.input)
|
||||
T = len(frames_bgr)
|
||||
print(f" {T} frames, {W}x{H}, {fps:.1f} fps")
|
||||
t1 = time.perf_counter()
|
||||
|
||||
# ── intrinsics + camera geometry (c2w) ──
|
||||
camera_cache = Path(args.output_dir) / "camera_slam.npz"
|
||||
camera_source = Path(args.camera_npz) if args.camera_npz else camera_cache
|
||||
if camera_source.exists():
|
||||
camera_data = np.load(camera_source)
|
||||
intr = np.asarray(camera_data["intrinsics"], dtype=np.float32).reshape(-1)[:4]
|
||||
c2w = np.asarray(camera_data["c2w"], dtype=np.float32)
|
||||
if c2w.shape[0] < T:
|
||||
raise ValueError(f"Camera npz has {c2w.shape[0]} poses for {T} video frames")
|
||||
c2w = c2w[:T]
|
||||
vipe_t = 0.0
|
||||
print(f"[demo] Reusing camera geometry from {camera_source}")
|
||||
else:
|
||||
intr, vipe_estimate = resolve_intrinsics(args.intrinsics, W, H)
|
||||
intr, c2w, vipe_t = get_camera_geometry(frames_bgr, args.fix_camera, intr, vipe_estimate, T)
|
||||
if not args.fix_camera:
|
||||
os.makedirs(args.output_dir, exist_ok=True)
|
||||
np.savez(camera_cache, intrinsics=intr, c2w=c2w)
|
||||
print(f"[demo] Cached camera geometry -> {camera_cache}")
|
||||
intr_list = [intr] * T
|
||||
t2 = time.perf_counter()
|
||||
|
||||
# ── load models ──
|
||||
print("[demo] Loading denoiser / HaMeR / MANO ...")
|
||||
mano_root = cfg.eval.mano_root or os.environ.get("MANO_ROOT")
|
||||
if not mano_root:
|
||||
raise RuntimeError("MANO_ROOT not set: please export MANO_ROOT=/path/to/mano")
|
||||
denoiser = load_denoiser_from_ckpt(OmegaConf.merge(model_cfg, cfg), args.fm_ckpt, device)
|
||||
denoiser.hamer_backbone.to("cpu")
|
||||
gc.collect()
|
||||
torch.cuda.empty_cache()
|
||||
print("[demo] Offloaded unused frozen HaMeR backbone to CPU")
|
||||
online_cache = Path(args.output_dir) / "online_hamer.pt"
|
||||
online = None if online_cache.exists() else OnlineHaMeRPipeline(device=str(device))
|
||||
mano_fk = MANOForwardKinematics(str(mano_root), device)
|
||||
t3 = time.perf_counter()
|
||||
|
||||
# ── Online HaMeR ──
|
||||
if online_cache.exists():
|
||||
print(f"[demo] Reusing Online HaMeR features from {online_cache}")
|
||||
online_result = torch.load(online_cache, map_location="cpu", weights_only=False)
|
||||
else:
|
||||
print("[demo] Online HaMeR inference ...")
|
||||
online_result = online.process_sequence(frames_bgr, intr_list, target_side=args.side)
|
||||
torch.save(online_result, online_cache)
|
||||
print(f"[demo] Cached Online HaMeR features -> {online_cache}")
|
||||
det_valid = online_result["detection_valid"].numpy()
|
||||
det_sides = online_result["sides"]
|
||||
bbox_xyxy = online_result["bbox_xyxy"].numpy()
|
||||
bbox_conf = online_result["bbox_conf"].numpy()
|
||||
dominant_side = max(set(det_sides), key=det_sides.count) if det_sides else args.side
|
||||
print(f" Detections: {int(det_valid.sum())}/{T} frames, dominant hand side={dominant_side}")
|
||||
t4 = time.perf_counter()
|
||||
|
||||
bf = online_result["backbone_features"]
|
||||
if online is not None:
|
||||
print("[demo] HaMeR cache ready; restart to release third-party CUDA hooks")
|
||||
return
|
||||
token_chunks = []
|
||||
compressor_batch_size = 1
|
||||
with torch.no_grad():
|
||||
for s in range(0, T, compressor_batch_size):
|
||||
e = min(s + compressor_batch_size, T)
|
||||
chunk = denoiser.frame_compressor(bf[s:e].to(device).unsqueeze(0)).squeeze(0)
|
||||
token_chunks.append(chunk.detach().cpu())
|
||||
image_tokens = torch.cat(token_chunks, dim=0)
|
||||
del bf, token_chunks
|
||||
|
||||
seq_batch = {
|
||||
"mano_params": torch.zeros((1, T, 48), dtype=torch.float32),
|
||||
"mano_trans": torch.zeros((1, T, 3), dtype=torch.float32),
|
||||
"mano_betas": torch.zeros((1, 10), dtype=torch.float32),
|
||||
"padding_mask": torch.zeros((1, T), dtype=torch.bool),
|
||||
"images": online_result["crop_images"].unsqueeze(0),
|
||||
"hamer_landmarks": online_result["hamer_landmarks"].unsqueeze(0),
|
||||
"crop_intrinsics": online_result["crop_intrinsics"].unsqueeze(0),
|
||||
"hamer_confidence": online_result["hamer_confidence"].unsqueeze(0),
|
||||
"image_tokens": image_tokens.unsqueeze(0),
|
||||
"side": [dominant_side],
|
||||
"source": ["custom"],
|
||||
}
|
||||
|
||||
# ── FM inference (overlapping windows) ──
|
||||
win = int(cfg.inference.window_size)
|
||||
overlap = int(cfg.inference.overlap_size)
|
||||
ode_steps = int(cfg.inference.ode_steps)
|
||||
stride = win - overlap
|
||||
win_batch, _ = split_sequence_into_windows(seq_batch, win, stride, device)
|
||||
win_batch = {k: (v.to(device) if isinstance(v, torch.Tensor) else v) for k, v in win_batch.items()}
|
||||
|
||||
print("[demo] FM Denoiser inference ...")
|
||||
pose_seq, trans_seq, betas_pred = run_fm_inference_with_overlap(
|
||||
denoiser, denoiser, win_batch, win, overlap, ode_steps, device,
|
||||
overlap_method=cfg.inference.get("overlap_method", "vblend"),
|
||||
)
|
||||
if args.rotation_smooth_window > 1:
|
||||
pose_seq = smooth_pose_rotations(pose_seq, args.rotation_smooth_window)
|
||||
print(f" Applied quaternion rotation smoothing: window={args.rotation_smooth_window}")
|
||||
nf = pose_seq.shape[0]
|
||||
print(f" Output {nf} frames of pose/trans")
|
||||
t5 = time.perf_counter()
|
||||
|
||||
# ── MANO FK -> verts_cam (meters, per-frame hand side from detection) ──
|
||||
sides = (det_sides[:nf] if len(det_sides) >= nf
|
||||
else det_sides + [det_sides[-1]] * (nf - len(det_sides)))
|
||||
verts_cam_m = (mano_fk.verts(pose_seq, betas_pred, trans_seq, sides) / 1000.0
|
||||
).cpu().numpy().astype(np.float32) # (nf,778,3) m
|
||||
faces = mano_fk.get_faces(args.side)
|
||||
t6 = time.perf_counter()
|
||||
|
||||
# ── world verts + camera positions for the ortho view ──
|
||||
n = min(nf, T, c2w.shape[0])
|
||||
verts_world = transform_verts_to_world(verts_cam_m[:n], c2w[:n]) # identity if fix_camera
|
||||
cam_positions = c2w[:n, :3, 3] # zeros if fix_camera
|
||||
|
||||
# ── Output 1: cam overlay (clean verts_cam projection onto RGB) ──
|
||||
print("[demo] Rendering overlay ...")
|
||||
renderer = PhongRenderer(device)
|
||||
n_render = min(nf, T)
|
||||
out_frames = []
|
||||
for fi in range(T):
|
||||
frame = frames_bgr[fi]
|
||||
if fi < n_render and det_valid[fi]:
|
||||
x1, y1, x2, y2 = bbox_xyxy[fi].astype(int)
|
||||
cv2.rectangle(frame, (x1, y1), (x2, y2), (0, 255, 0), 2)
|
||||
label = f"{det_sides[fi]} {bbox_conf[fi]:.2f}"
|
||||
cv2.putText(frame, label, (x1, max(8, y1 - 8)),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2, cv2.LINE_AA)
|
||||
frame = renderer.render_overlay(frame, verts_cam_m[fi], faces, intr, sides[fi])
|
||||
out_frames.append(frame)
|
||||
os.makedirs(args.output_dir, exist_ok=True)
|
||||
overlay_path = str(Path(args.output_dir) / "overlay.mp4")
|
||||
write_video_ffmpeg(out_frames, overlay_path, fps)
|
||||
print(f"[demo] ✅ {overlay_path}")
|
||||
t7 = time.perf_counter()
|
||||
|
||||
# ── Output 2: orthographic view (side for fix_camera / level; else world topdown) ──
|
||||
view = args.view or ("side" if args.fix_camera else "third_person")
|
||||
print(f"[demo] Rendering ortho ({view}) ...")
|
||||
traj_frames = renderer.render_ortho_video(verts_world, faces, c2w[:n], view=view, side=args.side)
|
||||
ortho_path = str(Path(args.output_dir) / f"ortho_{view}.mp4")
|
||||
write_video_ffmpeg(traj_frames, ortho_path, fps)
|
||||
print(f"[demo] ✅ {ortho_path}")
|
||||
t8 = time.perf_counter()
|
||||
|
||||
# ── timing breakdown ──
|
||||
det_t = float(online_result.get("det_time_s", 0.0))
|
||||
hamer_t = float(online_result.get("hamer_time_s", 0.0))
|
||||
print(
|
||||
f"[timing] read={t1-t0:.1f}s | "
|
||||
f"vipe={vipe_t:.1f}s(+frames/subprocess {t2-t1-vipe_t:.1f}s) | "
|
||||
f"load={t3-t2:.1f}s | "
|
||||
f"hamer={t4-t3:.1f}s (det={det_t:.1f} infer={hamer_t:.1f}) | "
|
||||
f"fm={t5-t4:.1f}s | mano={t6-t5:.1f}s | render={t7-t6:.1f}s | ortho={t8-t7:.1f}s | "
|
||||
f"total={t8-t0:.1f}s"
|
||||
)
|
||||
|
||||
# ── optional: save inference npz ──
|
||||
if args.save_npz:
|
||||
np.savez(
|
||||
args.save_npz,
|
||||
verts_cam=verts_cam_m[:n_render], verts_world=verts_world, faces=faces,
|
||||
intrinsics=intr, c2w=c2w[:n], fix_camera=bool(args.fix_camera),
|
||||
pose=pose_seq.cpu().numpy(), trans=trans_seq.cpu().numpy(),
|
||||
betas=betas_pred.cpu().numpy(), pred_valid=det_valid[:n_render],
|
||||
side=args.side, source=args.input, fps=fps,
|
||||
)
|
||||
print(f"[demo] npz -> {args.save_npz}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,54 @@
|
||||
"""Independently evaluate SPIDER's zero-assistance rollout and a control-replay baseline."""
|
||||
import os
|
||||
os.environ.setdefault('MUJOCO_GL','osmesa')
|
||||
from pathlib import Path
|
||||
import json,re
|
||||
import numpy as np
|
||||
import mujoco,imageio.v2 as imageio
|
||||
from PIL import Image,ImageDraw
|
||||
ROOT=Path(__file__).resolve().parents[1];OUT=ROOT/'output/spider_l20_contact';TASK=OUT/'datasets/processed/current/l20/right/bottle_hold';TRIAL=TASK/'0'
|
||||
DT=json.loads((OUT/'config.json').read_text())['sim_dt']
|
||||
ref=np.load(TRIAL/'trajectory_kinematic_act.npz');result=np.load(TRIAL/'trajectory_mjwp_act.npz');q=result['qpos'].reshape(-1,result['qpos'].shape[-1]);ctrl=result['ctrl'].reshape(-1,result['ctrl'].shape[-1]);v=result['qvel'].reshape(-1,result['qvel'].shape[-1]);time=result['time'].ravel();N=len(q)
|
||||
model=mujoco.MjModel.from_xml_path(str(TASK/'scene_act.xml'));model.opt.iterations=20;model.opt.ls_iterations=50;model.opt.o_solref[:]=[.02,1];model.opt.o_solimp[:]=[0,.95,.03,.5,2]
|
||||
obj=model.body('right_object').id;hand=model.body('hand_base_link').id
|
||||
assert np.all(model.actuator_gainprm[-6:]==0) and np.all(model.actuator_biasprm[-6:]==0)
|
||||
def replay(controls):
|
||||
d=mujoco.MjData(model);d.qpos[:]=ref['qpos'][0];d.qvel[:]=ref['qvel'][0];d.ctrl[:]=ref['ctrl'][0];mujoco.mj_step(model,d);d.time=0
|
||||
rows=[]
|
||||
for u in controls:
|
||||
d.ctrl[:]=u;mujoco.mj_step(model,d);rows.append(d.qpos.copy())
|
||||
return np.array(rows)
|
||||
baseline=replay(ref['ctrl'][1:N+1]);cpu=replay(ctrl)
|
||||
def metrics(rows):
|
||||
d=mujoco.MjData(model);dr=mujoco.MjData(model);gaps=[];errors=[];relerrors=[];counts=[];wrist=[];objheight=[]
|
||||
for t,row in enumerate(rows):
|
||||
d.qpos[:]=row;mujoco.mj_forward(model,d);dr.qpos[:]=ref['qpos'][min(t+1,len(ref['qpos'])-1)];mujoco.mj_forward(model,dr)
|
||||
errors.append(np.linalg.norm(d.xpos[obj]-dr.xpos[obj]));relerrors.append(np.linalg.norm((d.xpos[obj]-d.xpos[hand])-(dr.xpos[obj]-dr.xpos[hand])))
|
||||
selected=[c for c in d.contact if {int(model.geom_contype[c.geom[0]]),int(model.geom_contype[c.geom[1]])}=={1,2}]
|
||||
counts.append(len(selected));gaps.append(max([max(0,-c.dist) for c in selected],default=0));wrist.append(d.xpos[hand].copy());objheight.append(d.xpos[obj,2])
|
||||
e=np.array(errors);r=np.array(relerrors)
|
||||
return dict(finite=bool(np.isfinite(rows).all()),max_object_position_error_mm=float(e.max()*1000),final_object_position_error_mm=float(e[-1]*1000),mean_object_position_error_mm=float(e.mean()*1000),max_object_hand_relative_error_mm=float(r.max()*1000),contact_frames=int(np.sum(np.array(counts)>0)),contact_duration_s=float(np.sum(np.array(counts)>0)*DT),last_hand_contact_time_s=float((np.flatnonzero(np.array(counts)>0)[-1]+1)*DT) if np.any(np.array(counts)>0) else 0.,frames=len(rows),max_hand_object_penetration_mm=float(np.max(gaps)*1000),wrist_range_m=np.ptp(wrist,axis=0).tolist(),final_bottle_base_height_m=float(objheight[-1]),retained=bool(r.max()<.02 and counts[-1]>0)),np.array(counts),e
|
||||
mr,counts,errors=metrics(q);mb,_,_=metrics(baseline);mc,_,_=metrics(cpu)
|
||||
report=dict(spider=mr,reference_control_baseline=mb,independent_cpu_control_replay=mc,zero_object_actuator_gains=True,zero_object_actuator_bias=True,source_frames=[600,660],simulation_seconds=float(time[-1]),nonlinear_json_coupling=False,linear_mimic=True,source_object_pose='Estimated target, not measured video object motion',cpu_gpu_max_qpos_difference=float(np.abs(cpu-q).max()))
|
||||
log=(OUT/'run.log').read_text();report['nonfinite_sample_warnings']=len(re.findall('NaNs or infs in rews',log));report['upstream_completion_marker']='SPIDER_RUN_COMPLETE' in log
|
||||
if (OUT/'independent_gpu_replay.npz').exists():
|
||||
independent=np.load(OUT/'independent_gpu_replay.npz')
|
||||
report['independent_gpu_control_replay']=metrics(independent['optimized_controls'])[0]
|
||||
report['reference_control_gpu_baseline']=metrics(independent['baseline'])[0]
|
||||
(OUT/'validation.json').write_text(json.dumps(report,indent=2));np.savez_compressed(OUT/'comparison_motion.npz',spider=q,baseline=baseline,cpu_replay=cpu,reference=ref['qpos'][1:N+1],ctrl=ctrl,qvel=v,time=time,contact_counts=counts,object_error_m=errors)
|
||||
np.savetxt(OUT/'optimized_trajectory.csv',np.c_[time,q,ctrl],delimiter=',',header=','.join(['time_s']+[model.joint(i).name for i in range(model.njnt)]+['ctrl_'+model.actuator(i).name for i in range(model.nu)]),comments='')
|
||||
model.vis.headlight.ambient[:]=.8;model.vis.quality.offsamples=1
|
||||
renderer=mujoco.Renderer(model,height=400,width=400);cam=mujoco.MjvCamera();cam.lookat[:]=[-.02,.045,.34];cam.distance=.65;cam.azimuth=50;cam.elevation=12
|
||||
opt=mujoco.MjvOption();opt.geomgroup[3]=0;opt.sitegroup[:]=0;data=mujoco.MjData(model)
|
||||
frames=np.minimum(np.round(np.arange(60)/30/DT).astype(int),N-1)
|
||||
writer=imageio.get_writer(OUT/'spider_contact_comparison.mp4',fps=30,codec='libx264',quality=8)
|
||||
for i,t in enumerate(frames):
|
||||
panels=[]
|
||||
for row in [ref['qpos'][t+1],baseline[t],q[t],cpu[t]]:
|
||||
data.qpos[:]=row;mujoco.mj_forward(model,data);renderer.update_scene(data,camera=cam,scene_option=opt);panels.append(renderer.render().copy())
|
||||
img=Image.fromarray(np.concatenate(panels,axis=1));draw=ImageDraw.Draw(img);draw.rectangle([0,0,1600,54],fill=(20,25,35))
|
||||
for j,label in enumerate(['Estimated reference','Reference controls, free bottle (CPU)','SPIDER contact guidance (GPU)','Same controls, independent CPU replay']):draw.text((j*400+8,8),label,fill='white')
|
||||
draw.text((8,30),f't={time[t]:.2f}s | Moving wrist + URDF linear mimic | SPIDER error={errors[t]*1000:.1f} mm, contacts={counts[t]} | 20mm tracking gate={mr["retained"]}',fill='white')
|
||||
writer.append_data(np.asarray(img))
|
||||
if i in [0,30,59]:img.save(OUT/f'comparison_{i:02d}.png')
|
||||
writer.close();renderer.close();print(json.dumps(report,indent=2))
|
||||
@@ -0,0 +1,50 @@
|
||||
"""Export Dyn-HaMR MANO joints, preserving the exact reconstruction FK convention."""
|
||||
import os
|
||||
os.environ.setdefault('OMP_NUM_THREADS', '4')
|
||||
import sys
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
import torch
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
sys.path.insert(0, str(ROOT / 'third_party/Dyn-HaMR/dyn-hamr'))
|
||||
from body_model.mano_wrapper import MANO
|
||||
|
||||
def main():
|
||||
torch.set_num_threads(4)
|
||||
source = ROOT / 'output/results_2047635068/world_results.npz'
|
||||
out = ROOT / 'output/dex_2047635068'
|
||||
out.mkdir(exist_ok=True)
|
||||
data = np.load(source)
|
||||
candidates = np.flatnonzero(np.all(data['is_right'] > .5, axis=1))
|
||||
assert len(candidates) == 1, 'Expected exactly one consistent right-hand track'
|
||||
track = int(candidates[0])
|
||||
pose = data['pose_body'][track].reshape(-1, 45)
|
||||
model = MANO(model_path=str(ROOT / 'third_party/Dyn-HaMR/_DATA/data/mano'),
|
||||
batch_size=128, pose2rot=True)
|
||||
joints = []
|
||||
wrists = []
|
||||
with torch.no_grad():
|
||||
for start in range(0, len(pose), 128):
|
||||
p = torch.from_numpy(pose[start:start+128]).float()
|
||||
n = len(p)
|
||||
result = model(hand_pose=p, global_orient=torch.zeros(n, 3),
|
||||
transl=torch.zeros(n, 3),
|
||||
betas=torch.from_numpy(data['betas'][track]).float().expand(n, -1))
|
||||
j = result.joints.numpy()
|
||||
joints.append(j - j[:, :1])
|
||||
world = model(hand_pose=p,
|
||||
global_orient=torch.from_numpy(data['root_orient'][track,start:start+n]).float(),
|
||||
transl=torch.from_numpy(data['trans'][track,start:start+n]).float(),
|
||||
betas=torch.from_numpy(data['betas'][track]).float().expand(n,-1))
|
||||
wrists.append(world.joints[:,0].numpy())
|
||||
joints = np.concatenate(joints)
|
||||
assert joints.shape == (len(pose), 21, 3) and np.isfinite(joints).all()
|
||||
np.savez_compressed(out / 'human_joints.npz', joints=joints, fps=30,
|
||||
source_track=track, source=str(source.resolve()),
|
||||
wrist_world=np.concatenate(wrists),
|
||||
root_orient=data['root_orient'][track], trans=data['trans'][track])
|
||||
print(f'Exported {joints.shape}, right track {track}, to {out}', flush=True)
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,100 @@
|
||||
"""Export HandFlow with its own MANO convention and camera-to-world transform."""
|
||||
import argparse
|
||||
import json
|
||||
from pathlib import Path
|
||||
import sys
|
||||
|
||||
import numpy as np
|
||||
import torch
|
||||
from scipy.spatial.transform import Rotation, Slerp
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
sys.path.insert(0, str(ROOT))
|
||||
from utils.mano_utils import MANOForwardKinematics
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument('--input', type=Path, required=True)
|
||||
parser.add_argument('--output', type=Path, required=True)
|
||||
parser.add_argument('--gap-policy', choices=['interpolate', 'raw'], default='interpolate')
|
||||
parser.add_argument('--min-detection-run', type=int, default=3,
|
||||
help='Reject isolated detection runs shorter than this before gap filling')
|
||||
args = parser.parse_args()
|
||||
torch.set_num_threads(4)
|
||||
source = np.load(args.input)
|
||||
assert str(source['side']) == 'right', 'This exporter targets the L20 right hand'
|
||||
pose = torch.tensor(source['pose'], dtype=torch.float32)
|
||||
count = len(pose)
|
||||
betas = torch.tensor(source['betas'], dtype=torch.float32)
|
||||
if betas.ndim == 1:
|
||||
betas = betas[None].expand(count, -1)
|
||||
elif len(betas) == 1:
|
||||
betas = betas.expand(count, -1)
|
||||
assert betas.shape == (count, 10)
|
||||
trans = torch.tensor(source['trans'], dtype=torch.float32)
|
||||
c2w = source['c2w']
|
||||
assert len(c2w) == count and len(source['verts_cam']) == count
|
||||
fk = MANOForwardKinematics(str(ROOT / 'third_party/hamer/_DATA/data/mano'), torch.device('cpu'))
|
||||
local_pose = pose.clone()
|
||||
local_pose[:, :3] = 0
|
||||
local, camera, verts = [], [], []
|
||||
for start in range(0, count, 64):
|
||||
end = min(count, start + 64)
|
||||
sides = ['right'] * (end - start)
|
||||
local.append(fk.joints(local_pose[start:end], betas[start:end], torch.zeros_like(trans[start:end]), sides).numpy() / 1000)
|
||||
camera.append(fk.joints(pose[start:end], betas[start:end], trans[start:end], sides).numpy() / 1000)
|
||||
verts.append(fk.verts(pose[start:end], betas[start:end], trans[start:end], sides).numpy() / 1000)
|
||||
local = np.concatenate(local)
|
||||
camera = np.concatenate(camera)
|
||||
verts = np.concatenate(verts)
|
||||
local -= local[:, :1].copy()
|
||||
world = np.einsum('tij,tkj->tki', c2w[:, :3, :3], camera) + c2w[:, None, :3, 3]
|
||||
root_R = c2w[:, :3, :3] @ Rotation.from_rotvec(pose[:, :3].numpy()).as_matrix()
|
||||
reconstructed = np.einsum('tij,tkj->tki', root_R, local) + world[:, :1]
|
||||
vertex_error = float(np.abs(verts - source['verts_cam']).max())
|
||||
rigid_error = float(np.abs(reconstructed - world).max())
|
||||
world_vertex_error = float(np.abs(np.einsum('tij,tkj->tki', c2w[:, :3, :3], verts) + c2w[:, None, :3, 3] - source['verts_world']).max())
|
||||
assert np.isfinite(world).all() and np.isfinite(local).all()
|
||||
assert max(vertex_error, rigid_error, world_vertex_error) < 1e-5
|
||||
valid = np.asarray(source['pred_valid'], dtype=bool)
|
||||
observed_valid = valid.copy()
|
||||
valid_ids = np.flatnonzero(valid)
|
||||
for group in np.split(valid_ids, np.flatnonzero(np.diff(valid_ids) > 1) + 1):
|
||||
if len(group) < args.min_detection_run:
|
||||
valid[group] = False
|
||||
local_raw, wrist_raw, root_raw = local.copy(), world[:, 0].copy(), root_R.copy()
|
||||
wrist = wrist_raw.copy()
|
||||
if a_policy := (args.gap_policy == 'interpolate' and not valid.all()):
|
||||
observed = np.flatnonzero(valid)
|
||||
assert len(observed) >= 2, 'Too few detected frames for motion export'
|
||||
times = np.arange(count)
|
||||
local = np.stack([np.interp(times, observed, local[:, i, j][observed])
|
||||
for i in range(21) for j in range(3)], axis=1).reshape(count,21,3)
|
||||
wrist = np.stack([np.interp(times, observed, wrist[:, j][observed]) for j in range(3)],axis=1)
|
||||
root_R = Slerp(observed, Rotation.from_matrix(root_R[observed]))(np.clip(times,observed[0],observed[-1])).as_matrix()
|
||||
args.output.parent.mkdir(parents=True, exist_ok=True)
|
||||
np.savez_compressed(args.output, joints=local, wrist_world=wrist,
|
||||
root_orient=Rotation.from_matrix(root_R).as_rotvec(), fps=source['fps'],
|
||||
detection_valid=valid, detection_valid_raw=observed_valid, source=str(args.input.resolve()),
|
||||
source_model='HandFlow/manopth flat_hand_mean=True', joints_world_raw=world,
|
||||
joints_raw=local_raw, wrist_world_raw=wrist_raw, root_world_R_raw=root_raw,
|
||||
gap_policy=args.gap_policy)
|
||||
report = dict(frames=count, fps=float(source['fps']), finite=True,
|
||||
detected_frames=int(source['pred_valid'].sum()),
|
||||
usable_detection_frames=int(valid.sum()),
|
||||
rejected_isolated_detection_frames=np.flatnonzero(observed_valid & ~valid).tolist(),
|
||||
min_detection_run=args.min_detection_run,
|
||||
camera_vertex_reproduction_max_error_m=vertex_error,
|
||||
world_vertex_reproduction_max_error_m=world_vertex_error,
|
||||
local_to_world_joint_max_error_m=rigid_error,
|
||||
source=str(args.input.resolve()),
|
||||
gap_policy=args.gap_policy, filled_frames=int((~valid).sum()) if a_policy else 0,
|
||||
gap_note='Retargeting holds endpoint detections and interpolates internal gaps; raw inference preserved',
|
||||
convention='HandFlow manopth, flat_hand_mean=True; mm converted to m; c2w applied once')
|
||||
args.output.with_suffix('.validation.json').write_text(json.dumps(report, indent=2))
|
||||
print(json.dumps(report, indent=2))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,201 @@
|
||||
"""Export the existing right-hand replay without changing its model or motion.
|
||||
|
||||
Uses the dataset layout of HDF5_REQUIREMENTS.md, with explicit right-hand
|
||||
model/coordinate exceptions. This is not a left-hand l20_tracking_v1 PASS.
|
||||
"""
|
||||
import argparse
|
||||
import hashlib
|
||||
import json
|
||||
from pathlib import Path
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
import h5py
|
||||
import numpy as np
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
NAMES = [f'{f}_{j}' for f in ('index', 'middle', 'pinky', 'ring')
|
||||
for j in ('dip', 'mcp_pitch', 'mcp_roll', 'pip')] + [
|
||||
'thumb_cmc_pitch', 'thumb_cmc_roll', 'thumb_cmc_yaw', 'thumb_ip', 'thumb_mcp']
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument('--source', type=Path, default=ROOT/'output/l20_15886123_stable/motion.npz')
|
||||
parser.add_argument('--output', type=Path, default=ROOT/'output/hdf5_delivery/right')
|
||||
parser.add_argument('--scene', type=Path, help='MuJoCo scene for a full-replay qpos source')
|
||||
args = parser.parse_args()
|
||||
args.output.mkdir(parents=True, exist_ok=True)
|
||||
a = np.load(args.source, allow_pickle=False)
|
||||
scene_description = None
|
||||
if args.scene:
|
||||
import mujoco
|
||||
model = mujoco.MjModel.from_xml_path(str(args.scene.resolve()))
|
||||
data = mujoco.MjData(model)
|
||||
rows = a['qpos']
|
||||
assert rows.ndim == 2 and rows.shape[1] == model.nq and np.isfinite(rows).all()
|
||||
addresses = [model.jnt_qposadr[model.joint(n).id] for n in NAMES]
|
||||
body_id = model.body('hand_base_link').id
|
||||
positions, rotations = [], []
|
||||
for row in rows:
|
||||
data.qpos[:] = row
|
||||
mujoco.mj_forward(model, data)
|
||||
positions.append(data.xpos[body_id].copy())
|
||||
rotations.append(data.xquat[body_id].copy())
|
||||
positions = np.asarray(positions)
|
||||
assert np.allclose(positions, a['wrist_pos'], atol=1e-9, rtol=0)
|
||||
a = dict(qpos=rows[:,addresses], joint_names=np.asarray(NAMES), wrist_pos=positions,
|
||||
wrist_quat_wxyz=np.asarray(rotations), time=a['time'],
|
||||
detection_valid=np.ones(len(rows), dtype=bool),
|
||||
scene_rotation=np.eye(3), scene_translation=np.zeros(3))
|
||||
scene_description = (
|
||||
'Video 2047635068, full 1333-frame right-hand replay: Dyn-HaMR -> '
|
||||
'run_dex_l20.py -> temporal smoothing -> build_l20_full_replay.py contact registration. '
|
||||
'Actual source is the final replay qpos, not the earlier raw retargeting. '
|
||||
'hand_base_link world position and active local-to-world WXYZ rotation extracted '
|
||||
'with MuJoCo mj_forward for every frame; finger columns resolved by joint name. '
|
||||
'Source world is explicitly the FINAL replay scene world; world_from_source is identity '
|
||||
'because exported FK is already in that same world, not because camera calibration is known. '
|
||||
'Axes/origin are the existing fixed right-handed replay scene with display Z up: upstream '
|
||||
'scene was aligned using estimated bottle orientation at frame 600, shifted to the display '
|
||||
'placement, then raised for floor clearance; physical gravity alignment is unverified. '
|
||||
'Original human wrist/palm mapping, temporal smoothing and per-frame contact-registration '
|
||||
'corrections are inherited unchanged. Root is robot link origin, not its center of mass. '
|
||||
'valid=true means finite kinematic reference state satisfying the actual right URDF '
|
||||
'limits and mimic; original detection validity was not supplied, so this is NOT an '
|
||||
'observed-frame mask or measurement confidence. Motion and object registration are '
|
||||
'estimated, not ground truth; no full dynamic rollout or hardware execution claim. '
|
||||
'Source scene: ' + str(args.scene.resolve()))
|
||||
asset = ROOT/'third_party/l20_assets/L20/RIGHT'
|
||||
urdf = asset/'linkerhand_g20_right.urdf'
|
||||
tree = ET.parse(urdf).getroot()
|
||||
files = {urdf}
|
||||
for mesh in tree.iter('mesh'):
|
||||
files.add((asset/mesh.attrib['filename']).resolve())
|
||||
entries = [{'path': p.relative_to(asset.resolve()).as_posix(),
|
||||
'sha256': hashlib.sha256(p.read_bytes()).hexdigest()}
|
||||
for p in sorted(files, key=lambda p: p.relative_to(asset.resolve()).as_posix())]
|
||||
# Explicit local algorithm, not the unavailable receiver's asset hash tool.
|
||||
payload = ''.join(f"{e['path']}\t{e['sha256']}\n" for e in entries).encode('utf-8')
|
||||
digest = hashlib.sha256(payload).hexdigest()
|
||||
js = {j.attrib['name']: j for j in tree.findall('joint') if j.attrib['type'] != 'fixed'}
|
||||
manifest = dict(hand_side='right', root_link='hand_base_link', asset_sha256=digest,
|
||||
hash_algorithm='sha256(UTF-8 concatenation of sorted relative_path TAB file_sha256 LF); URDF plus referenced meshes only; local export convention',
|
||||
source_urdf=str(urdf), files=entries, joints=[])
|
||||
for name in NAMES:
|
||||
j = js[name]
|
||||
item = dict(name=name, lower_rad=float(j.find('limit').get('lower')),
|
||||
upper_rad=float(j.find('limit').get('upper')))
|
||||
m = j.find('mimic')
|
||||
if m is not None:
|
||||
item['mimic'] = dict(joint=m.get('joint'), multiplier=float(m.get('multiplier', '1')),
|
||||
offset=float(m.get('offset', '0')))
|
||||
manifest['joints'].append(item)
|
||||
(args.output/'right_model_manifest.json').write_text(json.dumps(manifest, indent=2)+'\n')
|
||||
source_names = a['joint_names'].tolist()
|
||||
assert len(source_names) == len(set(source_names)) == len(NAMES)
|
||||
assert set(source_names) == set(NAMES)
|
||||
q = a['qpos'][:, [source_names.index(n) for n in NAMES]].astype(np.float32)
|
||||
pos = a['wrist_pos'].astype(np.float32)
|
||||
quat = a['wrist_quat_wxyz'].copy()
|
||||
quat /= np.linalg.norm(quat, axis=1, keepdims=True)
|
||||
for i in range(1, len(quat)):
|
||||
if quat[i] @ quat[i-1] < 0:
|
||||
quat[i] *= -1
|
||||
quat = quat.astype(np.float32)
|
||||
valid = a['detection_valid'].astype(bool)
|
||||
transform = np.eye(4, dtype=np.float64)
|
||||
transform[:3, :3] = a['scene_rotation']
|
||||
transform[:3, 3] = a['scene_translation']
|
||||
description = (
|
||||
'Video 15886123; HandFlow/manopth + ViPE camera-to-world; '
|
||||
'scripts/export_handflow_dex.py -> retarget_l20_video.py -> stabilize_l20_temporal.py. '
|
||||
'Right L20 URDF linear mimic, 16 independent and 21 total joints. '
|
||||
'hand_base_link origin assigned to estimated MANO wrist; orientation uses human palm_basis '
|
||||
'and robot neutral palm_basis, not a measured anatomical mount. '
|
||||
'Positions already use the stored fixed scene transform, followed by offline temporal smoothing. '
|
||||
'Scene origin and XYZ axes follow the original replay: first unsmoothed wrist at (0,0,0.2), '
|
||||
'first unsmoothed root orientation identity. Right-handed fixed scene with display Z up; '
|
||||
'physical gravity alignment is unknown. No new pose recentering applied by exporter. '
|
||||
'world_from_source maps the upstream estimated world to this existing display world; '
|
||||
'smoothing additionally changes individual poses. '
|
||||
'valid is upstream usable detection mask, not ground-truth tracking accuracy: '
|
||||
'first 24 frames are held/interpolated finite estimates and remain false. '
|
||||
'All frames have monocular scale/depth uncertainty, possible drift/occlusion errors, '
|
||||
'future-frame temporal smoothing; no contact, dynamics or hardware validation.')
|
||||
attrs = dict(schema_version='l20_tracking_v1', embodiment='L20', hand_side='right',
|
||||
asset_sha256=digest, root_link='hand_base_link', provenance='expert_retargeted',
|
||||
source_description=description,
|
||||
metric_scale_provenance='Upstream MANO FK millimeters divided by 1000; camera translations used in upstream meter convention. Monocular/model-estimated metric scale, no measured reference length or gravity calibration; absolute scale error unknown. Input motion.npz positions already in estimated meters, so export scale_to_meters=1.0.',
|
||||
scale_to_meters=1.0,
|
||||
contract_status='RIGHT_HAND_VARIANT_NOT_STRICT_LEFT_V1; actual URDF mimic; local asset hash algorithm; display-world gravity unverified',
|
||||
source_sha256=hashlib.sha256(args.source.read_bytes()).hexdigest(),
|
||||
asset_hash_algorithm=manifest['hash_algorithm'])
|
||||
if scene_description:
|
||||
attrs['source_description'] = scene_description
|
||||
attrs['metric_scale_provenance'] = (
|
||||
'Final replay positions already in estimated meters, inherited from Dyn-HaMR MANO '
|
||||
'world reconstruction and robot geometry; export scale_to_meters=1.0. '
|
||||
'No measured absolute scale reference is established; error unknown. '
|
||||
'Scene alignment and contact fitting are engineering estimates, not scale measurements.')
|
||||
attrs['valid_semantics'] = 'kinematic_reference_valid; source_detection_validity_unavailable'
|
||||
attrs['source_scene_sha256'] = hashlib.sha256(args.scene.read_bytes()).hexdigest()
|
||||
good = np.flatnonzero(valid)
|
||||
runs = [r for r in np.split(good, np.flatnonzero(np.diff(good) > 1)+1) if len(r) >= 2]
|
||||
assert runs
|
||||
run = max(runs, key=len)
|
||||
sample = slice(int(run[0]), int(min(run[-1]+1, run[0]+90)))
|
||||
reports = []
|
||||
for filename, selection in [('sample_right.hdf5', sample), ('demonstrations_right.hdf5', slice(0, len(q)))]:
|
||||
path = args.output/filename
|
||||
times = a['time'][selection].astype(np.float64)
|
||||
times -= times[0]
|
||||
with h5py.File(path, 'w') as f:
|
||||
for k, v in attrs.items():
|
||||
f.attrs[k] = v
|
||||
meta = f.create_group('metadata')
|
||||
meta.create_dataset('joint_names', data=NAMES, dtype=h5py.string_dtype('utf-8'))
|
||||
meta.create_dataset('world_from_source', data=transform)
|
||||
ep = f.create_group('episodes/demo_000000')
|
||||
ep.attrs['source_frame_start'] = selection.start
|
||||
ep.attrs['source_frame_end_exclusive'] = selection.stop
|
||||
for name, value in dict(time=times, wrist_position=pos[selection],
|
||||
wrist_quaternion=quat[selection], joint_position=q[selection],
|
||||
valid=valid[selection]).items():
|
||||
ep.create_dataset(name, data=value)
|
||||
# Reopen the actual serialized file and validate against the actual right URDF.
|
||||
with h5py.File(path, 'r') as f:
|
||||
ep = f['episodes/demo_000000']
|
||||
t, p, r, angles, mask = [ep[n][:] for n in
|
||||
('time','wrist_position','wrist_quaternion','joint_position','valid')]
|
||||
count = len(t)
|
||||
assert [(x.shape, x.dtype) for x in (t,p,r,angles,mask)] == [
|
||||
((count,),np.dtype('float64')),((count,3),np.dtype('float32')),
|
||||
((count,4),np.dtype('float32')),((count,21),np.dtype('float32')),((count,),np.dtype('bool'))]
|
||||
assert count >= 2 and t[0] == 0 and np.all(np.diff(t)>0) and mask.any()
|
||||
assert all(np.isfinite(x).all() for x in (t,p,r,angles))
|
||||
assert f['metadata/joint_names'].asstr()[:].tolist() == NAMES
|
||||
assert h5py.check_string_dtype(f['metadata/joint_names'].dtype).encoding == 'utf-8'
|
||||
assert np.max(np.abs(np.linalg.norm(r[mask],axis=1)-1)) < 1e-4
|
||||
assert np.all(np.sum(r[1:]*r[:-1],axis=1)[mask[1:] & mask[:-1]] >= 0)
|
||||
assert np.allclose(transform[3], [0,0,0,1])
|
||||
assert np.allclose(transform[:3,:3].T@transform[:3,:3], np.eye(3))
|
||||
assert abs(np.linalg.det(transform[:3,:3])-1)<1e-8
|
||||
residuals = {}
|
||||
for i, joint in enumerate(manifest['joints']):
|
||||
assert angles[mask,i].min() >= joint['lower_rad']-1e-6
|
||||
assert angles[mask,i].max() <= joint['upper_rad']+1e-6
|
||||
if 'mimic' in joint:
|
||||
m = joint['mimic']
|
||||
err = np.abs(angles[mask,i]-m['multiplier']*angles[mask,NAMES.index(m['joint'])]-m['offset'])
|
||||
residuals[joint['name']] = float(err.max())
|
||||
assert err.max() <= 1e-3
|
||||
reports.append(dict(file=filename, right_model_validation='PASS', strict_left_v1='NOT_COMPATIBLE',
|
||||
frames=count, valid_frames=int(mask.sum()), source_frame_start=selection.start,
|
||||
source_frame_end_exclusive=selection.stop, time_last_s=float(t[-1]),
|
||||
mimic_residual_rad=residuals, sha256=hashlib.sha256(path.read_bytes()).hexdigest()))
|
||||
(args.output/'validation.json').write_text(json.dumps(reports,indent=2)+'\n')
|
||||
print(json.dumps(reports,indent=2))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,190 @@
|
||||
"""Actuated L20 + freely simulated metric bottle, with a prescribed moving wrist."""
|
||||
import os
|
||||
os.environ.setdefault('MUJOCO_GL','osmesa')
|
||||
import argparse,json
|
||||
import xml.etree.ElementTree as ET
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
import mujoco
|
||||
from scipy.spatial.transform import Rotation
|
||||
from scipy.optimize import lsq_linear
|
||||
from l20_calibrated import ROOT,Calibration
|
||||
|
||||
BASE=ROOT/'output/l20_2047635068'
|
||||
OUT=BASE/'bottle_grasp'
|
||||
|
||||
def feedback_torque(model,data,cal,addr,objects,object_body,target_pos,target_velocity):
|
||||
selected=[c for c in data.contact if any(g in objects for g in c.geom)]
|
||||
if not selected:return None,None
|
||||
center=data.xipos[object_body].copy();velocity=np.zeros(6)
|
||||
mujoco.mj_objectVelocity(model,data,mujoco.mjtObj.mjOBJ_BODY,object_body,velocity,0)
|
||||
desiredforce=np.array([0,0,model.body_mass[object_body]*9.81])+100*(target_pos-center)+2*(target_velocity-velocity[3:])
|
||||
rotation=Rotation.from_matrix(data.xmat[object_body].reshape(3,3))
|
||||
desiredtorque=-.08*rotation.as_rotvec()-.008*velocity[:3]
|
||||
target=np.r_[desiredforce,desiredtorque/.03]
|
||||
columns=[];jacobians=[];rays_all=[];preferred=[]
|
||||
for c in selected:
|
||||
object_first=c.geom[0] in objects
|
||||
normal=-c.frame[:3]*(1 if object_first else -1)
|
||||
tangent=np.cross(normal,[0,0,1])
|
||||
if np.linalg.norm(tangent)<.01:tangent=np.cross(normal,[1,0,0])
|
||||
tangent/=np.linalg.norm(tangent);second=np.cross(normal,tangent)
|
||||
rays=np.array([normal+.6*(np.cos(a)*tangent+np.sin(a)*second) for a in np.arange(4)*np.pi/2]).T
|
||||
rays_all.append(rays);columns.append(np.vstack([rays,np.cross((c.pos-center)[None,:],rays.T).T/.03]))
|
||||
body=int(model.geom_bodyid[c.geom[1] if object_first else c.geom[0]])
|
||||
preferred.append(6 if model.body(body).name.startswith('thumb') else 1.5)
|
||||
jac=np.zeros((3,model.nv));mujoco.mj_jac(model,data,jac,None,c.pos,body)
|
||||
ja=np.stack([jac[:,model.jnt_dofadr[model.joint(n).id]] for n in cal.active],axis=1)
|
||||
for n,(parent,_,_) in cal.curves.items():
|
||||
slope=cal.passive_value(n,data.qpos[addr[parent]])[1]
|
||||
ja[:,cal.active.index(parent)]+=slope*jac[:,model.jnt_dofadr[model.joint(n).id]]
|
||||
jacobians.append(ja)
|
||||
A=np.concatenate(columns,axis=1);reg=np.zeros((len(selected),len(selected)*4))
|
||||
for i in range(len(selected)):reg[i,i*4:i*4+4]=.025
|
||||
sol=lsq_linear(np.vstack([A,reg]),np.r_[target,.025*np.array(preferred)],bounds=(0,10),tol=1e-6,max_iter=50)
|
||||
tau=sum(j.T@(r@sol.x[i*4:i*4+4]) for i,(j,r) in enumerate(zip(jacobians,rays_all)))
|
||||
for i,n in enumerate(cal.active):tau[i]+=data.qfrc_bias[model.jnt_dofadr[model.joint(n).id]]
|
||||
return tau,float(np.linalg.norm(A@sol.x-target))
|
||||
|
||||
def build_physics(kp=30):
|
||||
tree=ET.parse(BASE/'contact_object/contact_scene.xml');root=tree.getroot()
|
||||
hand=root.find("worldbody/body[@name='hand_base_link']")
|
||||
hand.remove(hand.find('freejoint'));hand.set('mocap','true')
|
||||
option=root.find('option');option.set('timestep','.001');option.set('integrator','implicitfast');option.set('iterations','80')
|
||||
root.find('default/joint').set('damping','.08')
|
||||
eq=root.find('equality')
|
||||
if eq is None:eq=ET.SubElement(root,'equality')
|
||||
actuator=ET.SubElement(root,'actuator')
|
||||
cal=Calibration()
|
||||
for n in cal.active:
|
||||
ET.SubElement(actuator,'position',name='motor_'+n,joint=n,kp=str(kp),kv='.3',
|
||||
forcelimited='true',forcerange='-1 1')
|
||||
for n,(parent,x,y) in cal.curves.items():
|
||||
ET.SubElement(eq,'joint',name='lut_'+n,joint1=n,joint2=parent,
|
||||
polycoef='0 1 0 0 0',solref='.003 1',solimp='.95 .99 .001')
|
||||
path=OUT/'physics_scene.xml';tree.write(path)
|
||||
return path
|
||||
|
||||
def run(seconds=3.,lift=False,kp=30.,preload=0.,force_preload=False,force_control=False,feedback=False):
|
||||
path=build_physics(kp);model=mujoco.MjModel.from_xml_path(str(path));data=mujoco.MjData(model)
|
||||
cal=Calibration();old=mujoco.MjModel.from_xml_path(str(BASE/'contact_object/contact_scene.xml'))
|
||||
fitted=np.load(BASE/'contact_object/contact_result.npz')['qpos']
|
||||
names=list(cal.tables)
|
||||
addr={n:model.jnt_qposadr[mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_JOINT,n)] for n in names}
|
||||
for n in names:
|
||||
oldaddr=old.jnt_qposadr[mujoco.mj_name2id(old,mujoco.mjtObj.mjOBJ_JOINT,n)]
|
||||
data.qpos[addr[n]]=fitted[oldaddr]
|
||||
ow=old.jnt_qposadr[mujoco.mj_name2id(old,mujoco.mjtObj.mjOBJ_JOINT,'wrist_free')]
|
||||
oo=old.jnt_qposadr[mujoco.mj_name2id(old,mujoco.mjtObj.mjOBJ_JOINT,'object_free')]
|
||||
obj=model.jnt_qposadr[mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_JOINT,'object_free')]
|
||||
# Rigidly reorient the entire registration so bottle +Z is world up.
|
||||
rotation=Rotation.from_quat(fitted[oo+3:oo+7][[1,2,3,0]]).as_matrix().T
|
||||
handpos=rotation@(fitted[ow:ow+3]-fitted[oo:oo+3])+[0,0,.3]
|
||||
handrot=rotation@Rotation.from_quat(fitted[ow+3:ow+7][[1,2,3,0]]).as_matrix()
|
||||
data.mocap_pos[0]=handpos;data.mocap_quat[0]=Rotation.from_matrix(handrot).as_quat()[[3,0,1,2]]
|
||||
data.qpos[obj:obj+7]=[0,0,.3,1,0,0,0]
|
||||
controls=np.array([data.qpos[addr[n]] for n in cal.active])
|
||||
reference_controls=controls.copy()
|
||||
for i,n in enumerate(cal.active):
|
||||
if n.endswith(('pitch','pip','mcp')):controls[i]+=preload
|
||||
controls[i]=np.clip(controls[i],*cal.bounds[n])
|
||||
data.ctrl[:]=controls
|
||||
objects=np.flatnonzero(model.geom_contype==2)
|
||||
object_body=mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_BODY,'grasp_object')
|
||||
preload_report={}
|
||||
if force_preload:
|
||||
for n,(parent,x,y) in cal.curves.items():
|
||||
a=data.qpos[addr[parent]];v,s=cal.passive_value(n,a)
|
||||
ei=mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_EQUALITY,'lut_'+n)
|
||||
model.eq_data[ei,:5]=[v-s*a,s,0,0,0]
|
||||
mujoco.mj_forward(model,data)
|
||||
selected=[c for c in data.contact if any(g in objects for g in c.geom)]
|
||||
columns=[];jacobians=[];cone=[]
|
||||
center=data.xipos[object_body].copy()
|
||||
for c in selected:
|
||||
object_first=c.geom[0] in objects
|
||||
outward=c.frame[:3]*(1 if object_first else -1)
|
||||
normal=-outward
|
||||
tangent=np.cross(normal,[0,0,1])
|
||||
if np.linalg.norm(tangent)<.01:tangent=np.cross(normal,[1,0,0])
|
||||
tangent/=np.linalg.norm(tangent);second=np.cross(normal,tangent)
|
||||
rays=np.array([normal+.6*(np.cos(a)*tangent+np.sin(a)*second) for a in np.arange(4)*np.pi/2]).T
|
||||
cone.append(rays)
|
||||
columns.append(np.vstack([rays,np.cross((c.pos-center)[None,:],rays.T).T/.03]))
|
||||
body=model.geom_bodyid[c.geom[1] if object_first else c.geom[0]]
|
||||
jac=np.zeros((3,model.nv));mujoco.mj_jac(model,data,jac,None,c.pos,int(body))
|
||||
activejac=np.stack([jac[:,model.jnt_dofadr[mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_JOINT,n)]] for n in cal.active],axis=1)
|
||||
for n,(parent,x,y) in cal.curves.items():
|
||||
slope=cal.passive_value(n,data.qpos[addr[parent]])[1]
|
||||
di=model.jnt_dofadr[mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_JOINT,n)]
|
||||
activejac[:,cal.active.index(parent)]+=slope*jac[:,di]
|
||||
jacobians.append(activejac)
|
||||
if not selected:raise RuntimeError('No contacts for force preload')
|
||||
A=np.concatenate(columns,axis=1)
|
||||
regular=np.zeros((len(selected),len(selected)*4))
|
||||
for i in range(len(selected)):regular[i,i*4:i*4+4]=.025
|
||||
target=np.r_[0,0,model.body_mass[object_body]*9.81,0,0,0]
|
||||
preferred=np.array([6.]+[1.5]*(len(selected)-1))
|
||||
solution=lsq_linear(np.vstack([A,regular]),np.r_[target,.025*preferred],bounds=(0,10),tol=1e-10)
|
||||
tau=np.zeros(16)
|
||||
for i,jac in enumerate(jacobians):tau+=jac.T@(cone[i]@solution.x[i*4:i*4+4])
|
||||
for i,n in enumerate(cal.active):
|
||||
dof=model.jnt_dofadr[mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_JOINT,n)]
|
||||
tau[i]+=data.qfrc_bias[dof]
|
||||
controls+=np.clip(tau,-1,1)/kp
|
||||
for i,n in enumerate(cal.active):controls[i]=np.clip(controls[i],*cal.bounds[n])
|
||||
data.ctrl[:]=controls
|
||||
preload_report=dict(wrench_residual=float(np.linalg.norm(A@solution.x-target)),desired_motor_torque_Nm=tau.tolist(),contact_normal_loads_N=(regular@solution.x/.025).tolist())
|
||||
rows=[];forces=[];contacts=[];relative=[];torques=[];wrist_path=[]
|
||||
initial_rel=np.array([0,0,.3])-handpos
|
||||
feedback_errors=[]
|
||||
for k in range(int(seconds/model.opt.timestep)):
|
||||
t=k*model.opt.timestep
|
||||
for n,(parent,x,y) in cal.curves.items():
|
||||
active_angle=data.qpos[addr[parent]];passive,slope=cal.passive_value(n,active_angle)
|
||||
eid=mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_EQUALITY,'lut_'+n)
|
||||
model.eq_data[eid,:5]=[passive-slope*active_angle,slope,0,0,0]
|
||||
displacement=np.zeros(3)
|
||||
if lift:
|
||||
phase=np.clip((t-.5)/1.,0,1);smooth=phase*phase*(3-2*phase)
|
||||
displacement=np.array([.02*smooth,0,.10*smooth])
|
||||
data.mocap_pos[0]=handpos+displacement
|
||||
if feedback and k%10==0:
|
||||
phase=np.clip((t-.5),0,1)
|
||||
target_velocity=np.array([.02,0,.10])*6*phase*(1-phase) if lift else np.zeros(3)
|
||||
target_center=np.array([0,0,.3663])+displacement
|
||||
newtau,residual=feedback_torque(model,data,cal,addr,objects,object_body,target_center,target_velocity)
|
||||
if newtau is not None:tau=newtau;feedback_errors.append(residual)
|
||||
if force_control:
|
||||
if not force_preload:raise ValueError('force-control requires force-preload')
|
||||
angles=np.array([data.qpos[addr[n]] for n in cal.active])
|
||||
data.ctrl[:]=angles+(tau+1.0*(reference_controls-angles))/kp
|
||||
mujoco.mj_step(model,data)
|
||||
if not np.isfinite(data.qpos).all():raise RuntimeError('Nonfinite simulation')
|
||||
if k%33==0:
|
||||
rows.append(data.qpos.copy());wrist_path.append(np.r_[data.mocap_pos[0],data.mocap_quat[0]])
|
||||
selected=[i for i,c in enumerate(data.contact) if any(g in objects for g in c.geom)]
|
||||
normalforce=0
|
||||
for ci in selected:
|
||||
f=np.zeros(6);mujoco.mj_contactForce(model,data,ci,f);normalforce+=max(0,f[0])
|
||||
forces.append(normalforce);contacts.append(len(selected));torques.append(data.actuator_force.copy())
|
||||
relative.append(np.linalg.norm(data.qpos[obj:obj+3]-data.mocap_pos[0]-initial_rel))
|
||||
rows=np.array(rows);relative=np.array(relative)
|
||||
report=dict(seconds=seconds,lift=lift,kp=kp,preload_rad=preload,force_preload=preload_report,force_control=force_control,feedback=feedback,
|
||||
feedback_wrench_residual_mean=float(np.mean(feedback_errors)) if feedback_errors else None,
|
||||
max_relative_drift_m=float(relative.max()),final_relative_drift_m=float(relative[-1]),
|
||||
object_vertical_displacement_m=float(data.qpos[obj+2]-.3),
|
||||
contact_frames=int(np.sum(np.array(contacts)>0)),total_frames=len(rows),
|
||||
normal_force_mean_N=float(np.mean(forces)),peak_actuator_torque_Nm=float(np.max(np.abs(torques))),
|
||||
retained=bool(relative.max()<.02),
|
||||
mass_kg=float(model.body_mass[object_body]),friction=.6,
|
||||
boundaries=['Prescribed moving wrist, no arm dynamics','16 force-limited active joints; 5 nonlinear LUT equality constraints','No object weld or position reset','Object registration is estimated from hand pose, not tri-camera object tracking'])
|
||||
np.savez_compressed(OUT/'physics_motion.npz',qpos=rows,wrist=np.array(wrist_path),fps=1/(33*model.opt.timestep),
|
||||
object_qpos_address=obj,contact_count=contacts,normal_force=forces,relative_drift=relative,actuator_force=torques,
|
||||
source_to_upright_rotation=rotation,controls=controls)
|
||||
(OUT/'physics_validation.json').write_text(json.dumps(report,indent=2));print(json.dumps(report,indent=2),flush=True)
|
||||
return report
|
||||
|
||||
if __name__=='__main__':
|
||||
p=argparse.ArgumentParser();p.add_argument('--seconds',type=float,default=3);p.add_argument('--lift',action='store_true');p.add_argument('--kp',type=float,default=30);p.add_argument('--preload',type=float,default=0);p.add_argument('--force-preload',action='store_true');p.add_argument('--force-control',action='store_true');p.add_argument('--feedback',action='store_true');a=p.parse_args()
|
||||
run(a.seconds,a.lift,a.kp,a.preload,a.force_preload,a.force_control,a.feedback)
|
||||
@@ -0,0 +1,73 @@
|
||||
"""Full-length contact registration; object poses are inferred targets, not physics."""
|
||||
import json
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation,Slerp
|
||||
from l20_contact import ContactRefiner
|
||||
from l20_calibrated import ROOT
|
||||
|
||||
BASE=ROOT/'output/l20_2047635068';OUT=BASE/'bottle_grasp'
|
||||
def main():
|
||||
motion=np.load(BASE/'motion.npz');spec=json.loads((OUT/'object_spec.json').read_text())
|
||||
solver=ContactRefiner(spec,OUT)
|
||||
count=len(motion['qpos']);times=np.arange(count)
|
||||
rotation=Rotation.from_quat(motion['wrist_quat_wxyz'][:,[1,2,3,0]])
|
||||
reference=666
|
||||
# Infer a constant bottle-to-palm registration over the holding phase.
|
||||
rel_pos=rotation[reference].inv().apply(np.array(spec['position'])-motion['wrist_pos'][reference])
|
||||
rel_rot=rotation[reference].inv()*Rotation.from_quat(np.array(spec['quat_wxyz'])[[1,2,3,0]])
|
||||
bottle_pos=rotation.apply(np.tile(rel_pos,(count,1)))+motion['wrist_pos']
|
||||
bottle_rot=rotation*rel_rot
|
||||
# Contact onset bracket comes from visual inspection: open at 100, grasped by 200.
|
||||
onset=160
|
||||
bottle_pos[:onset]=bottle_pos[onset]
|
||||
bottle_quat=bottle_rot.as_quat()[:,[3,0,1,2]]
|
||||
bottle_quat[:onset]=bottle_quat[onset]
|
||||
samples=np.unique(np.r_[np.arange(onset,count,8),count-1])
|
||||
fits=[];reports=[]
|
||||
for t in samples:
|
||||
active=np.array([motion['qpos'][t,list(motion['joint_names']).index(n)] for n in solver.cal.active])
|
||||
x,q,report=solver.solve(active,motion['wrist_pos'][t],motion['wrist_quat_wxyz'][t],bottle_pos[t],bottle_quat[t])
|
||||
fits.append(x);reports.append(report)
|
||||
if len(fits)%20==0:print(f'contact keyframes {len(fits)}/{len(samples)}',flush=True)
|
||||
fits=np.array(fits)
|
||||
interp=np.stack([np.interp(times,samples,fits[:,j]) for j in range(22)],axis=1)
|
||||
source_active=np.stack([motion['qpos'][:,list(motion['joint_names']).index(n)] for n in solver.cal.active],axis=1)
|
||||
# Blend acquisition only; preserve pre-contact source motion.
|
||||
weight=np.clip((times-(onset-30))/30,0,1);weight=weight*weight*(3-2*weight)
|
||||
interp[:,:16]=source_active*(1-weight[:,None])+interp[:,:16]*weight[:,None]
|
||||
interp[:,16:]*=weight[:,None]
|
||||
repairs=[]
|
||||
for t in range(onset,count):
|
||||
solver.set_pose(interp[t],motion['wrist_pos'][t],motion['wrist_quat_wxyz'][t],bottle_pos[t],bottle_quat[t])
|
||||
if -solver.distances().min()>.0015:
|
||||
x,_,r=solver.solve(source_active[t],motion['wrist_pos'][t],motion['wrist_quat_wxyz'][t],bottle_pos[t],bottle_quat[t],initial=interp[t])
|
||||
if r['max_penetration_after_mm']>1.5 and t>onset:
|
||||
alternative,_,retry=solver.solve(source_active[t],motion['wrist_pos'][t],motion['wrist_quat_wxyz'][t],bottle_pos[t],bottle_quat[t],initial=interp[t-1])
|
||||
if retry['max_penetration_after_mm']<r['max_penetration_after_mm']:x,r=alternative,retry
|
||||
interp[t]=x;repairs.append(dict(frame=t,**r))
|
||||
if t%100==0:print(f'frame collision repair {t}/{count}: {len(repairs)} repaired',flush=True)
|
||||
qpos=[];wp=[];wq=[];penetration=[]
|
||||
for t,x in enumerate(interp):
|
||||
solver.set_pose(x,motion['wrist_pos'][t],motion['wrist_quat_wxyz'][t],bottle_pos[t],bottle_quat[t])
|
||||
values=solver.expand(x[:16]);qpos.append([values[str(n)] for n in motion['joint_names']])
|
||||
wp.append(solver.data.qpos[solver.wrist:solver.wrist+3].copy());wq.append(solver.data.qpos[solver.wrist+3:solver.wrist+7].copy())
|
||||
penetration.append(max(0,-solver.distances().min()))
|
||||
qpos=np.array(qpos);wp=np.array(wp);wq=np.array(wq)
|
||||
commands=np.array([solver.cal.command(dict(zip(motion['joint_names'],q))) for q in qpos])
|
||||
np.savez_compressed(OUT/'registered_motion.npz',qpos=qpos,joint_names=motion['joint_names'],wrist_pos=wp,wrist_quat_wxyz=wq,
|
||||
bottle_pos=bottle_pos,bottle_quat_wxyz=bottle_quat,command_u8=commands,fps=30,
|
||||
keyframes=samples,optimization_variables=interp,penetration_m=penetration,contact_onset_frame=onset)
|
||||
np.savetxt(OUT/'registered_trajectory.csv',np.c_[times/30,wp,wq,qpos,bottle_pos,bottle_quat],delimiter=',',
|
||||
header=','.join(['time_s','wrist_x','wrist_y','wrist_z','wrist_qw','wrist_qx','wrist_qy','wrist_qz']+list(motion['joint_names'])+['bottle_x','bottle_y','bottle_z','bottle_qw','bottle_qx','bottle_qy','bottle_qz']),comments='')
|
||||
report=dict(frames=count,fps=30,contact_keyframes=len(samples),converged_keyframes=sum(r['converged'] for r in reports),
|
||||
mean_keyframe_gap_before_mm=float(np.mean([r['mean_selected_surface_gap_before_mm'] for r in reports])),
|
||||
mean_keyframe_gap_after_mm=float(np.mean([r['mean_selected_surface_gap_after_mm'] for r in reports])),
|
||||
max_holding_penetration_mm=float(np.max(penetration[onset:])*1000),max_all_frame_penetration_mm=float(np.max(penetration)*1000),
|
||||
contact_onset_frame=onset,source='current 2047635068 video + supplied tri-camera bottle mesh',
|
||||
repaired_frames=len(repairs),repair_reports=repairs,
|
||||
alignment='Inferred fixed bottle-to-palm transform during holding, with contact pose corrections; no measured object trajectory or camera extrinsic alignment',
|
||||
boundary='Object poses in this file are prescribed registration targets. Dynamic grasp tests are separate and have not passed.',keyframe_reports=reports)
|
||||
(OUT/'sequence_validation.json').write_text(json.dumps(report,indent=2));print({k:v for k,v in report.items() if not k.endswith('_reports')},flush=True)
|
||||
|
||||
if __name__=='__main__':main()
|
||||
@@ -0,0 +1,104 @@
|
||||
"""L20 assets and JSON-calibrated passive coupling for dex-retargeting."""
|
||||
from pathlib import Path
|
||||
import json
|
||||
import xml.etree.ElementTree as ET
|
||||
import numpy as np
|
||||
from dex_retargeting.kinematics_adaptor import MimicJointKinematicAdaptor
|
||||
import l20_model_source as source
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
source.REPO_ROOT = ROOT / 'third_party/l20_assets'
|
||||
ASSET = source.REPO_ROOT / 'L20/RIGHT'
|
||||
FINGERS = ['thumb','index','middle','ring','pinky']
|
||||
TIPS = np.array([4,8,12,16,20])
|
||||
CHAINS = [(1,2,3,4),(5,6,7,8),(9,10,11,12),(13,14,15,16),(17,18,19,20)]
|
||||
|
||||
def palm_basis(p):
|
||||
z=p[9]-p[0]; z=z/np.linalg.norm(z)
|
||||
y=p[5]-p[17]; y=y-y.dot(z)*z; y=y/np.linalg.norm(y)
|
||||
return np.column_stack([np.cross(y,z),y,z])
|
||||
|
||||
class Calibration:
|
||||
def __init__(self):
|
||||
self.raw=json.loads((ASSET/'g20_right_G20.json').read_text())
|
||||
self.tables=self.raw['joints']
|
||||
urdf=ET.parse(ASSET/'linkerhand_g20_right.urdf').getroot()
|
||||
self.urdf_limits={j.get('name'):np.array([float(j.find('limit').get(k)) for k in ['lower','upper']]) for j in urdf.findall('joint')}
|
||||
self.active=sorted([n for n,x in self.tables.items() if not x.get('passive',False)],key=lambda n:self.tables[n]['motor_index'])
|
||||
self.passive=[n for n in self.tables if n not in self.active]
|
||||
self.values={n:np.array(x['angle_rad'])+x.get('zero_angles',{}).get('urdf_zero_offset_rad',0) for n,x in self.tables.items()}
|
||||
self.valid_commands={}
|
||||
self.bounds={}
|
||||
for n in self.active:
|
||||
motor=self.tables[n]['motor_index']
|
||||
valid=np.ones(256,dtype=bool)
|
||||
for name,x in self.tables.items():
|
||||
if x['motor_index']==motor:
|
||||
low,high=self.urdf_limits[name]
|
||||
valid &= (self.values[name]>=low)&(self.values[name]<=high)
|
||||
commands=np.flatnonzero(valid)
|
||||
assert len(commands)>1 and np.all(np.diff(commands)==1),n
|
||||
self.valid_commands[n]=commands
|
||||
self.bounds[n]=[self.values[n][commands].min(),self.values[n][commands].max()]
|
||||
self.curves={}
|
||||
for n in self.passive:
|
||||
motor=self.tables[n]['motor_index']
|
||||
active=next(a for a in self.active if self.tables[a]['motor_index']==motor)
|
||||
order=np.argsort(self.values[active])
|
||||
x=self.values[active][order]; y=self.values[n][order]
|
||||
x,idx=np.unique(x,return_index=True)
|
||||
self.curves[n]=(active,x,y[idx])
|
||||
|
||||
def passive_value(self,name,q):
|
||||
_,x,y=self.curves[name]
|
||||
i=np.clip(np.searchsorted(x,q,side='right')-1,0,len(x)-2)
|
||||
slope=(y[i+1]-y[i])/(x[i+1]-x[i])
|
||||
return float(np.interp(q,x,y)),float(slope)
|
||||
|
||||
def command(self,q_by_name):
|
||||
cmd=np.array(self.raw['baseline_command_u8'],dtype=np.uint8)
|
||||
for n in self.active:
|
||||
allowed=self.valid_commands[n]
|
||||
cmd[self.tables[n]['motor_index']]=allowed[np.argmin(np.abs(self.values[n][allowed]-q_by_name[n]))]
|
||||
return cmd
|
||||
|
||||
def decode(self,commands,joint_names):
|
||||
return np.array([self.values[n][commands[self.tables[n]['motor_index']]] for n in joint_names])
|
||||
|
||||
class CalibratedAdaptor(MimicJointKinematicAdaptor):
|
||||
def __init__(self,robot,calibration):
|
||||
c=calibration
|
||||
parents=[c.curves[n][0] for n in c.passive]
|
||||
super().__init__(robot,c.active,parents,c.passive,[1.]*len(parents),[0.]*len(parents))
|
||||
self.calibration=c
|
||||
|
||||
def forward_qpos(self,pin_qpos):
|
||||
for i,n in enumerate(self.calibration.passive):
|
||||
value,slope=self.calibration.passive_value(n,pin_qpos[self.idx_pin2source[i]])
|
||||
pin_qpos[self.idx_pin2mimic[i]]=value
|
||||
self.multipliers[i]=slope
|
||||
return pin_qpos
|
||||
|
||||
def build_assets(out):
|
||||
out.mkdir(parents=True,exist_ok=True)
|
||||
fixed=source.build_model('right',out)
|
||||
mj=ET.parse(fixed)
|
||||
# Preserve both the original URDF and JSON. Adapt only generated files.
|
||||
eq=mj.getroot().find('equality')
|
||||
if eq is not None: mj.getroot().remove(eq)
|
||||
mj.write(fixed)
|
||||
tree=ET.parse(ASSET/'linkerhand_g20_right.urdf')
|
||||
root=tree.getroot()
|
||||
for mesh in root.iter('mesh'): mesh.set('filename',str(ASSET/mesh.get('filename')))
|
||||
for j in root.findall('joint'):
|
||||
mimic=j.find('mimic')
|
||||
if mimic is not None:j.remove(mimic)
|
||||
for body in mj.getroot().iter('body'):
|
||||
for site in body.findall('site'):
|
||||
name=site.get('name')
|
||||
ET.SubElement(root,'link',name=name)
|
||||
j=ET.SubElement(root,'joint',name=name+'_fixed',type='fixed')
|
||||
ET.SubElement(j,'parent',link=body.get('name'));ET.SubElement(j,'child',link=name)
|
||||
ET.SubElement(j,'origin',xyz=site.get('pos'),rpy='0 0 0')
|
||||
path=out/'l20_dex.urdf';tree.write(path)
|
||||
return fixed,path
|
||||
@@ -0,0 +1,11 @@
|
||||
"""Local correction for dex 0.5.0 PositionOptimizer's inconsistent loss/gradient."""
|
||||
import numpy as np
|
||||
from dex_retargeting.optimizer import PositionOptimizer
|
||||
|
||||
class ConsistentPositionOptimizer(PositionOptimizer):
|
||||
def get_objective_function(self,target_pos,fixed_qpos,last_qpos):
|
||||
original=super().get_objective_function(target_pos,fixed_qpos,last_qpos)
|
||||
def objective(x,grad):
|
||||
# Upstream already includes this term's gradient, but omits its value.
|
||||
return original(x,grad)+self.norm_delta*float(np.sum((x-last_qpos)**2))
|
||||
return objective
|
||||
@@ -0,0 +1,176 @@
|
||||
"""Contact-aware L20 pose refinement with a required, explicit object specification.
|
||||
|
||||
Optimizes calibrated finger coordinates and all six wrist correction coordinates.
|
||||
The --fixture path is an analytic sphere test, NOT the object in the source video.
|
||||
"""
|
||||
import os
|
||||
os.environ.setdefault('MUJOCO_GL','osmesa')
|
||||
import argparse,json
|
||||
from pathlib import Path
|
||||
import xml.etree.ElementTree as ET
|
||||
import copy
|
||||
import numpy as np
|
||||
import mujoco
|
||||
from scipy.optimize import least_squares
|
||||
from scipy.spatial.transform import Rotation
|
||||
from l20_calibrated import ROOT,Calibration,FINGERS,TIPS
|
||||
|
||||
OUT=ROOT/'output/l20_2047635068'
|
||||
|
||||
def scene_for_object(spec,out):
|
||||
tree=ET.parse(OUT/'l20_moving.xml');root=tree.getroot()
|
||||
root.find('option').set('gravity','0 0 -9.81')
|
||||
root.find('option').set('timestep','0.001')
|
||||
root.find('option').set('cone','elliptic')
|
||||
# Resolve all mesh references relative to the existing scene.
|
||||
for mesh in root.findall('asset/mesh'):
|
||||
mesh.set('file',str((OUT/mesh.get('file')).resolve()))
|
||||
hand=root.find("worldbody/body[@name='hand_base_link']")
|
||||
for i,geom in enumerate(hand.iter('geom')):
|
||||
geom.set('name',f'hand_collision_{i}')
|
||||
geom.set('contype','1');geom.set('conaffinity','2')
|
||||
geom.set('friction',f"{spec.get('friction',.8)} .005 .0001")
|
||||
geom.set('condim','4')
|
||||
obj=ET.SubElement(root.find('worldbody'),'body',name='grasp_object',pos='0 0 0')
|
||||
ET.SubElement(obj,'freejoint',name='object_free')
|
||||
if spec['type']=='mjcf':
|
||||
source=Path(spec['mjcf_path']).resolve();bottle=ET.parse(source).getroot()
|
||||
for asset in bottle.find('asset'):
|
||||
item=copy.deepcopy(asset)
|
||||
if item.get('file'):item.set('file',str((source.parent/item.get('file')).resolve()))
|
||||
root.find('asset').append(item)
|
||||
for item in bottle.find('worldbody/body'):
|
||||
if item.tag=='freejoint':continue
|
||||
child=copy.deepcopy(item)
|
||||
if child.tag=='geom':
|
||||
child.set('name','bottle_part_'+str(len(obj)))
|
||||
if child.get('contype','1')!='0':
|
||||
child.set('contype','2');child.set('conaffinity','1')
|
||||
child.set('friction',f"{spec.get('friction',.6)} .005 .001")
|
||||
child.set('solref','.004 1');child.set('solimp','.95 .99 .001')
|
||||
obj.append(child)
|
||||
path=out/'contact_scene.xml';tree.write(path)
|
||||
return path
|
||||
attrs=dict(name='object_geom',type=spec['type'],mass=str(spec.get('mass_kg',.1)),
|
||||
contype='2',conaffinity='1',friction=f"{spec.get('friction',.8)} .005 .0001",
|
||||
condim='4',rgba='.9 .65 .2 1',solref='.005 1')
|
||||
if spec['type']=='mesh':
|
||||
mesh_path=Path(spec['mesh_path']).resolve()
|
||||
ET.SubElement(root.find('asset'),'mesh',name='object_mesh',file=str(mesh_path),
|
||||
scale=' '.join(map(str,spec.get('mesh_scale',[1,1,1]))))
|
||||
attrs['mesh']='object_mesh'
|
||||
else:attrs['size']=' '.join(map(str,spec['size']))
|
||||
ET.SubElement(obj,'geom',**attrs)
|
||||
path=out/'contact_scene.xml';tree.write(path)
|
||||
return path
|
||||
|
||||
class ContactRefiner:
|
||||
def __init__(self,spec,out):
|
||||
self.spec=spec;self.cal=Calibration()
|
||||
self.model=mujoco.MjModel.from_xml_path(str(scene_for_object(spec,out)))
|
||||
self.data=mujoco.MjData(self.model)
|
||||
self.names=[n for n in self.cal.tables]
|
||||
self.addr={n:self.model.jnt_qposadr[mujoco.mj_name2id(self.model,mujoco.mjtObj.mjOBJ_JOINT,n)] for n in self.names}
|
||||
self.wrist=self.model.jnt_qposadr[mujoco.mj_name2id(self.model,mujoco.mjtObj.mjOBJ_JOINT,'wrist_free')]
|
||||
self.obj=self.model.jnt_qposadr[mujoco.mj_name2id(self.model,mujoco.mjtObj.mjOBJ_JOINT,'object_free')]
|
||||
self.object_geoms=np.flatnonzero(self.model.geom_contype==2)
|
||||
self.object_geom=int(self.object_geoms[0])
|
||||
self.hand_geoms=np.flatnonzero(self.model.geom_contype==1)
|
||||
self.tip_geoms=[]
|
||||
contact_links=spec.get('contact_links',[f+'_distal' for f in spec.get('contact_fingers',['thumb','index','middle'])])
|
||||
for link in contact_links:
|
||||
bid=mujoco.mj_name2id(self.model,mujoco.mjtObj.mjOBJ_BODY,link)
|
||||
self.tip_geoms.append(next(g for g in self.hand_geoms if self.model.geom_bodyid[g]==bid))
|
||||
self.lower=np.array([self.cal.bounds[n][0] for n in self.cal.active]+[-.06]*3+[-.5]*3)
|
||||
self.upper=np.array([self.cal.bounds[n][1] for n in self.cal.active]+[.06]*3+[.5]*3)
|
||||
|
||||
def expand(self,active):
|
||||
q=dict(zip(self.cal.active,active))
|
||||
for n in self.cal.passive:q[n]=self.cal.passive_value(n,q[self.cal.curves[n][0]])[0]
|
||||
return q
|
||||
|
||||
def set_pose(self,x,wrist_pos,wrist_quat,obj_pos,obj_quat):
|
||||
q=self.expand(x[:16])
|
||||
for n,v in q.items():self.data.qpos[self.addr[n]]=v
|
||||
self.data.qpos[self.wrist:self.wrist+3]=wrist_pos+x[16:19]
|
||||
rotation=Rotation.from_quat(wrist_quat[[1,2,3,0]])*Rotation.from_rotvec(x[19:22])
|
||||
self.data.qpos[self.wrist+3:self.wrist+7]=rotation.as_quat()[[3,0,1,2]]
|
||||
self.data.qpos[self.obj:self.obj+3]=obj_pos
|
||||
self.data.qpos[self.obj+3:self.obj+7]=obj_quat
|
||||
mujoco.mj_kinematics(self.model,self.data)
|
||||
|
||||
def distances(self):
|
||||
return np.array([min(mujoco.mj_geomDistance(self.model,self.data,int(g),int(o),.5,None) for o in self.object_geoms) for g in self.hand_geoms])
|
||||
|
||||
def solve(self,reference,wrist_pos,wrist_quat,obj_pos,obj_quat,initial=None):
|
||||
initial=np.r_[reference,np.zeros(6)] if initial is None else np.asarray(initial).copy()
|
||||
tip_index=[list(self.hand_geoms).index(g) for g in self.tip_geoms]
|
||||
def residual(x):
|
||||
self.set_pose(x,wrist_pos,wrist_quat,obj_pos,obj_quat)
|
||||
dist=self.distances()
|
||||
compression=self.spec.get('compression_m',.0002)
|
||||
return np.r_[(dist[tip_index]+compression)/.004,
|
||||
np.maximum(-dist-max(.0003,compression),0)/.0015,
|
||||
.15*(x[:16]-reference),.2*x[16:19]/.03,.2*x[19:22]/.25]
|
||||
before=residual(initial);before_distance=self.distances()
|
||||
result=least_squares(residual,np.clip(initial,self.lower+1e-9,self.upper-1e-9),
|
||||
bounds=(self.lower,self.upper),diff_step=1e-4,max_nfev=100,ftol=1e-5,xtol=1e-5,gtol=1e-5)
|
||||
after=residual(result.x);after_distance=self.distances()
|
||||
report=dict(converged=bool(result.success),nfev=result.nfev,
|
||||
cost_before=float(before@before),cost_after=float(after@after),
|
||||
mean_selected_surface_gap_before_mm=float(np.abs(before_distance[tip_index]).mean()*1000),
|
||||
mean_selected_surface_gap_after_mm=float(np.abs(after_distance[tip_index]).mean()*1000),
|
||||
max_penetration_before_mm=float(max(0,-before_distance.min())*1000),
|
||||
max_penetration_after_mm=float(max(0,-after_distance.min())*1000),
|
||||
wrist_correction_m=result.x[16:19].tolist(),wrist_correction_rotvec=result.x[19:22].tolist())
|
||||
return result.x,self.data.qpos.copy(),report
|
||||
|
||||
def dynamic_hold(self,qpos,seconds=2.):
|
||||
# A diagnostic with prescribed hand coordinates and a freely simulated object.
|
||||
# It does not validate actuator effort, compliant tracking, or force closure.
|
||||
mujoco.mj_resetData(self.model,self.data);self.data.qpos[:]=qpos
|
||||
object_initial=qpos[self.obj:self.obj+3].copy()
|
||||
hand_addr=[self.addr[n] for n in self.names]+list(range(self.wrist,self.wrist+7))
|
||||
object_dof=self.model.jnt_dofadr[mujoco.mj_name2id(self.model,mujoco.mjtObj.mjOBJ_JOINT,'object_free')]
|
||||
controlled_dof=[i for i in range(self.model.nv) if not object_dof<=i<object_dof+6]
|
||||
frames=[];contacts=[]
|
||||
steps=int(seconds/self.model.opt.timestep)
|
||||
for k in range(steps):
|
||||
self.data.qpos[hand_addr]=qpos[hand_addr];self.data.qvel[controlled_dof]=0
|
||||
mujoco.mj_step(self.model,self.data)
|
||||
if not np.isfinite(self.data.qpos).all():raise RuntimeError('Non-finite physics state')
|
||||
if k%33==0:
|
||||
frames.append(self.data.qpos.copy())
|
||||
contacts.append(sum(any(g in self.object_geoms for g in c.geom) for c in self.data.contact))
|
||||
displacement=float(np.linalg.norm(self.data.qpos[self.obj:self.obj+3]-object_initial))
|
||||
return np.array(frames),dict(seconds=seconds,object_displacement_m=displacement,
|
||||
contact_frames=int(np.sum(np.array(contacts)>0)),total_frames=len(frames),
|
||||
retained_within_2cm=displacement<.02,
|
||||
boundary='Prescribed hand, free object under gravity. No object weld or pose resets. Hand actuator/arm dynamics not validated.')
|
||||
|
||||
def main():
|
||||
parser=argparse.ArgumentParser();parser.add_argument('--spec',type=Path);parser.add_argument('--fixture',action='store_true')
|
||||
parser.add_argument('--frame',type=int,default=666);args=parser.parse_args()
|
||||
motion=np.load(OUT/'motion.npz');t=args.frame
|
||||
if not args.fixture and args.spec is None:parser.error('Provide an object specification, or explicitly select the synthetic --fixture test.')
|
||||
out=OUT/('contact_fixture' if args.fixture else 'contact_object');out.mkdir(exist_ok=True)
|
||||
if args.fixture:
|
||||
pts=motion['actual'][t]
|
||||
center=(pts[4]+pts[12])*.5
|
||||
radius=np.linalg.norm(pts[4]-pts[12])*.42
|
||||
r=Rotation.from_quat(motion['wrist_quat_wxyz'][t,[1,2,3,0]]).as_matrix()
|
||||
spec=dict(type='sphere',size=[float(radius)],mass_kg=.05,friction=.8,
|
||||
position=(r@center+motion['wrist_pos'][t]).tolist(),quat_wxyz=[1,0,0,0],
|
||||
contact_fingers=['thumb','index','middle'],purpose='Synthetic interface/physics fixture; not source-video reconstruction')
|
||||
else:spec=json.loads(args.spec.read_text())
|
||||
(out/'object_spec.json').write_text(json.dumps(spec,indent=2))
|
||||
solver=ContactRefiner(spec,out)
|
||||
active=np.array([motion['qpos'][t,list(motion['joint_names']).index(n)] for n in solver.cal.active])
|
||||
x,q,report=solver.solve(active,motion['wrist_pos'][t],motion['wrist_quat_wxyz'][t],np.array(spec['position']),np.array(spec['quat_wxyz']))
|
||||
path,dynamic=solver.dynamic_hold(q)
|
||||
report['dynamic_hold']=dynamic;report['frame']=t;report['fixture']=args.fixture
|
||||
np.savez_compressed(out/'contact_result.npz',optimization_variables=x,qpos=q,dynamic_qpos=path)
|
||||
(out/'validation.json').write_text(json.dumps(report,indent=2))
|
||||
print(json.dumps(report,indent=2),flush=True)
|
||||
|
||||
if __name__=='__main__':main()
|
||||
@@ -0,0 +1,43 @@
|
||||
"""Isolate contact geometry using prescribed link poses and a free bottle."""
|
||||
from pathlib import Path
|
||||
import xml.etree.ElementTree as E
|
||||
import copy,json
|
||||
import numpy as np
|
||||
import mujoco
|
||||
|
||||
p=Path(__file__).resolve().parents[1]/'output/l20_2047635068/bottle_grasp'
|
||||
tree=E.parse(p/'physics_scene.xml');root=tree.getroot();world=root.find('worldbody')
|
||||
hand=world.find("body[@name='hand_base_link']")
|
||||
original=mujoco.MjModel.from_xml_path(str(p/'physics_scene.xml'));d=mujoco.MjData(original)
|
||||
a=np.load(p/'physics_motion.npz');d.qpos[:]=a['qpos'][0]
|
||||
d.mocap_pos[0]=a['wrist'][0,:3];d.mocap_quat[0]=a['wrist'][0,3:];mujoco.mj_forward(original,d)
|
||||
poses=[]
|
||||
for b in hand.iter('body'):
|
||||
bid=original.body(b.get('name')).id
|
||||
new=E.SubElement(world,'body',name=b.get('name'),mocap='true')
|
||||
poses.append((d.xpos[bid].copy(),d.xquat[bid].copy()))
|
||||
for g in b.findall('geom'):new.append(copy.deepcopy(g))
|
||||
world.remove(hand)
|
||||
for tag in ['equality','actuator']:
|
||||
e=root.find(tag)
|
||||
if e is not None:root.remove(e)
|
||||
path=p/'kinematic_grasp_scene.xml';tree.write(path)
|
||||
m=mujoco.MjModel.from_xml_path(str(path));s=mujoco.MjData(m)
|
||||
oi=original.jnt_qposadr[original.joint('object_free').id]
|
||||
s.qpos[:]=d.qpos[oi:oi+7]
|
||||
pos=np.array([x[0] for x in poses]);quat=np.array([x[1] for x in poses])
|
||||
s.mocap_pos[:]=pos;s.mocap_quat[:]=quat
|
||||
start=s.qpos[:3].copy();rows=[];mp=[];counts=[];drift=[]
|
||||
for k in range(3000):
|
||||
t=k*.001;f=np.clip(t-.5,0,1);lift=.1*f*f*(3-2*f)
|
||||
s.mocap_pos[:]=pos+[0,0,lift];mujoco.mj_step(m,s)
|
||||
if k%33==0:
|
||||
rows.append(s.qpos.copy());mp.append(s.mocap_pos.copy());counts.append(s.ncon)
|
||||
drift.append(np.linalg.norm(s.qpos[:3]-start-[0,0,lift]))
|
||||
np.savez_compressed(p/'kinematic_grasp_motion.npz',qpos=rows,mocap_pos=mp,mocap_quat=quat,fps=1/.033)
|
||||
r={'free_object':True,'hand_prescribed_kinematics':True,
|
||||
'final_object_displacement':(s.qpos[:3]-start).tolist(),'expected_lift_m':.1,
|
||||
'relative_drift_m':float(np.linalg.norm(s.qpos[:3]-start-[0,0,.1])),
|
||||
'maximum_drift_m':float(max(drift)),'retained':bool(max(drift)<.02),
|
||||
'contact_frames':int(np.sum(np.array(counts)>0))}
|
||||
(p/'kinematic_grasp_validation.json').write_text(json.dumps(r,indent=2));print(json.dumps(r,indent=2))
|
||||
@@ -0,0 +1,276 @@
|
||||
"""Build an L20 visual MJCF and provide coupled 21-landmark kinematics.
|
||||
|
||||
The supplied L20 URDFs have 21 revolute joints and five passive mimic joints.
|
||||
The accompanying G20 calibration also has only 16 active motor channels; its
|
||||
20-byte command slots 11--14 are unassigned. This module preserves the URDF
|
||||
linear mimic model. Calibration tables are metadata, not a hardware command
|
||||
conversion: their nonlinear passive curves differ from the URDF mimic ratios.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from collections import defaultdict
|
||||
import json
|
||||
import os
|
||||
from pathlib import Path
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
import mujoco
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
|
||||
REPO_ROOT = Path(__file__).resolve().parents[1]
|
||||
FINGERS = ("thumb", "index", "middle", "ring", "pinky")
|
||||
LANDMARK_NAMES = (
|
||||
"wrist", "thumb_cmc", "thumb_mcp", "thumb_ip", "thumb_tip",
|
||||
"index_mcp", "index_pip", "index_dip", "index_tip",
|
||||
"middle_mcp", "middle_pip", "middle_dip", "middle_tip",
|
||||
"ring_mcp", "ring_pip", "ring_dip", "ring_tip",
|
||||
"pinky_mcp", "pinky_pip", "pinky_dip", "pinky_tip",
|
||||
)
|
||||
|
||||
|
||||
def source_urdf(side: str = "right") -> Path:
|
||||
side = side.lower()
|
||||
if side not in ("right", "left"):
|
||||
raise ValueError(f"Expected right or left hand, got {side!r}")
|
||||
return REPO_ROOT / "L20" / side.upper() / f"linkerhand_g20_{side}.urdf"
|
||||
|
||||
|
||||
def _fmt(values) -> str:
|
||||
return " ".join(f"{float(value):.12g}" for value in values)
|
||||
|
||||
|
||||
def _origin(element: ET.Element | None) -> dict[str, str]:
|
||||
if element is None:
|
||||
return {"pos": "0 0 0", "quat": "1 0 0 0"}
|
||||
rpy = np.fromstring(element.get("rpy", "0 0 0"), sep=" ")
|
||||
quat = Rotation.from_euler("xyz", rpy).as_quat()
|
||||
return {"pos": element.get("xyz", "0 0 0"), "quat": _fmt(quat[[3, 0, 1, 2]])}
|
||||
|
||||
|
||||
def _mesh_tip(path: Path) -> np.ndarray:
|
||||
"""Approximate distal surface landmark from the terminal 2 mm STL cap.
|
||||
|
||||
Distal link mesh coordinates have their fingertip in local +Z. Averaging
|
||||
unique cap vertices avoids triangle tessellation multiplicity bias. This
|
||||
is a geometric approximation, not a measured tactile contact center.
|
||||
"""
|
||||
dtype = np.dtype([("normal", "<f4", (3,)), ("vertices", "<f4", (3, 3)), ("attr", "<u2")])
|
||||
with path.open("rb") as stream:
|
||||
stream.seek(80)
|
||||
count = int(np.fromfile(stream, dtype="<u4", count=1)[0])
|
||||
triangles = np.fromfile(stream, dtype=dtype, count=count)
|
||||
if len(triangles) != count:
|
||||
raise ValueError(f"Invalid binary STL: {path}")
|
||||
vertices = np.unique(triangles["vertices"].reshape(-1, 3), axis=0)
|
||||
return vertices[vertices[:, 2] >= vertices[:, 2].max() - .002].mean(axis=0)
|
||||
|
||||
|
||||
def build_model(side: str = "right", output_dir: str | Path | None = None) -> Path:
|
||||
"""Create a fixed-wrist, visual-only MJCF without modifying source assets.
|
||||
|
||||
Mesh references are relative to the generated XML. The output therefore
|
||||
remains relocatable with its enclosing repository, but is not an asset
|
||||
bundle independent of the repository. All URDF joint limits and all five
|
||||
mimic equalities are preserved. Contacts are disabled for kinematic replay.
|
||||
"""
|
||||
urdf_path = source_urdf(side)
|
||||
side = side.lower()
|
||||
output_dir = Path(output_dir or (REPO_ROOT / "retarget_l20" / "models")).resolve()
|
||||
output_dir.mkdir(parents=True, exist_ok=True)
|
||||
tree = ET.parse(urdf_path).getroot()
|
||||
links = {link.get("name"): link for link in tree.findall("link")}
|
||||
joints = tree.findall("joint")
|
||||
children = defaultdict(list)
|
||||
for joint in joints:
|
||||
children[joint.find("parent").get("link")].append(joint)
|
||||
|
||||
root = ET.Element("mujoco", model=f"l20_{side}_retarget")
|
||||
root.append(ET.Comment("Kinematic preview; meshes and joint limits from the original L20 URDF."))
|
||||
ET.SubElement(root, "compiler", angle="radian", balanceinertia="true", autolimits="true", strippath="false")
|
||||
ET.SubElement(root, "option", timestep="0.002", gravity="0 0 0")
|
||||
ET.SubElement(root, "statistic", center="0 0 0.135", extent="0.32")
|
||||
visual = ET.SubElement(root, "visual")
|
||||
ET.SubElement(visual, "global", offwidth="1600", offheight="1200", azimuth="20", elevation="-8")
|
||||
ET.SubElement(visual, "quality", shadowsize="2048", offsamples="4")
|
||||
ET.SubElement(visual, "headlight", ambient="0.22 0.22 0.26", diffuse="0.35 0.35 0.35", specular="0.15 0.15 0.15")
|
||||
ET.SubElement(visual, "rgba", haze="0.08 0.10 0.14 1")
|
||||
default = ET.SubElement(root, "default")
|
||||
ET.SubElement(default, "joint", damping="0.1", armature="0.00001")
|
||||
ET.SubElement(default, "geom", contype="0", conaffinity="0", group="1")
|
||||
ET.SubElement(default, "site", size="0.0018", rgba="0.1 0.9 0.8 0", group="3")
|
||||
asset = ET.SubElement(root, "asset")
|
||||
ET.SubElement(asset, "material", name="shell", rgba="0.76 0.82 0.90 1", specular="0.4", shininess="0.5")
|
||||
ET.SubElement(asset, "material", name="joints", rgba="0.15 0.24 0.34 1", specular="0.35", shininess="0.4")
|
||||
ET.SubElement(asset, "material", name="tips", rgba="0.10 0.68 0.72 1", specular="0.3", shininess="0.3")
|
||||
ET.SubElement(asset, "material", name="palm", rgba="0.36 0.44 0.55 1", specular="0.45", shininess="0.5")
|
||||
for name, link in links.items():
|
||||
for index, visual_link in enumerate(link.findall("visual")):
|
||||
mesh = visual_link.find("geometry/mesh")
|
||||
if mesh is None:
|
||||
raise ValueError(f"Expected visual mesh for {name}")
|
||||
path = (urdf_path.parent / mesh.get("filename")).resolve()
|
||||
attributes = {"name": f"{name}_mesh_{index}", "file": os.path.relpath(path, output_dir)}
|
||||
if mesh.get("scale"):
|
||||
attributes["scale"] = mesh.get("scale")
|
||||
ET.SubElement(asset, "mesh", **attributes)
|
||||
|
||||
world = ET.SubElement(root, "worldbody")
|
||||
ET.SubElement(world, "light", name="key", pos="0.4 0.25 0.5", dir="-0.7 -0.3 -0.65", diffuse="0.55 0.6 0.65", castshadow="true")
|
||||
ET.SubElement(world, "light", name="fill", pos="-0.3 -0.25 0.3", dir="0.8 0.4 -0.3", diffuse="0.25 0.3 0.35", castshadow="false")
|
||||
ET.SubElement(world, "camera", name="palm", pos="0.56 0 0.14", xyaxes="0 1 0 0 0 1", fovy="36")
|
||||
ET.SubElement(world, "geom", name="floor", type="plane", pos="0 0 -0.003", size="0.7 0.7 0.01", rgba="0.07 0.09 0.13 1", group="0")
|
||||
body_elements = {}
|
||||
|
||||
def add_link(parent: ET.Element, name: str, joint: ET.Element | None = None):
|
||||
body = ET.SubElement(parent, "body", name=name, **_origin(None if joint is None else joint.find("origin")))
|
||||
body_elements[name] = body
|
||||
link = links[name]
|
||||
inertial = link.find("inertial")
|
||||
if inertial is not None:
|
||||
inertia = inertial.find("inertia")
|
||||
# MuJoCo fullinertia uses the body-aligned frame. All current L20
|
||||
# inertial origins have zero orientation; rotate defensively.
|
||||
matrix = np.array([[float(inertia.get("ixx")), float(inertia.get("ixy")), float(inertia.get("ixz"))],
|
||||
[float(inertia.get("ixy")), float(inertia.get("iyy")), float(inertia.get("iyz"))],
|
||||
[float(inertia.get("ixz")), float(inertia.get("iyz")), float(inertia.get("izz"))]])
|
||||
attributes = _origin(inertial.find("origin"))
|
||||
quat = np.fromstring(attributes.pop("quat"), sep=" ")
|
||||
rotation = Rotation.from_quat(quat[[1, 2, 3, 0]]).as_matrix()
|
||||
matrix = rotation @ matrix @ rotation.T
|
||||
ET.SubElement(body, "inertial", **attributes, mass=inertial.find("mass").get("value"),
|
||||
fullinertia=_fmt([matrix[0, 0], matrix[1, 1], matrix[2, 2], matrix[0, 1], matrix[0, 2], matrix[1, 2]]))
|
||||
if joint is not None and joint.get("type") != "fixed":
|
||||
if joint.get("type") != "revolute":
|
||||
raise ValueError(f"Unsupported joint type {joint.get('type')}")
|
||||
limit = joint.find("limit")
|
||||
ET.SubElement(body, "joint", name=joint.get("name"), type="hinge", axis=joint.find("axis").get("xyz"),
|
||||
range=f"{limit.get('lower')} {limit.get('upper')}")
|
||||
material = "palm" if name == "hand_base_link" else "tips" if name.endswith("distal") else "joints" if "metacarpals" in name else "shell"
|
||||
for index, link_visual in enumerate(link.findall("visual")):
|
||||
ET.SubElement(body, "geom", name=f"{name}_visual_{index}", type="mesh", mesh=f"{name}_mesh_{index}", material=material, **_origin(link_visual.find("origin")))
|
||||
for child_joint in children[name]:
|
||||
add_link(body, child_joint.find("child").get("link"), child_joint)
|
||||
|
||||
add_link(world, "hand_base_link")
|
||||
placements = [("hand_base_link", "0 0 0"), ("thumb_metacarpals", "0 0 0"),
|
||||
("thumb_proximal", "0 0 0"), ("thumb_distal", "0 0 0"),
|
||||
("thumb_distal", _fmt(_mesh_tip(urdf_path.parent / "meshes/thumb_distal.STL")))]
|
||||
for finger in FINGERS[1:]:
|
||||
placements.extend([(f"{finger}_proximal", "0 0 0"), (f"{finger}_middle", "0 0 0"),
|
||||
(f"{finger}_distal", "0 0 0"), (f"{finger}_distal", _fmt(_mesh_tip(urdf_path.parent / f"meshes/{finger}_distal.STL")))])
|
||||
for index, (body_name, pos) in enumerate(placements):
|
||||
ET.SubElement(body_elements[body_name], "site", name=f"landmark_{index:02d}", pos=pos)
|
||||
equality = ET.SubElement(root, "equality")
|
||||
for joint in joints:
|
||||
mimic = joint.find("mimic")
|
||||
if mimic is not None:
|
||||
ET.SubElement(equality, "joint", name=f"mimic_{joint.get('name')}", joint1=joint.get("name"), joint2=mimic.get("joint"),
|
||||
polycoef=f"{mimic.get('offset', '0')} {mimic.get('multiplier', '1')} 0 0 0")
|
||||
ET.indent(root, space=" ")
|
||||
output_path = output_dir / f"l20_{side}.xml"
|
||||
ET.ElementTree(root).write(output_path, encoding="utf-8", xml_declaration=True)
|
||||
return output_path
|
||||
|
||||
|
||||
class HandKinematics:
|
||||
"""FK in native URDF coordinates; optimization variables are in radians.
|
||||
|
||||
+Z points from wrist to extended fingers. +X is palm-facing and positive
|
||||
flexion brings fingers toward +X. Radial direction (pinky toward index) is
|
||||
+Y for the right hand and -Y for the left. Wrist pose is fixed at identity.
|
||||
|
||||
`landmarks` and `jacobian` accept independent coordinates (16). `landmarks`
|
||||
additionally accepts full URDF coordinates (21) for playback. `expand`
|
||||
enforces exact linear mimic coupling. Returned arrays are fresh copies.
|
||||
"""
|
||||
|
||||
def __init__(self, model_path: str | Path, side: str = "right"):
|
||||
self.side = side.lower()
|
||||
self.model_path = Path(model_path).resolve()
|
||||
self.model = mujoco.MjModel.from_xml_path(str(self.model_path))
|
||||
self.data = mujoco.MjData(self.model)
|
||||
urdf = ET.parse(source_urdf(side)).getroot()
|
||||
xml_joints = [j for j in urdf.findall("joint") if j.get("type") != "fixed"]
|
||||
by_name = {joint.get("name"): joint for joint in xml_joints}
|
||||
self.joint_names = [mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_JOINT, j) for j in range(self.model.njnt)]
|
||||
self.independent_joint_names = [name for name in self.joint_names if by_name[name].find("mimic") is None]
|
||||
self.independent_indices = np.array([self.joint_names.index(name) for name in self.independent_joint_names], dtype=int)
|
||||
self.expansion = np.zeros((len(self.joint_names), len(self.independent_joint_names)))
|
||||
self.offset = np.zeros(len(self.joint_names))
|
||||
self.mimic = {}
|
||||
for index, name in enumerate(self.joint_names):
|
||||
mimic = by_name[name].find("mimic")
|
||||
if mimic is None:
|
||||
self.expansion[index, self.independent_joint_names.index(name)] = 1.
|
||||
else:
|
||||
parent = mimic.get("joint")
|
||||
multiplier = float(mimic.get("multiplier", "1"))
|
||||
offset = float(mimic.get("offset", "0"))
|
||||
if parent not in self.independent_joint_names:
|
||||
raise ValueError("Nested mimic coupling is not supported")
|
||||
self.expansion[index, self.independent_joint_names.index(parent)] = multiplier
|
||||
self.offset[index] = offset
|
||||
self.mimic[name] = {"joint": parent, "multiplier": multiplier, "offset": offset}
|
||||
self.full_lower = self.model.jnt_range[:, 0].copy()
|
||||
self.full_upper = self.model.jnt_range[:, 1].copy()
|
||||
self.lower = self.full_lower[self.independent_indices].copy()
|
||||
self.upper = self.full_upper[self.independent_indices].copy()
|
||||
# Shrink active limits as needed so passive limits are respected too.
|
||||
for index, name in enumerate(self.joint_names):
|
||||
if name in self.mimic:
|
||||
column = np.flatnonzero(self.expansion[index])[0]
|
||||
limits = (np.array([self.full_lower[index], self.full_upper[index]]) - self.offset[index]) / self.expansion[index, column]
|
||||
self.lower[column] = max(self.lower[column], min(limits))
|
||||
self.upper[column] = min(self.upper[column], max(limits))
|
||||
self.site_ids = np.array([mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SITE, f"landmark_{i:02d}") for i in range(21)])
|
||||
if np.any(self.site_ids < 0):
|
||||
raise ValueError("MJCF does not contain all 21 hand landmark sites")
|
||||
self.calibration = json.loads((source_urdf(side).parent / f"g20_{self.side}_G20.json").read_text())
|
||||
self.motor_indices = np.array([self.calibration["joints"][name]["motor_index"] for name in self.independent_joint_names], dtype=int)
|
||||
self.palm_basis = np.array([[0., 0., 1.], [1. if self.side == "right" else -1., 0., 0.], [0., 1., 0.]])
|
||||
self.neutral_landmarks = self.landmarks(np.zeros(len(self.independent_joint_names)))
|
||||
|
||||
def expand(self, q: np.ndarray) -> np.ndarray:
|
||||
q = np.asarray(q, dtype=float)
|
||||
if q.shape[-1] != len(self.independent_joint_names):
|
||||
raise ValueError(f"Expected {len(self.independent_joint_names)} independent coordinates, got {q.shape}")
|
||||
return q @ self.expansion.T + self.offset
|
||||
|
||||
def landmarks(self, q: np.ndarray) -> np.ndarray:
|
||||
q = np.asarray(q, dtype=float)
|
||||
if q.shape == (len(self.independent_joint_names),):
|
||||
q = self.expand(q)
|
||||
if q.shape != (self.model.nq,):
|
||||
raise ValueError(f"Expected independent or full hand qpos, got {q.shape}")
|
||||
self.data.qpos[:] = q
|
||||
mujoco.mj_kinematics(self.model, self.data)
|
||||
return self.data.site_xpos[self.site_ids].copy()
|
||||
|
||||
def jacobian(self, q: np.ndarray) -> np.ndarray:
|
||||
self.landmarks(q)
|
||||
mujoco.mj_comPos(self.model, self.data)
|
||||
jacobian = np.empty((21, 3, len(self.independent_joint_names)))
|
||||
jacp = np.zeros((3, self.model.nv))
|
||||
for index, site_id in enumerate(self.site_ids):
|
||||
mujoco.mj_jacSite(self.model, self.data, jacp, None, int(site_id))
|
||||
jacobian[index] = jacp @ self.expansion
|
||||
return jacobian
|
||||
|
||||
def metadata(self) -> dict:
|
||||
return {
|
||||
"source_urdf": str(source_urdf(self.side).relative_to(REPO_ROOT)),
|
||||
"side": self.side,
|
||||
"joint_names": self.joint_names,
|
||||
"independent_joint_names": self.independent_joint_names,
|
||||
"motor_indices": self.motor_indices.tolist(),
|
||||
"unused_command_indices": [11, 12, 13, 14],
|
||||
"independent_limits_rad": np.column_stack([self.lower, self.upper]).tolist(),
|
||||
"mimic": self.mimic,
|
||||
"landmark_names": list(LANDMARK_NAMES),
|
||||
"landmark_definition": "Joint origins; wrist at base origin; thumb CMC at thumb_metacarpals origin; fingertips approximate distal STL terminal-cap centers.",
|
||||
"model_note": "URDF kinematics and linear mimic coupling; calibration nonlinear passive tables differ. No hardware-equivalence claim or motor command conversion.",
|
||||
"contact_note": "Collision geoms and contact disabled for kinematic visual replay; no object interaction or collision avoidance.",
|
||||
}
|
||||
@@ -0,0 +1,151 @@
|
||||
"""Build a source-only, history-free snapshot for an internal Git platform."""
|
||||
import argparse
|
||||
import hashlib
|
||||
import json
|
||||
from pathlib import Path
|
||||
import re
|
||||
import shutil
|
||||
import subprocess
|
||||
import tarfile
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
EXTENSIONS = {'.py','.pyi','.sh','.bash','.md','.rst','.txt','.json','.yaml','.yml',
|
||||
'.toml','.cfg','.ini','.xml','.urdf','.cpp','.cc','.c','.h','.hpp','.cu',
|
||||
'.cuh','.cmake','.in','.lock','.gitignore','.gitmodules','.bib','.css',
|
||||
'.html','.js','.ipynb','.pxd','.pyx','.bat','.ps1','.m','.glsl','.vert','.frag'}
|
||||
SPECIAL = {'LICENSE','LICENCE','COPYING','NOTICE','AUTHORS','Makefile','CMakeLists.txt',
|
||||
'Dockerfile','.gitignore','.gitattributes','environment.yml'}
|
||||
EXCLUDE_PARTS = {'.git','__pycache__','.dynhamr','.venv','venv','outputs','output',
|
||||
'_DATA','checkpoints','weights','.pytest_cache','.mypy_cache'}
|
||||
|
||||
|
||||
def git(repo, *args):
|
||||
return subprocess.check_output(['git','-C',str(repo),*args], stderr=subprocess.DEVNULL).decode().strip()
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument('--output', type=Path, default=ROOT/'dist/hand-motion-pipeline')
|
||||
args = parser.parse_args()
|
||||
dest = args.output.resolve()
|
||||
if dest.exists():
|
||||
raise SystemExit(f'Refusing to overwrite existing release: {dest}')
|
||||
dest.mkdir(parents=True)
|
||||
excluded, repos, portability = [], [], []
|
||||
|
||||
def copy_file(src, relative):
|
||||
reason = None
|
||||
if src.is_symlink():
|
||||
reason = 'symlink: restore dependency explicitly'
|
||||
elif any(p in EXCLUDE_PARTS for p in relative.parts):
|
||||
reason = 'runtime/data/cache directory'
|
||||
elif relative.name == '.gitmodules' or (relative.parts[0] == 'third_party' and relative.name in {'.gitignore','.gitattributes'}):
|
||||
reason = 'vendored snapshot: omit nested Git filters and metadata'
|
||||
elif src.suffix not in EXTENSIONS and src.name not in SPECIAL and not src.name.startswith(('LICENSE','COPYING','NOTICE')):
|
||||
reason = 'non-source file; restore separately when needed'
|
||||
elif src.stat().st_size > 5*1024*1024:
|
||||
reason = 'file exceeds 5 MiB source limit'
|
||||
if reason:
|
||||
excluded.append({'path':relative.as_posix(),'reason':reason})
|
||||
return
|
||||
raw = src.read_bytes()
|
||||
if b'\0' in raw:
|
||||
excluded.append({'path':relative.as_posix(),'reason':'binary contents'})
|
||||
return
|
||||
target = dest/relative
|
||||
target.parent.mkdir(parents=True, exist_ok=True)
|
||||
shutil.copy2(src,target)
|
||||
if src.suffix in {'.py','.sh','.yaml','.yml','.json','.toml'}:
|
||||
for i,line in enumerate(raw.decode('utf-8',errors='replace').splitlines(),1):
|
||||
if re.search(r'/home/|/tmp/|2047635068|15886123',line):
|
||||
portability.append({'path':relative.as_posix(),'line':i,
|
||||
'kind':'machine path or fixed example identifier'})
|
||||
|
||||
def snapshot(repo, relative, ancestors=()):
|
||||
real = repo.resolve()
|
||||
if real in ancestors:
|
||||
raise RuntimeError(f'Recursive repository: {repo}')
|
||||
records = subprocess.check_output(['git','-C',str(repo),'ls-files','--stage','-z']).split(b'\0')
|
||||
repos.append({'path':relative.as_posix(),'commit':git(repo,'rev-parse','HEAD'),
|
||||
'origin':git(repo,'remote','get-url','origin'),
|
||||
'local_changes':git(repo,'status','--porcelain'),
|
||||
'snapshot':'working tree content; tracked files plus source additions'})
|
||||
visited = set()
|
||||
for record in records:
|
||||
if not record:
|
||||
continue
|
||||
metadata,name = record.decode().split('\t',1)
|
||||
visited.add(name)
|
||||
src = repo/name
|
||||
if metadata.startswith('160000'):
|
||||
if src.exists() and (src/'.git').exists():
|
||||
snapshot(src,relative/name,(*ancestors,real))
|
||||
else:
|
||||
excluded.append({'path':(relative/name).as_posix(),'reason':'unavailable submodule; restore pinned dependency', 'commit':metadata.split()[1]})
|
||||
elif src.is_file() or src.is_symlink():
|
||||
copy_file(src,relative/name)
|
||||
additions = subprocess.check_output(['git','-C',str(repo),'ls-files','--others','--exclude-standard','-z']).split(b'\0')
|
||||
for item in additions:
|
||||
if item:
|
||||
name=item.decode()
|
||||
if name not in visited and (repo/name).is_file():
|
||||
copy_file(repo/name,relative/name)
|
||||
|
||||
for folder in ['scripts','docs','model','utils','preprocessing','visualization','configs','requirements']:
|
||||
for src in sorted((ROOT/folder).rglob('*')):
|
||||
if src.is_file():
|
||||
copy_file(src,src.relative_to(ROOT))
|
||||
for name in ['README.md','LICENSE','.gitignore','requirements.txt','setup_env.sh','setup_vipe_env.sh']:
|
||||
copy_file(ROOT/name,Path(name))
|
||||
for name in ['hamer','vipe','Dyn-HaMR','spider']:
|
||||
snapshot(ROOT/'third_party'/name,Path('third_party')/name)
|
||||
repos.insert(0,{'path':'.','commit':git(ROOT,'rev-parse','HEAD'),
|
||||
'origin':git(ROOT,'remote','get-url','origin'),
|
||||
'snapshot':'selected integration source and documentation from current working tree'})
|
||||
# Broad upstream patterns such as data/ would hide vendored Python source.
|
||||
(dest/'.gitignore').write_text(
|
||||
'# Source snapshot: runtime exclusions anchored to repository root.\n'
|
||||
'/output/\n/results/\n/logs/\n/weights/\n/dist/\n/.venv/\n/venv/\n'
|
||||
'/.dex/\n/.spider/\n/.cuda/\n/.hf_cache/\n/.torch_cache/\n/.spider_cache/\n'
|
||||
'/.uv_cache_spider/\n/.env\n__pycache__/\n*.pyc\n')
|
||||
assets=[]
|
||||
for name in ['l20_assets','bottle_model_parametric','dex-assets']:
|
||||
folder=ROOT/'third_party'/name
|
||||
files=[{'path':p.relative_to(ROOT).as_posix(),'bytes':p.stat().st_size,
|
||||
'sha256':hashlib.sha256(p.read_bytes()).hexdigest()}
|
||||
for p in sorted(folder.rglob('*')) if p.is_file() and not p.is_symlink()]
|
||||
assets.append({'path':folder.relative_to(ROOT).as_posix(),'bundled':False,'files':files})
|
||||
manifests={'UPSTREAM_SOURCES.json':repos,'EXTERNAL_ASSETS.json':assets,
|
||||
'EXCLUDED_FILES.json':excluded,'PORTABILITY_REPORT.json':portability}
|
||||
for name,content in manifests.items():
|
||||
(dest/name).write_text(json.dumps(content,indent=2,ensure_ascii=False)+'\n')
|
||||
# Validate local Python syntax without importing scripts (some execute on import).
|
||||
checked=[]
|
||||
for folder in ['scripts','model','utils','preprocessing','visualization']:
|
||||
for p in (dest/folder).rglob('*.py'):
|
||||
compile(p.read_bytes(),str(p),'exec')
|
||||
checked.append(p.relative_to(dest).as_posix())
|
||||
for p in (dest/'scripts').glob('*.sh'):
|
||||
subprocess.run(['bash','-n',str(p)],check=True)
|
||||
allfiles=[p for p in dest.rglob('*') if p.is_file()]
|
||||
assert not any(p.is_symlink() or '.git' in p.relative_to(dest).parts for p in allfiles)
|
||||
report={'local_python_syntax_checked':len(checked),'shell_syntax':'PASS',
|
||||
'source_files':len(allfiles),'bytes':sum(p.stat().st_size for p in allfiles),
|
||||
'portability_findings':len(portability),'excluded_files':len(excluded),
|
||||
'end_to_end_reconstruction_rerun':False,'remote_upload_performed':False}
|
||||
(dest/'RELEASE_VALIDATION.json').write_text(json.dumps(report,indent=2)+'\n')
|
||||
hashes=[f'{hashlib.sha256(p.read_bytes()).hexdigest()} {p.relative_to(dest).as_posix()}'
|
||||
for p in sorted(dest.rglob('*')) if p.is_file()]
|
||||
(dest/'SHA256SUMS').write_text('\n'.join(hashes)+'\n')
|
||||
archive=dest.with_suffix('.tar.gz')
|
||||
with tarfile.open(archive,'w:gz') as tar:
|
||||
tar.add(dest,arcname=dest.name)
|
||||
with tarfile.open(archive) as tar:
|
||||
members=tar.getmembers()
|
||||
assert all(not m.issym() and not m.islnk() and '..' not in Path(m.name).parts for m in members)
|
||||
print(json.dumps({**report,'directory':str(dest),'archive':str(archive),
|
||||
'archive_bytes':archive.stat().st_size},indent=2))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,58 @@
|
||||
"""Interactive MuJoCo full replay, or export the exact same poses as video."""
|
||||
import argparse,os,time,json
|
||||
from pathlib import Path
|
||||
p=argparse.ArgumentParser(description=__doc__)
|
||||
p.add_argument('--directory',type=Path,default=Path(__file__).resolve().parents[1]/'output/l20_full_replay')
|
||||
p.add_argument('--start',type=int,default=0);p.add_argument('--render',action='store_true');p.add_argument('--frames',type=int,default=0);p.add_argument('--check',action='store_true')
|
||||
a=p.parse_args()
|
||||
if a.render or a.check:os.environ.setdefault('MUJOCO_GL','osmesa')
|
||||
import numpy as np
|
||||
import mujoco
|
||||
motion=np.load(a.directory/'motion.npz');q=motion['qpos'];fps=float(motion['fps']);N=len(q)
|
||||
m=mujoco.MjModel.from_xml_path(str(a.directory/'scene.xml'));d=mujoco.MjData(m)
|
||||
option=mujoco.MjvOption();option.sitegroup[:]=0;option.geomgroup[3:]=0
|
||||
cam=mujoco.MjvCamera();cam.lookat[:]=motion['camera_center'];cam.distance=1.05;cam.azimuth=135;cam.elevation=-23
|
||||
if a.check:
|
||||
for row in q:d.qpos[:]=row;mujoco.mj_forward(m,d)
|
||||
print(json.dumps(dict(frames=N,nq=m.nq,actuators=m.nu,finite=bool(np.isfinite(q).all()),mode='kinematic')))
|
||||
elif a.render:
|
||||
import imageio.v2 as imageio
|
||||
from PIL import Image,ImageDraw,ImageFont
|
||||
total=min(a.frames or N,N-a.start)
|
||||
renderer=mujoco.Renderer(m,height=720,width=640)
|
||||
close=mujoco.MjvCamera();close.distance=.62;close.azimuth=35;close.elevation=-12
|
||||
font=ImageFont.truetype('/usr/share/fonts/truetype/dejavu/DejaVuSans.ttf',16)
|
||||
path=a.directory/('preview.mp4' if a.frames else 'full_replay.mp4')
|
||||
writer=imageio.get_writer(path,fps=fps,codec='libx264',quality=8,macro_block_size=1)
|
||||
try:
|
||||
for t in range(a.start,a.start+total):
|
||||
d.qpos[:]=q[t];mujoco.mj_forward(m,d)
|
||||
close.lookat[:]=(d.xpos[m.body('hand_base_link').id]+d.xpos[m.body('right_object').id])/2;close.lookat[2]+=.06
|
||||
panels=[]
|
||||
for c in [cam,close]:renderer.update_scene(d,camera=c,scene_option=option);panels.append(renderer.render().copy())
|
||||
img=Image.fromarray(np.concatenate(panels,1));draw=ImageDraw.Draw(img)
|
||||
draw.rectangle((0,0,1280,64),fill=(28,38,49))
|
||||
draw.text((18,9),f'L20 + bottle | full video motion | {t/fps:05.2f} / {N/fps:.2f} s',font=font,fill='white')
|
||||
draw.text((18,36),'World view - moving wrist',font=font,fill=(195,220,235));draw.text((660,36),'Grasp detail - linear URDF coupling',font=font,fill=(195,220,235))
|
||||
draw.rectangle((0,690,1280,720),fill=(28,38,49));draw.text((18,696),'Kinematic contact replay | estimated bottle trajectory | floor + ambient lighting',font=font,fill=(210,220,230))
|
||||
writer.append_data(np.asarray(img))
|
||||
if t in [a.start,0,160,600,N//2,N-1]:img.save(a.directory/f'preview_{t:04d}.png')
|
||||
if t%100==0:print(f'Render {t}/{total}',flush=True)
|
||||
finally:writer.close();renderer.close()
|
||||
print('RENDER_COMPLETE',path,flush=True)
|
||||
else:
|
||||
import mujoco.viewer
|
||||
state={'paused':False,'frame':0.,'last':time.perf_counter()}
|
||||
def key(k):
|
||||
if k==32:state['paused']=not state['paused']
|
||||
elif k in [82,114]:state['frame']=0.
|
||||
elif k==262:state['frame']=(int(state['frame'])+1)%N;state['paused']=True
|
||||
elif k==263:state['frame']=(int(state['frame'])-1)%N;state['paused']=True
|
||||
with mujoco.viewer.launch_passive(m,d,key_callback=key) as viewer:
|
||||
viewer.cam.lookat[:]=cam.lookat;viewer.cam.distance=cam.distance;viewer.cam.azimuth=cam.azimuth;viewer.cam.elevation=cam.elevation
|
||||
viewer.opt.sitegroup[:]=0;viewer.opt.geomgroup[3:]=0
|
||||
while viewer.is_running():
|
||||
now=time.perf_counter();elapsed=now-state['last'];state['last']=now
|
||||
if not state['paused']:state['frame']=(state['frame']+elapsed*fps)%N
|
||||
with viewer.lock():d.qpos[:]=q[int(state['frame'])];d.time=int(state['frame'])/fps;mujoco.mj_forward(m,d)
|
||||
viewer.sync();time.sleep(.005)
|
||||
@@ -0,0 +1,51 @@
|
||||
"""Play a hand-only L20 motion file with a free wrist in MuJoCo."""
|
||||
import argparse
|
||||
import time
|
||||
import mujoco
|
||||
import mujoco.viewer
|
||||
import numpy as np
|
||||
|
||||
p = argparse.ArgumentParser()
|
||||
p.add_argument('--directory', required=True)
|
||||
a = p.parse_args()
|
||||
from pathlib import Path
|
||||
directory = Path(a.directory)
|
||||
with np.load(directory / 'motion.npz') as data:
|
||||
motion = {name:data[name] for name in data.files}
|
||||
model = mujoco.MjModel.from_xml_path(str(directory / 'l20_moving.xml'))
|
||||
data = mujoco.MjData(model)
|
||||
addr = [model.jnt_qposadr[model.joint(str(n)).id] for n in motion['joint_names']]
|
||||
wrist = model.jnt_qposadr[model.joint('wrist_free').id]
|
||||
fps = float(motion['fps'])
|
||||
count = len(motion['qpos'])
|
||||
state = dict(frame=0., paused=False)
|
||||
|
||||
def key(k):
|
||||
if k == 32:
|
||||
state['paused'] = not state['paused']
|
||||
elif k in [82,114]:
|
||||
state['frame'] = 0.
|
||||
elif k in [262,263]:
|
||||
state['frame'] = (int(state['frame']) + (1 if k == 262 else -1)) % count
|
||||
state['paused'] = True
|
||||
|
||||
with mujoco.viewer.launch_passive(model, data, key_callback=key) as viewer:
|
||||
viewer.cam.lookat[:] = motion['wrist_pos'].mean(0)
|
||||
viewer.cam.distance = .9
|
||||
viewer.cam.azimuth, viewer.cam.elevation = 135, 10
|
||||
viewer.opt.sitegroup[:] = 0
|
||||
last = time.perf_counter()
|
||||
while viewer.is_running():
|
||||
now = time.perf_counter()
|
||||
if not state['paused']:
|
||||
state['frame'] = (state['frame'] + (now-last)*fps) % count
|
||||
last = now
|
||||
t = int(state['frame'])
|
||||
with viewer.lock():
|
||||
data.qpos[addr] = motion['qpos'][t]
|
||||
data.qpos[wrist:wrist+3] = motion['wrist_pos'][t]
|
||||
data.qpos[wrist+3:wrist+7] = motion['wrist_quat_wxyz'][t]
|
||||
data.time = t/fps
|
||||
mujoco.mj_forward(model,data)
|
||||
viewer.sync()
|
||||
time.sleep(.005)
|
||||
@@ -0,0 +1,118 @@
|
||||
"""Prepare an L20 linear-mimic bottle task for the actual SPIDER contact-guided runner."""
|
||||
import os
|
||||
os.environ.setdefault('MUJOCO_GL','osmesa')
|
||||
from pathlib import Path
|
||||
import json,copy,xml.etree.ElementTree as E
|
||||
import numpy as np
|
||||
import mujoco
|
||||
from scipy.spatial.transform import Rotation
|
||||
from scipy.optimize import least_squares
|
||||
import l20_model_source as source
|
||||
ROOT=Path(__file__).resolve().parents[1];OUT=ROOT/'output/spider_l20_contact'
|
||||
TASK=OUT/'datasets/processed/current/l20/right/bottle_hold';TRIAL=TASK/'0';TRIAL.mkdir(parents=True,exist_ok=True)
|
||||
source.REPO_ROOT=ROOT/'third_party/l20_assets'
|
||||
p=source.build_model('right',OUT);tree=E.parse(p);root=tree.getroot();world=root.find('worldbody');hand=world.find("body[@name='hand_base_link']")
|
||||
for mesh in root.findall('asset/mesh'):
|
||||
mesh.set('file',str((OUT/mesh.get('file')).resolve()));mesh.set('maxhullvert','64')
|
||||
root.find('option').attrib.update(timestep='.0025',gravity='0 0 -9.81',integrator='implicitfast',cone='elliptic')
|
||||
world.find("geom[@name='floor']").attrib.update(pos='0 0 0',contype='4',conaffinity='2')
|
||||
for g in hand.iter('geom'):g.attrib.update(contype='1',conaffinity='2',friction='.6 .005 .001',condim='3',solref='.01 1')
|
||||
root.find('default/joint').attrib.update(damping='.05',armature='.002')
|
||||
# Scalar wrist coordinates are SPIDER's dexterous-hand control convention.
|
||||
axes=['1 0 0','0 1 0','0 0 1'];wristnames=[]
|
||||
for i in range(6):
|
||||
name='right_hand_'+('pos_'+'xyz'[i] if i<3 else 'rot_'+'xyz'[i-3]);wristnames.append(name)
|
||||
hand.insert(i,E.Element('joint',name=name,type='slide' if i<3 else 'hinge',axis=axes[i%3],limited='false',damping='0',armature='.02'))
|
||||
urdf=E.parse(source.source_urdf()).getroot();joints=urdf.findall('joint');names=[j.get('name') for j in joints];active=[j.get('name') for j in joints if j.find('mimic') is None]
|
||||
limits={j.get('name'):np.array([float(j.find('limit').get('lower')),float(j.find('limit').get('upper'))]) for j in joints};mimic={}
|
||||
for j in joints:
|
||||
mi=j.find('mimic')
|
||||
if mi is not None:
|
||||
name=j.get('name');a=mi.get('joint');scale=float(mi.get('multiplier','1'));offset=float(mi.get('offset','0'));mimic[name]=(a,scale,offset)
|
||||
lo,hi=np.sort((limits[name]-offset)/scale);limits[a]=np.array([max(limits[a][0],lo),min(limits[a][1],hi)])
|
||||
for eq in root.findall('equality/joint'):eq.attrib.update(solref='.01 1')
|
||||
act=E.SubElement(root,'actuator')
|
||||
for i,name in enumerate(wristnames):E.SubElement(act,'position',name=name+'_act',joint=name,kp='2000' if i<3 else '40',kv='60' if i<3 else '2',forcerange='-200 200' if i<3 else '-20 20')
|
||||
for name in active:E.SubElement(act,'position',name=name+'_act',joint=name,kp='8',kv='.15',forcerange='-1 1',ctrlrange=source._fmt(limits[name]))
|
||||
bottlepath=ROOT/'third_party/bottle_model_parametric/bottle.xml';bottle=E.parse(bottlepath).getroot()
|
||||
for item in bottle.find('asset'):
|
||||
item=copy.deepcopy(item)
|
||||
if item.get('file'):item.set('file',str((bottlepath.parent/item.get('file')).resolve()))
|
||||
if item.tag=='mesh':item.set('maxhullvert','64')
|
||||
root.find('asset').append(item)
|
||||
obj=E.SubElement(world,'body',name='right_object')
|
||||
objectnames=[]
|
||||
for i in range(6):
|
||||
name='right_object_'+('pos_'+'xyz'[i] if i<3 else 'rot_'+'xyz'[i-3]);objectnames.append(name)
|
||||
E.SubElement(obj,'joint',name=name,type='slide' if i<3 else 'hinge',axis=axes[i%3],limited='false',damping='0',armature='0')
|
||||
E.SubElement(act,'position',name=name,joint=name,kp='0',kv='0')
|
||||
for j,item in enumerate(bottle.find('worldbody/body')):
|
||||
if item.tag=='freejoint':continue
|
||||
item=copy.deepcopy(item)
|
||||
if item.tag=='geom':
|
||||
item.set('name',f'bottle_{j}')
|
||||
if item.get('contype','1')!='0':item.attrib.update(contype='2',conaffinity='5',friction='.6 .005 .001',condim='3',solref='.01 1')
|
||||
obj.append(item)
|
||||
E.SubElement(obj,'site',name='right_object_track',pos='0 0 .07')
|
||||
scene=TASK/'scene_act.xml';tree.write(scene)
|
||||
model=mujoco.MjModel.from_xml_path(str(scene));data=mujoco.MjData(model)
|
||||
addr={n:int(model.jnt_qposadr[model.joint(n).id]) for n in names+wristnames+objectnames}
|
||||
actnames=[wristnames[i] for i in range(6)]+active+objectnames
|
||||
source_motion=np.load(ROOT/'output/l20_2047635068/jitter_audit/temporal/motion.npz');start=600;stop=660
|
||||
sourcenames=list(source_motion['joint_names']);wp=source_motion['wrist_pos'][start:stop];wr=Rotation.from_quat(source_motion['wrist_quat_wxyz'][start:stop][:,[1,2,3,0]])
|
||||
# Estimated bottle registration, not an observed video object track.
|
||||
relpos=np.array([.12,-.065,.15]);relrot=Rotation.from_euler('x',-np.pi/2)
|
||||
op=wp+wr.apply(np.tile(relpos,(len(wp),1)));orr=wr*relrot
|
||||
align=orr[0].inv();shift=np.array([0,0,.3])-align.apply(op[0]);wp=align.apply(wp)+shift;wr=align*wr;op=align.apply(op)+shift;orr=align*orr
|
||||
baseq=np.stack([source_motion['qpos'][start:stop,sourcenames.index(n)] for n in active],axis=1)
|
||||
low=np.array([limits[n][0] for n in active]);high=np.array([limits[n][1] for n in active]);baseq=np.clip(baseq,low,high)
|
||||
handgeoms=np.flatnonzero(model.geom_contype==1);objectgeoms=np.flatnonzero(model.geom_contype==2)
|
||||
tips=[next(g for g in handgeoms if model.geom_bodyid[g]==model.body(f+'_distal').id) for f in source.FINGERS]
|
||||
def pose(a,pos,rot,objpos,objrot):
|
||||
q=dict(zip(active,a));q.update({n:sc*q[parent]+off for n,(parent,sc,off) in mimic.items()})
|
||||
for n,v in q.items():data.qpos[addr[n]]=v
|
||||
data.qpos[[addr[n] for n in wristnames]]=np.r_[pos,rot.as_euler('XYZ')]
|
||||
data.qpos[[addr[n] for n in objectnames]]=np.r_[objpos,objrot.as_euler('XYZ')]
|
||||
mujoco.mj_kinematics(model,data)
|
||||
def distances():return np.array([min(mujoco.mj_geomDistance(model,data,int(g),int(o),.5,None) for o in objectgeoms) for g in handgeoms])
|
||||
ti=[list(handgeoms).index(g) for g in tips]
|
||||
def residual(x):
|
||||
pose(x[:16],wp[0]+x[16:19],wr[0]*Rotation.from_rotvec(x[19:]),op[0],orr[0]);d=distances()
|
||||
return np.r_[(d[ti]+.0004)/.004,np.maximum(-d-.0005,0)/.001,.08*(x[:16]-baseq[0]),.2*x[16:19]/.03,.15*x[19:]/.3]
|
||||
x0=np.r_[baseq[0],np.zeros(6)];residual(x0);before=distances()
|
||||
fits=[]
|
||||
for shift_x,shift_z in [(0,0),(.025,0),(-.025,0),(0,.025),(0,-.025),(.025,.025),(-.025,-.025)]:
|
||||
seed=x0.copy();seed[16]=shift_x;seed[18]=shift_z
|
||||
trial=least_squares(residual,seed,bounds=(np.r_[low,[-.06]*3,[-.5]*3],np.r_[high,[.06]*3,[.5]*3]),max_nfev=250,diff_step=1e-4)
|
||||
residual(trial.x);ds=distances();score=float(np.abs(ds[ti]).mean()+3*max(0,-ds.min()-.001))
|
||||
fits.append((score,trial));print('initialization seed',shift_x,shift_z,'gap mm',np.abs(ds[ti]).mean()*1000,'penetration mm',max(0,-ds.min())*1000,flush=True)
|
||||
fit=min(fits,key=lambda x:x[0])[1]
|
||||
residual(fit.x);after=distances();contactlocal=[]
|
||||
# Sites mark the actual closest point on each distal link at initialization.
|
||||
for finger,g in zip(source.FINGERS,tips):
|
||||
candidates=[]
|
||||
for o in objectgeoms:
|
||||
segment=np.zeros(6);dist=mujoco.mj_geomDistance(model,data,int(g),int(o),.5,segment);candidates.append((dist,segment.copy()))
|
||||
_,segment=min(candidates,key=lambda x:x[0]);bid=model.geom_bodyid[g];local=data.xmat[bid].reshape(3,3).T@(segment[:3]-data.xpos[bid])
|
||||
link=hand.find(f".//body[@name='{finger}_distal']");E.SubElement(link,'site',name=f'right_hand_{finger}_track',pos=source._fmt(local))
|
||||
contactlocal.append(orr[0].inv().apply(segment[3:]-op[0]))
|
||||
tree.write(scene);model=mujoco.MjModel.from_xml_path(str(scene));data=mujoco.MjData(model)
|
||||
contacts=[model.site(f'right_hand_{f}_track').id for f in source.FINGERS]
|
||||
# Preserve source movement, applying one smooth initial registration correction.
|
||||
source_times=np.arange(stop-start)/30;times=np.arange(801)*.0025;rows=[];controls=[];contactpos=[]
|
||||
from scipy.spatial.transform import Slerp
|
||||
wpi=np.stack([np.interp(times,source_times,wp[:,k]) for k in range(3)],axis=1)
|
||||
opi=np.stack([np.interp(times,source_times,op[:,k]) for k in range(3)],axis=1)
|
||||
wri=Slerp(source_times,wr)(np.minimum(times,source_times[-1]));ori=Slerp(source_times,orr)(np.minimum(times,source_times[-1]))
|
||||
ai=np.stack([np.interp(times,source_times,baseq[:,k]) for k in range(16)],axis=1);ai=np.clip(ai+fit.x[:16]-baseq[0],low,high)
|
||||
for t in range(len(times)):
|
||||
pose(ai[t],wpi[t]+fit.x[16:19],wri[t]*Rotation.from_rotvec(fit.x[19:]),opi[t],ori[t])
|
||||
rows.append(data.qpos.copy());controls.append([data.qpos[addr[n]] for n in actnames]);contactpos.append(ori[t].apply(contactlocal)+opi[t])
|
||||
q=np.array(rows);ctrl=np.array(controls);vel=np.gradient(q,.0025,axis=0);vel[0]=0
|
||||
np.savez_compressed(TRIAL/'trajectory_kinematic_act.npz',qpos=q,qvel=vel,ctrl=ctrl,contact=np.ones((len(q),5)),contact_pos=contactpos,time=times)
|
||||
(TASK/'task_info.json').write_text(json.dumps(dict(ref_dt=.0025,contact_site_ids=contacts),indent=2))
|
||||
# The final real rollout has zero object actuator gains; virtual assistance only during search.
|
||||
config=dict(dataset_dir=str(OUT/'datasets'),dataset_name='current',robot_type='l20',embodiment_type='right',task='bottle_hold',data_id=0,contact_guidance=True,contact_rew_scale=1.,device='cuda:0',sim_dt=.0025,ref_dt=.0025,ctrl_dt=.1,knot_dt=.1,horizon=.6,trace_dt=.01,render_dt=.02,num_samples=512,max_num_iterations=6,num_dr=1,num_dyn=1,pair_margin_range=[0.,0.],xy_offset_range=[0.,0.],guidance_decay_ratio=.4,init_pos_actuator_gain=10.,init_pos_actuator_bias=1.,init_rot_actuator_gain=.1,init_rot_actuator_bias=.02,max_sim_steps=800,show_viewer=False,viewer='',wait_on_finish=False,save_video=False,save_info=True,save_metrics=False,save_config=True,sanity_check_seconds=0.,use_torch_compile=False,improvement_threshold=0.,pos_noise_scale=.008,rot_noise_scale=.05,joint_noise_scale=.2,exploit_ratio=.02,base_pos_rew_scale=.3,base_rot_rew_scale=.1,joint_rew_scale=.003,pos_rew_scale=5.,rot_rew_scale=.3,vel_rew_scale=.0001,temperature=.1,nconmax_per_env=128,njmax_per_env=512)
|
||||
(OUT/'config.json').write_text(json.dumps(config,indent=2))
|
||||
report=dict(source_frames=[start,stop],duration_s=2.,source='Current video trajectory, offline smoothed',bottle_alignment='Estimated constant bottle-to-palm relation, not tracked from video',coupling='Original URDF linear mimic, no nonlinear JSON adaptor',active_finger_joints=16,passive_finger_joints=5,controlled_wrist_dofs=6,object_virtual_actuators=6,object_real_rollout_gains_zero=True,initial_fit_converged=bool(fit.success),initial_mean_surface_gap_before_mm=float(np.abs(before[ti]).mean()*1000),initial_mean_surface_gap_after_mm=float(np.abs(after[ti]).mean()*1000),initial_max_penetration_mm=float(max(0,-after.min())*1000),nq=model.nq,nv=model.nv,nu=model.nu,contact_sites=contacts)
|
||||
(OUT/'preparation.json').write_text(json.dumps(report,indent=2));print(json.dumps(report,indent=2))
|
||||
@@ -0,0 +1,42 @@
|
||||
"""Prepare project-local Hugging Face cache metadata and resumable downloads."""
|
||||
import json
|
||||
import subprocess
|
||||
from pathlib import Path
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
CACHE = ROOT / '.hf_cache' / 'hub'
|
||||
|
||||
def fetch(url, target):
|
||||
target.parent.mkdir(parents=True, exist_ok=True)
|
||||
subprocess.run(['curl', '-fsSL', '--retry', '3', '--retry-all-errors', '--max-time', '45', url, '-o', str(target)], check=True)
|
||||
|
||||
manifest = []
|
||||
for repo, files, aliases in [
|
||||
('google-bert/bert-base-uncased', ['config.json', 'tokenizer.json', 'tokenizer_config.json', 'vocab.txt', 'model.safetensors'], ['bert-base-uncased']),
|
||||
('lpiccinelli/unidepth-v2-vitl14', ['config.json', 'model.safetensors'], []),
|
||||
('Rain729/Prior-Depth-Anything', ['depth_anything_v2_vitb.pth', 'prior_depth_anything_vitb.pth'], []),
|
||||
]:
|
||||
meta = ROOT / 'output' / (repo.replace('/', '_') + '_metadata.json')
|
||||
fetch('https://hf-mirror.com/api/models/' + repo, meta)
|
||||
revision = json.loads(meta.read_text())['sha']
|
||||
cache = CACHE / ('models--' + repo.replace('/', '--'))
|
||||
snapshot = cache / 'snapshots' / revision
|
||||
snapshot.mkdir(parents=True, exist_ok=True)
|
||||
(cache / 'refs').mkdir(exist_ok=True)
|
||||
(cache / 'refs' / 'main').write_text(revision)
|
||||
for name in files:
|
||||
target = snapshot / name
|
||||
url = 'https://hf-mirror.com/' + repo + '/resolve/' + revision + '/' + name
|
||||
if repo.startswith('lpiccinelli/'):
|
||||
local = ROOT / 'weights' / 'unidepth-v2-vitl14' / name
|
||||
if not target.exists() and not target.is_symlink():
|
||||
target.symlink_to(local)
|
||||
elif name.endswith(('.json', '.txt')):
|
||||
fetch(url, target)
|
||||
else:
|
||||
manifest.extend([url, ' dir=' + str(snapshot), ' out=' + name])
|
||||
for alias in aliases:
|
||||
alias_path = CACHE / ('models--' + alias.replace('/', '--'))
|
||||
if not alias_path.exists():
|
||||
alias_path.symlink_to(cache, target_is_directory=True)
|
||||
(ROOT / 'output' / 'vipe_weights_downloads.txt').write_text('\n'.join(manifest) + '\n')
|
||||
print('Prepared', len(manifest) // 3, 'weight downloads')
|
||||
@@ -0,0 +1,70 @@
|
||||
"""Render fixed-world and wrist-following views of a moving L20 trajectory."""
|
||||
import os
|
||||
os.environ.setdefault('MUJOCO_GL','osmesa')
|
||||
import argparse,json
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
import mujoco,imageio.v2 as imageio
|
||||
from PIL import Image,ImageDraw
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
def main():
|
||||
parser=argparse.ArgumentParser()
|
||||
parser.add_argument('--directory',type=Path,default=Path(__file__).resolve().parents[1]/'output/l20_2047635068')
|
||||
parser.add_argument('--frames',type=int,default=0)
|
||||
args=parser.parse_args();out=args.directory
|
||||
with np.load(out/'motion.npz') as archive:
|
||||
motion={name:archive[name] for name in archive.files}
|
||||
q=motion['qpos'][:args.frames or None];positions=motion['wrist_pos'][:len(q)]
|
||||
model=mujoco.MjModel.from_xml_path(str(out/'l20_moving.xml'));data=mujoco.MjData(model)
|
||||
addr=[model.jnt_qposadr[mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_JOINT,str(n))] for n in motion['joint_names']]
|
||||
wrist=model.jnt_qposadr[mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_JOINT,'wrist_free')]
|
||||
rotations=Rotation.from_quat(motion['wrist_quat_wxyz'][:,[1,2,3,0]]).as_matrix()
|
||||
world=np.einsum('tij,tkj->tki',rotations,motion['actual'])+motion['wrist_pos'][:,None]
|
||||
center=(world.min((0,1))+world.max((0,1)))/2
|
||||
span=np.ptp(world,axis=(0,1)).max()
|
||||
floor=mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_GEOM,'floor')
|
||||
model.geom_pos[floor,2]=world[:,:,2].min()-.035
|
||||
model.vis.quality.shadowsize=1024;model.vis.quality.offsamples=1
|
||||
model.vis.headlight.ambient[:]=.65;model.vis.headlight.diffuse[:]=.7
|
||||
renderer=mujoco.Renderer(model,height=480,width=640)
|
||||
cameras=[]
|
||||
for azimuth in [135,25]:
|
||||
cam=mujoco.MjvCamera();cam.lookat[:]=center;cam.distance=max(.65,span*2.3);cam.azimuth=azimuth;cam.elevation=10
|
||||
cameras.append(cam)
|
||||
option=mujoco.MjvOption();option.sitegroup[:]=0
|
||||
fps=float(motion['fps'])
|
||||
coupling=str(motion['coupling']) if 'coupling' in motion else 'JSON-calibrated'
|
||||
writer=imageio.get_writer(out/'l20_moving_replay.mp4',fps=fps,codec='libx264',quality=8)
|
||||
max_wrist_error=0.
|
||||
try:
|
||||
for t,pose in enumerate(q):
|
||||
data.qpos[addr]=pose;data.qpos[wrist:wrist+3]=positions[t];data.qpos[wrist+3:wrist+7]=motion['wrist_quat_wxyz'][t]
|
||||
mujoco.mj_forward(model,data)
|
||||
base=mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_BODY,'hand_base_link')
|
||||
max_wrist_error=max(max_wrist_error,float(np.abs(data.xpos[base]-positions[t]).max()))
|
||||
panels=[]
|
||||
cameras[1].lookat[:]=world[t].mean(0);cameras[1].distance=.58
|
||||
for vi,cam in enumerate(cameras):
|
||||
renderer.update_scene(data,camera=cam,scene_option=option)
|
||||
if vi==0:
|
||||
points=positions[np.unique(np.linspace(0,t,min(t+1,60),dtype=int))]
|
||||
for a,b in zip(points[:-1],points[1:]):
|
||||
scene=renderer.scene
|
||||
mujoco.mjv_initGeom(scene.geoms[scene.ngeom],mujoco.mjtGeom.mjGEOM_CAPSULE,np.ones(3)*.001,a,np.eye(3).ravel(),np.array([.2,.8,1,1]))
|
||||
mujoco.mjv_connector(scene.geoms[scene.ngeom],mujoco.mjtGeom.mjGEOM_CAPSULE,.0015,a,b)
|
||||
scene.ngeom+=1
|
||||
panels.append(renderer.render().copy())
|
||||
img=Image.fromarray(np.concatenate(panels,axis=1));draw=ImageDraw.Draw(img)
|
||||
draw.rectangle([0,0,1280,45],fill=(15,22,30))
|
||||
draw.text((10,8),f'L20 right | frame {t+1}/{len(q)} | moving 6D wrist + {coupling} fingers',fill='white')
|
||||
draw.text((10,27),'Fixed world camera / blue wrist trail',fill='white')
|
||||
draw.text((650,27),'Following camera / finger detail',fill='white')
|
||||
writer.append_data(np.asarray(img))
|
||||
if t in [0,len(q)//2,len(q)-1]:img.save(out/f'preview_{t:04d}.png')
|
||||
if t%200==0:print(f'render L20 {t}/{len(q)}',flush=True)
|
||||
finally:writer.close();renderer.close()
|
||||
(out/'render_validation.json').write_text(json.dumps(dict(frames=len(q),fps=fps,max_wrist_position_error_m=max_wrist_error),indent=2))
|
||||
print('COMPLETE',flush=True)
|
||||
|
||||
if __name__=='__main__':main()
|
||||
@@ -0,0 +1,73 @@
|
||||
"""Render contact registration separately from the free-object physics test."""
|
||||
import os
|
||||
os.environ.setdefault('MUJOCO_GL','osmesa')
|
||||
import argparse,json
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
import mujoco,imageio.v2 as imageio
|
||||
from PIL import Image,ImageDraw
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
OUT=Path(__file__).resolve().parents[1]/'output/l20_2047635068/bottle_grasp'
|
||||
def main():
|
||||
p=argparse.ArgumentParser();p.add_argument('--physics',action='store_true');p.add_argument('--frames',type=int,default=0);args=p.parse_args()
|
||||
physics=args.physics
|
||||
path=OUT/('physics_scene.xml' if physics else 'contact_scene.xml')
|
||||
motion=np.load(OUT/('physics_motion.npz' if physics else 'registered_motion.npz'))
|
||||
model=mujoco.MjModel.from_xml_path(str(path));data=mujoco.MjData(model)
|
||||
model.vis.headlight.ambient[:]=.65;model.vis.headlight.diffuse[:]=.7;model.vis.quality.offsamples=1
|
||||
renderer=mujoco.Renderer(model,height=480,width=640)
|
||||
option=mujoco.MjvOption();option.geomgroup[3]=0;option.sitegroup[:]=0
|
||||
count=len(motion['qpos']);count=min(count,args.frames) if args.frames else count
|
||||
fps=float(motion['fps'])
|
||||
obj=model.jnt_qposadr[model.joint('object_free').id]
|
||||
if not physics:
|
||||
addresses=[model.jnt_qposadr[model.joint(str(n)).id] for n in motion['joint_names']]
|
||||
wrist=model.jnt_qposadr[model.joint('wrist_free').id]
|
||||
reference=int(motion['contact_onset_frame'])
|
||||
rotation=Rotation.from_quat(motion['bottle_quat_wxyz'][reference,[1,2,3,0]]).inv()
|
||||
translation=np.array([0,0,.3])-rotation.apply(motion['bottle_pos'][reference])
|
||||
bottle_pos=rotation.apply(motion['bottle_pos'])+translation
|
||||
wrist_pos=rotation.apply(motion['wrist_pos'])+translation
|
||||
bottle_quat=(rotation*Rotation.from_quat(motion['bottle_quat_wxyz'][:,[1,2,3,0]])).as_quat()[:,[3,0,1,2]]
|
||||
wrist_quat=(rotation*Rotation.from_quat(motion['wrist_quat_wxyz'][:,[1,2,3,0]])).as_quat()[:,[3,0,1,2]]
|
||||
both=np.concatenate([bottle_pos+[0,0,.08],wrist_pos])
|
||||
center=(both.min(0)+both.max(0))/2;distance=max(.75,np.ptp(both,axis=0).max()*2.5)
|
||||
floor=model.geom('floor').id;model.geom_pos[floor,2]=min(both[:,2])-.06
|
||||
else:center=np.array([0,0,.38]);distance=.65
|
||||
cameras=[]
|
||||
for azimuth in [45,225]:
|
||||
cam=mujoco.MjvCamera();cam.lookat[:]=center;cam.distance=distance;cam.azimuth=azimuth;cam.elevation=12;cameras.append(cam)
|
||||
filename='physics_test.mp4' if physics else 'contact_registration.mp4'
|
||||
writer=imageio.get_writer(OUT/filename,fps=fps,codec='libx264',quality=8)
|
||||
try:
|
||||
for t in range(count):
|
||||
if physics:
|
||||
data.qpos[:]=motion['qpos'][t];data.mocap_pos[0]=motion['wrist'][t,:3];data.mocap_quat[0]=motion['wrist'][t,3:]
|
||||
else:
|
||||
data.qpos[addresses]=motion['qpos'][t]
|
||||
data.qpos[wrist:wrist+7]=np.r_[wrist_pos[t],wrist_quat[t]]
|
||||
data.qpos[obj:obj+7]=np.r_[bottle_pos[t],bottle_quat[t]]
|
||||
cameras[1].lookat[:]=(bottle_pos[t]+wrist_pos[t])/2+[0,0,.04];cameras[1].distance=.65
|
||||
mujoco.mj_forward(model,data)
|
||||
panels=[]
|
||||
for camera in cameras:
|
||||
renderer.update_scene(data,camera=camera,scene_option=option);panels.append(renderer.render().copy())
|
||||
img=Image.fromarray(np.concatenate(panels,axis=1));draw=ImageDraw.Draw(img);draw.rectangle([0,0,1280,48],fill=(15,22,30))
|
||||
if physics:
|
||||
title='FREE OBJECT PHYSICS TEST | 16 actuators + nonlinear coupling | moving wrist'
|
||||
note=f"t={t/fps:.2f}s | slip={motion['relative_drift'][t]*1000:.1f} mm | contacts={motion['contact_count'][t]} | result: NOT RETAINED"
|
||||
color=(255,120,100)
|
||||
else:
|
||||
title='L20 + tri-camera bottle | CONTACT REGISTRATION | object poses prescribed, not a physical grasp'
|
||||
note=f"frame {t+1}/{count} | moving wrist | maximum penetration {motion['penetration_m'][t]*1000:.2f} mm | estimated object-to-hand alignment"
|
||||
color='white'
|
||||
draw.text((10,8),title,fill='white');draw.text((10,28),note,fill=color)
|
||||
writer.append_data(np.asarray(img))
|
||||
if t in [0,count//2,count-1]:img.save(OUT/(('physics' if physics else 'registration')+f'_{t:04d}.png'))
|
||||
if t%200==0:print(f'{filename} {t}/{count}',flush=True)
|
||||
finally:writer.close();renderer.close()
|
||||
(OUT/(filename+'.json')).write_text(json.dumps({'frames':count,'fps':fps,'physics':physics},indent=2))
|
||||
print('COMPLETE '+filename,flush=True)
|
||||
|
||||
if __name__=='__main__':main()
|
||||
@@ -0,0 +1,32 @@
|
||||
"""Matched wrist-local diagnostic view of the worst contact-jump interval."""
|
||||
import os
|
||||
os.environ.setdefault('MUJOCO_GL','osmesa')
|
||||
import json
|
||||
import numpy as np
|
||||
import mujoco,imageio.v2 as imageio
|
||||
from PIL import Image,ImageDraw
|
||||
from l20_calibrated import ROOT
|
||||
base=ROOT/'output/l20_2047635068';out=base/'jitter_audit'
|
||||
sources=[base/'motion.npz',out/'temporal/motion.npz',base/'bottle_grasp/registered_motion.npz']
|
||||
motion=[np.load(p) for p in sources]
|
||||
model=mujoco.MjModel.from_xml_path(str(base/'l20_moving.xml'));data=mujoco.MjData(model)
|
||||
addr=[model.jnt_qposadr[model.joint(str(n)).id] for n in motion[0]['joint_names']]
|
||||
model.geom_rgba[model.geom('floor').id,3]=0;model.vis.headlight.ambient[:]=.8
|
||||
model.vis.quality.offsamples=1;renderer=mujoco.Renderer(model,height=400,width=400)
|
||||
cam=mujoco.MjvCamera();cam.lookat[:]=[.025,0,.105];cam.distance=.48;cam.azimuth=130;cam.elevation=5
|
||||
opt=mujoco.MjvOption();opt.sitegroup[:]=0
|
||||
writer=imageio.get_writer(out/'comparison_12s_18s.mp4',fps=30,codec='libx264',quality=8)
|
||||
labels=['Original L20 (hand only)','Corrected + offline temporal smoothing','Previous contact fit (known jumps)']
|
||||
for t in range(360,540):
|
||||
panels=[]
|
||||
for m in motion:
|
||||
data.qpos[addr]=m['qpos'][t];mujoco.mj_forward(model,data);renderer.update_scene(data,camera=cam,scene_option=opt);panels.append(renderer.render().copy())
|
||||
img=Image.fromarray(np.concatenate(panels,axis=1));draw=ImageDraw.Draw(img);draw.rectangle([0,0,1200,64],fill=(20,25,35))
|
||||
for j,label in enumerate(labels):
|
||||
step=np.rad2deg(np.abs(motion[j]['qpos'][t]-motion[j]['qpos'][t-1]).max())
|
||||
draw.text((j*400+8,7),label,fill='white');draw.text((j*400+8,24),f'frame {t} | maximum joint step {step:.2f} deg',fill='white')
|
||||
draw.text((8,45),'Wrist-local diagnostic view only; saved trajectories retain moving wrists. No physical grasp claim.',fill='white')
|
||||
writer.append_data(np.asarray(img))
|
||||
if t==461:img.save(out/'comparison_frame461.png')
|
||||
writer.close();renderer.close()
|
||||
print('COMPLETE 180 frames, source interval [360,540), 30 fps')
|
||||
@@ -0,0 +1,60 @@
|
||||
"""Compare two trajectories with exactly the same fixed world camera."""
|
||||
import os
|
||||
os.environ.setdefault('MUJOCO_GL','osmesa')
|
||||
from pathlib import Path
|
||||
import json
|
||||
import mujoco
|
||||
import numpy as np
|
||||
import imageio.v2 as imageio
|
||||
from PIL import Image,ImageDraw,ImageFont
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
ROOT=Path(__file__).resolve().parents[1]
|
||||
out=ROOT/'output/l20_15886123_stable'
|
||||
motions=[]
|
||||
for folder in [ROOT/'output/l20_15886123',out]:
|
||||
with np.load(folder/'motion.npz') as f:
|
||||
motions.append({k:f[k] for k in f.files})
|
||||
count=len(motions[0]['qpos']);fps=float(motions[0]['fps'])
|
||||
assert count==len(motions[1]['qpos'])
|
||||
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 motions[0]['joint_names']]
|
||||
wrist=model.jnt_qposadr[model.joint('wrist_free').id]
|
||||
world=[]
|
||||
for m in motions:
|
||||
r=Rotation.from_quat(m['wrist_quat_wxyz'][:,[1,2,3,0]]).as_matrix()
|
||||
world.append(np.einsum('tij,tkj->tki',r,m['actual'])+m['wrist_pos'][:,None])
|
||||
points=np.concatenate(world).reshape(-1,3)
|
||||
center=(points.min(0)+points.max(0))/2
|
||||
model.geom_pos[model.geom('floor').id,2]=points[:,2].min()-.035
|
||||
model.vis.quality.offsamples=1
|
||||
model.vis.headlight.ambient[:]=.65
|
||||
model.vis.headlight.diffuse[:]=.7
|
||||
cam=mujoco.MjvCamera();cam.lookat[:]=center;cam.distance=max(.65,float(np.ptp(points,axis=0).max())*2.1);cam.azimuth=100;cam.elevation=5
|
||||
option=mujoco.MjvOption();option.sitegroup[:]=0
|
||||
renderer=mujoco.Renderer(model,height=480,width=640)
|
||||
font=ImageFont.truetype('/usr/share/fonts/truetype/dejavu/DejaVuSans.ttf',18)
|
||||
writer=imageio.get_writer(out/'comparison.mp4',fps=fps,codec='libx264',quality=8)
|
||||
try:
|
||||
for t in range(count):
|
||||
panels=[]
|
||||
for m in motions:
|
||||
data.qpos[addr]=m['qpos'][t]
|
||||
data.qpos[wrist:wrist+3]=m['wrist_pos'][t]
|
||||
data.qpos[wrist+3:wrist+7]=m['wrist_quat_wxyz'][t]
|
||||
mujoco.mj_forward(model,data)
|
||||
renderer.update_scene(data,camera=cam,scene_option=option)
|
||||
panels.append(renderer.render().copy())
|
||||
im=Image.fromarray(np.concatenate(panels,axis=1));d=ImageDraw.Draw(im)
|
||||
d.rectangle([0,0,1280,50],fill=(15,22,30))
|
||||
d.text((12,8),'Before - previous smoothing',font=font,fill='white')
|
||||
d.text((652,8),'After - temporal regularization',font=font,fill='white')
|
||||
d.text((12,29),f'Same fixed camera | {t/fps:.2f} s | moving wrist preserved',font=font,fill=(190,210,225))
|
||||
writer.append_data(np.asarray(im))
|
||||
if t==280:im.save(out/'comparison_preview.png')
|
||||
if t%200==0:print('AB render',t,count,flush=True)
|
||||
finally:
|
||||
writer.close();renderer.close()
|
||||
(out/'comparison_camera.json').write_text(json.dumps(dict(same_camera=True,fixed_world_camera=True,frames=count,fps=fps,lookat=center.tolist(),distance=cam.distance,azimuth=cam.azimuth,elevation=cam.elevation),indent=2))
|
||||
print('COMPLETE',flush=True)
|
||||
@@ -0,0 +1,28 @@
|
||||
"""Retry residual collision failures from the preceding valid pose."""
|
||||
import json
|
||||
import numpy as np
|
||||
from l20_bottle_sequence import BASE,OUT
|
||||
from l20_contact import ContactRefiner
|
||||
|
||||
src=np.load(BASE/'motion.npz');m=dict(np.load(OUT/'registered_motion.npz'))
|
||||
s=ContactRefiner(json.loads((OUT/'object_spec.json').read_text()),OUT)
|
||||
names=list(m['joint_names']);reports=[]
|
||||
for t in np.where(m['penetration_m']>.0015)[0]:
|
||||
active=np.array([src['qpos'][t,names.index(n)] for n in s.cal.active])
|
||||
x,_,r=s.solve(active,src['wrist_pos'][t],src['wrist_quat_wxyz'][t],m['bottle_pos'][t],m['bottle_quat_wxyz'][t],initial=m['optimization_variables'][t-1])
|
||||
if r['max_penetration_after_mm']<m['penetration_m'][t]*1000:
|
||||
m['optimization_variables'][t]=x
|
||||
m['qpos'][t]=[s.expand(x[:16])[str(n)] for n in names]
|
||||
m['wrist_pos'][t]=s.data.qpos[s.wrist:s.wrist+3]
|
||||
m['wrist_quat_wxyz'][t]=s.data.qpos[s.wrist+3:s.wrist+7]
|
||||
m['penetration_m'][t]=r['max_penetration_after_mm']/1000
|
||||
m['command_u8'][t]=s.cal.command(dict(zip(names,m['qpos'][t])))
|
||||
reports.append(dict(frame=int(t),**r))
|
||||
np.savez_compressed(OUT/'registered_motion.npz',**m)
|
||||
header=(OUT/'registered_trajectory.csv').open().readline().strip()
|
||||
np.savetxt(OUT/'registered_trajectory.csv',np.c_[np.arange(len(m['qpos']))/30,m['wrist_pos'],m['wrist_quat_wxyz'],m['qpos'],m['bottle_pos'],m['bottle_quat_wxyz']],delimiter=',',header=header,comments='')
|
||||
r=json.loads((OUT/'sequence_validation.json').read_text());r['neighbor_retry_reports']=reports
|
||||
r['max_all_frame_penetration_mm']=float(m['penetration_m'].max()*1000)
|
||||
r['max_holding_penetration_mm']=float(m['penetration_m'][160:].max()*1000)
|
||||
(OUT/'sequence_validation.json').write_text(json.dumps(r,indent=2))
|
||||
print({'retries':len(reports),'max_penetration_mm':r['max_all_frame_penetration_mm'],'remaining_bad_frames':np.where(m['penetration_m']>.0015)[0].tolist()})
|
||||
@@ -0,0 +1,20 @@
|
||||
"""Fresh GPU rollout of saved controls, without optimization or virtual assistance."""
|
||||
import os
|
||||
from pathlib import Path
|
||||
ROOT=Path(__file__).resolve().parents[1];OUT=ROOT/'output/spider_l20_contact'
|
||||
os.environ['WARP_CACHE_PATH']=str(ROOT/'.spider_cache/warp');os.environ.setdefault('MUJOCO_GL','osmesa')
|
||||
import json,numpy as np,mujoco,mujoco_warp as mw,warp as wp
|
||||
TASK=OUT/'datasets/processed/current/l20/right/bottle_hold';P=TASK/'0'
|
||||
r=np.load(P/'trajectory_kinematic_act.npz');a=np.load(P/'trajectory_mjwp_act.npz');ctrl=a['ctrl'].reshape(-1,a['ctrl'].shape[-1]);expected=a['qpos'].reshape(-1,a['qpos'].shape[-1])
|
||||
m=mujoco.MjModel.from_xml_path(str(TASK/'scene_act.xml'));m.opt.iterations=20;m.opt.ls_iterations=50;m.opt.o_solref[:]=[.02,1];m.opt.o_solimp[:]=[0,.95,.03,.5,2]
|
||||
assert not m.actuator_gainprm[-6:].any() and not m.actuator_biasprm[-6:].any()
|
||||
d=mujoco.MjData(m);d.qpos[:]=r['qpos'][0];d.qvel[:]=r['qvel'][0];d.ctrl[:]=r['ctrl'][0];mujoco.mj_step(m,d);d.time=0
|
||||
wp.init();rows=[]
|
||||
with wp.ScopedDevice('cuda:0'):
|
||||
wm=mw.put_model(m);wd=mw.put_data(m,d,nworld=2,nconmax=128,njmax=512)
|
||||
with wp.ScopedCapture() as capture:mw.step(wm,wd)
|
||||
for i,u in enumerate(ctrl):
|
||||
wd.ctrl.assign(np.stack([r['ctrl'][i+1],u]).astype(np.float32));wp.capture_launch(capture.graph);wp.synchronize();rows.append(wd.qpos.numpy())
|
||||
rows=np.array(rows);np.savez_compressed(OUT/'independent_gpu_replay.npz',baseline=rows[:,0],optimized_controls=rows[:,1])
|
||||
report=dict(all_finite=bool(np.isfinite(rows).all()),max_difference_from_native_spider_qpos=float(np.abs(rows[:,1]-expected).max()),optimized_final_object_pos=rows[-1,1,-6:-3].tolist(),native_final_object_pos=expected[-1,-6:-3].tolist(),zero_assistance=True)
|
||||
(OUT/'independent_gpu_replay.json').write_text(json.dumps(report,indent=2));print(json.dumps(report,indent=2),flush=True)
|
||||
@@ -0,0 +1,4 @@
|
||||
# Install into HandFlow .venv with --no-deps; reuse existing Torch/CUDA.
|
||||
transformers==4.48.3
|
||||
tokenizers==0.21.0
|
||||
huggingface-hub==0.28.1
|
||||
@@ -0,0 +1,55 @@
|
||||
"""Resume the existing 1333-frame reconstruction with project-local model caches."""
|
||||
import json
|
||||
import os
|
||||
from pathlib import Path
|
||||
import subprocess
|
||||
import time
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
OUT = ROOT / 'output'
|
||||
STATUS = OUT / 'dynhamr_auto_status.json'
|
||||
def status(stage, **extra):
|
||||
payload = dict(stage=stage, time=time.strftime('%Y-%m-%d %H:%M:%S'), **extra)
|
||||
STATUS.write_text(json.dumps(payload, indent=2) + '\n')
|
||||
print(json.dumps(payload), flush=True)
|
||||
|
||||
def required_files():
|
||||
hub = ROOT / '.torch_cache/hub'
|
||||
files = [hub/'sam/sam_vit_b_01ec64.pth', hub/'aot/R50_DeAOTL_PRE_YTB_DAV.pth',
|
||||
hub/'checkpoints/groundingdino_swint_ogc.pth', hub/'checkpoints/video_depth_anything_vits.pth',
|
||||
ROOT/'weights/unidepth-v2-vitl14/model.safetensors']
|
||||
manifest = (OUT/'vipe_weights_downloads.txt').read_text().splitlines()
|
||||
for i in range(0, len(manifest), 3):
|
||||
files.append(Path(manifest[i+1].strip().removeprefix('dir='))/manifest[i+2].strip().removeprefix('out='))
|
||||
return files
|
||||
|
||||
files = required_files()
|
||||
deadline = time.monotonic() + 5400
|
||||
while True:
|
||||
missing = [str(p) for p in files if not p.is_file() or p.stat().st_size < 1000000 or Path(str(p)+'.aria2').exists()]
|
||||
if not missing:
|
||||
break
|
||||
status('WAITING_WEIGHTS', pending=missing)
|
||||
if time.monotonic() > deadline:
|
||||
status('FAILED', reason='Weight preparation timeout')
|
||||
raise SystemExit(1)
|
||||
time.sleep(30)
|
||||
|
||||
env = os.environ.copy()
|
||||
env.update(TORCH_HOME=str(ROOT/'.torch_cache'), HF_HOME=str(ROOT/'.hf_cache'),
|
||||
HF_HUB_OFFLINE='1', TRANSFORMERS_OFFLINE='1', PYTHONUNBUFFERED='1',
|
||||
VIPE_CACHE_DIR=str(OUT/'vipe_frame_cache'),
|
||||
MPLCONFIGDIR='/tmp/matplotlib-dynhamr', HYDRA_FULL_ERROR='1', PYOPENGL_PLATFORM='egl')
|
||||
dyn = ROOT/'third_party/Dyn-HaMR'
|
||||
env['PATH'] = str(dyn/'.dynhamr/bin') + ':' + env['PATH']
|
||||
env['PYTHONPATH'] = ':'.join(str(p) for p in [dyn/'dyn-hamr', dyn/'third-party/hamer', dyn/'third-party/hamer/third-party/ViTPose'])
|
||||
cmd = [str(dyn/'.dynhamr/bin/python'), '-u', 'run_opt.py', 'data=video_vipe',
|
||||
'data.root='+str(dyn/'test'), 'data.seq=2047635068',
|
||||
'data.vipe_dir='+str(OUT/'vipe_2047635068'), 'is_static=False',
|
||||
'run_opt=True', 'run_vis=True', 'run_prior=True', '+data.vipe_pipeline=dynhamr_cameras',
|
||||
'HMP.vid_path='+str(dyn/'test/videos/2047635068.mp4')]
|
||||
status('RUNNING', command=cmd)
|
||||
with (OUT/'dynhamr_auto_run.log').open('a') as log:
|
||||
result = subprocess.run(cmd, cwd=dyn/'dyn-hamr', env=env, stdout=log, stderr=subprocess.STDOUT)
|
||||
status('PROCESS_EXITED' if result.returncode == 0 else 'FAILED', returncode=result.returncode,
|
||||
log=str(OUT/'dynhamr_auto_run.log'))
|
||||
raise SystemExit(result.returncode)
|
||||
@@ -0,0 +1,165 @@
|
||||
"""Retarget exported video joints to L20 using original URDF linear coupling."""
|
||||
import os
|
||||
os.environ.setdefault('MUJOCO_GL', 'osmesa')
|
||||
import argparse
|
||||
import copy
|
||||
import json
|
||||
from pathlib import Path
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
import mujoco
|
||||
import numpy as np
|
||||
import torch
|
||||
from scipy.ndimage import gaussian_filter1d
|
||||
from scipy.spatial.transform import Rotation
|
||||
from dex_retargeting.retargeting_config import RetargetingConfig
|
||||
from dex_retargeting.kinematics_adaptor import MimicJointKinematicAdaptor
|
||||
from l20_calibrated import ASSET, build_assets, palm_basis, CHAINS, TIPS
|
||||
from l20_consistent_optimizer import ConsistentPositionOptimizer
|
||||
|
||||
|
||||
def main():
|
||||
p = argparse.ArgumentParser()
|
||||
p.add_argument('--input', type=Path, required=True)
|
||||
p.add_argument('--output-dir', type=Path, required=True)
|
||||
p.add_argument('--sigma', type=float, default=2.5)
|
||||
a = p.parse_args()
|
||||
torch.set_num_threads(4)
|
||||
out = a.output_dir.resolve()
|
||||
fixed, urdf = build_assets(out)
|
||||
original = ET.parse(ASSET / 'linkerhand_g20_right.urdf').getroot()
|
||||
joints = [j for j in original.findall('joint') if j.get('type') != 'fixed']
|
||||
active = [j.get('name') for j in joints if j.find('mimic') is None]
|
||||
limits = {j.get('name'):np.array([float(j.find('limit').get(k)) for k in ['lower','upper']]) for j in joints}
|
||||
bounds_by_name = {n: limits[n].copy() for n in active}
|
||||
mimic = {}
|
||||
for j in joints:
|
||||
m = j.find('mimic')
|
||||
if m is None:
|
||||
continue
|
||||
parent = m.get('joint')
|
||||
scale, offset = float(m.get('multiplier', '1')), float(m.get('offset', '0'))
|
||||
assert parent in active and scale != 0
|
||||
mimic[j.get('name')] = (parent, scale, offset)
|
||||
lo, hi = np.sort((limits[j.get('name')] - offset) / scale)
|
||||
bounds_by_name[parent] = np.array([max(bounds_by_name[parent][0], lo), min(bounds_by_name[parent][1], hi)])
|
||||
bounds = np.array([bounds_by_name[n] for n in active])
|
||||
config = dict(type='position', urdf_path=str(urdf), target_joint_names=active,
|
||||
target_link_names=[f'landmark_{i:02d}' for i in range(21)],
|
||||
target_link_human_indices=list(range(21)), low_pass_alpha=-1, normal_delta=.002)
|
||||
retarget = RetargetingConfig.from_dict(copy.deepcopy(config)).build()
|
||||
retarget.optimizer.__class__ = ConsistentPositionOptimizer
|
||||
robot = retarget.optimizer.robot
|
||||
adaptor = MimicJointKinematicAdaptor(robot, active, [m[0] for m in mimic.values()],
|
||||
list(mimic), [m[1] for m in mimic.values()], [m[2] for m in mimic.values()])
|
||||
retarget.optimizer.set_kinematic_adaptor(adaptor)
|
||||
retarget.joint_limits = bounds
|
||||
retarget.optimizer.set_joint_limit(bounds, epsilon=0)
|
||||
neutral_q = np.zeros(robot.dof)
|
||||
neutral_q[adaptor.idx_pin2target] = np.clip(np.zeros(len(active)), bounds[:, 0], bounds[:, 1])
|
||||
neutral_q = adaptor.forward_qpos(neutral_q)
|
||||
retarget.set_qpos(neutral_q)
|
||||
sites = [robot.get_link_index(f'landmark_{i:02d}') for i in range(21)]
|
||||
|
||||
def fk(q):
|
||||
robot.compute_forward_kinematics(q)
|
||||
return np.array([robot.get_link_pose(i)[:3, 3] for i in sites])
|
||||
|
||||
neutral = fk(neutral_q)
|
||||
robot_basis = palm_basis(neutral)
|
||||
human = np.load(a.input)
|
||||
hp = human['joints']
|
||||
count, fps = len(hp), float(human['fps'])
|
||||
assert hp.shape == (count, 21, 3) and np.isfinite(hp).all() and fps > 0
|
||||
targets, world_R, qraw, codes = [], [], [], []
|
||||
for t, pts in enumerate(hp):
|
||||
align = palm_basis(pts) @ robot_basis.T
|
||||
aligned = (pts - pts[0]) @ align
|
||||
target = neutral.copy()
|
||||
for chain in CHAINS:
|
||||
for i, j in zip(chain[:-1], chain[1:]):
|
||||
d = aligned[j] - aligned[i]
|
||||
assert np.linalg.norm(d) > 1e-8
|
||||
target[j] = target[i] + d / np.linalg.norm(d) * np.linalg.norm(neutral[j] - neutral[i])
|
||||
targets.append(target)
|
||||
world_R.append(Rotation.from_rotvec(human['root_orient'][t]).as_matrix() @ align)
|
||||
qraw.append(retarget.retarget(target).copy())
|
||||
codes.append(retarget.optimizer.opt.last_optimize_result())
|
||||
qraw, targets, world_R = np.array(qraw), np.array(targets), np.array(world_R)
|
||||
active_q = gaussian_filter1d(qraw[:, adaptor.idx_pin2target], a.sigma, axis=0) if a.sigma > 0 else qraw[:, adaptor.idx_pin2target]
|
||||
active_q = np.clip(active_q, bounds[:, 0], bounds[:, 1])
|
||||
qpos = []
|
||||
for row in active_q:
|
||||
q = np.zeros(robot.dof)
|
||||
q[adaptor.idx_pin2target] = row
|
||||
qpos.append(adaptor.forward_qpos(q).copy())
|
||||
qpos = np.array(qpos)
|
||||
actual = np.array([fk(q) for q in qpos])
|
||||
scene_R = world_R[0].T
|
||||
scene_t = np.array([0, 0, .2]) - scene_R @ human['wrist_world'][0]
|
||||
wrist_raw = human['wrist_world'] @ scene_R.T + scene_t
|
||||
wrist = gaussian_filter1d(wrist_raw, a.sigma, axis=0) if a.sigma > 0 else wrist_raw.copy()
|
||||
rot = Rotation.from_matrix(scene_R @ world_R)
|
||||
quat_raw = rot.as_quat()
|
||||
quat = quat_raw.copy()
|
||||
for t in range(1, count):
|
||||
if quat[t] @ quat[t-1] < 0:
|
||||
quat[t] *= -1
|
||||
if a.sigma > 0:
|
||||
quat = gaussian_filter1d(quat, a.sigma, axis=0)
|
||||
quat /= np.linalg.norm(quat, axis=1, keepdims=True)
|
||||
quat = quat[:, [3, 0, 1, 2]]
|
||||
times = np.arange(count) / fps
|
||||
np.savez_compressed(out / 'motion.npz', qpos=qpos, qpos_raw=qraw, joint_names=robot.dof_joint_names,
|
||||
active_qpos=active_q, active_joint_names=active, targets=targets, actual=actual,
|
||||
wrist_pos=wrist, wrist_pos_unsmoothed=wrist_raw, wrist_quat_wxyz=quat,
|
||||
wrist_quat_unsmoothed_wxyz=quat_raw[:, [3, 0, 1, 2]],
|
||||
wrist_world=human['wrist_world'], wrist_world_R=world_R,
|
||||
scene_rotation=scene_R, scene_translation=scene_t, time=times, fps=fps,
|
||||
coupling='linear_urdf', offline_sigma_frames=a.sigma,
|
||||
source=str(a.input.resolve()), detection_valid=human['detection_valid'])
|
||||
np.savetxt(out / 'trajectory.csv', np.c_[times, wrist, quat, qpos], delimiter=',',
|
||||
header=','.join(['time_s','wrist_x','wrist_y','wrist_z','qw','qx','qy','qz'] + robot.dof_joint_names), comments='')
|
||||
model = mujoco.MjModel.from_xml_path(str(fixed))
|
||||
data = mujoco.MjData(model)
|
||||
addresses = [model.jnt_qposadr[model.joint(n).id] for n in robot.dof_joint_names]
|
||||
mids = [model.site(f'landmark_{i:02d}').id for i in range(21)]
|
||||
fk_error = 0.
|
||||
for t, row in enumerate(qpos):
|
||||
data.qpos[addresses] = row
|
||||
mujoco.mj_forward(model, data)
|
||||
fk_error = max(fk_error, float(np.abs(data.site_xpos[mids] - actual[t]).max()))
|
||||
lim = np.array([limits[n] for n in robot.dof_joint_names])
|
||||
violation = float(np.maximum(lim[:, 0] - qpos, 0).max() + np.maximum(qpos - lim[:, 1], 0).max())
|
||||
names = list(robot.dof_joint_names)
|
||||
mimic_error = max(float(np.abs(qpos[:, names.index(n)] - (sc*qpos[:, names.index(parent)] + off)).max()) for n,(parent,sc,off) in mimic.items())
|
||||
assert violation < 1e-8 and mimic_error < 1e-10 and fk_error < 1e-5
|
||||
assert all(np.isfinite(x).all() for x in [qpos, wrist, quat, actual])
|
||||
error = np.linalg.norm(actual[:, TIPS] - targets[:, TIPS], axis=-1) * 1000
|
||||
report = dict(frames=count, fps=fps, duration_seconds=count/fps, source=str(a.input.resolve()),
|
||||
coupling='linear_urdf', independent_joints=len(active), urdf_joints=len(names),
|
||||
tip_error_mean_mm=float(error.mean()), tip_error_p95_mm=float(np.percentile(error,95)),
|
||||
tip_error_finger_mm=dict(zip(['thumb','index','middle','ring','pinky'],error.mean(0).tolist())),
|
||||
max_joint_step_deg=float(np.rad2deg(np.abs(np.diff(qpos,axis=0)).max())),
|
||||
joint_second_difference_rms_deg=float(np.rad2deg(np.sqrt(np.mean(np.diff(qpos,n=2,axis=0)**2)))),
|
||||
wrist_translation_range_m=np.ptp(wrist,axis=0).tolist(),
|
||||
wrist_smoothing_max_displacement_mm=float(np.linalg.norm(wrist-wrist_raw,axis=1).max()*1000),
|
||||
urdf_limit_violation_rad=violation, linear_mimic_max_error_rad=mimic_error,
|
||||
pinocchio_mujoco_fk_error_m=fk_error, fk_checked_frames=count, all_finite=True,
|
||||
quaternion_norm_max_error=float(np.abs(np.linalg.norm(quat,axis=1)-1).max()),
|
||||
offline_sigma_frames=a.sigma, uses_future_frames=a.sigma>0,
|
||||
optimizer_status_counts={str(k):int(v) for k,v in zip(*np.unique(codes,return_counts=True))},
|
||||
boundary='Hand-only kinematic retargeting; no object tracking, contact optimization, dynamics or hardware commands')
|
||||
(out / 'retarget_validation.json').write_text(json.dumps(report,indent=2))
|
||||
(out / 'dex_config.json').write_text(json.dumps(config,indent=2))
|
||||
tree = ET.parse(fixed)
|
||||
root = tree.getroot()
|
||||
ET.SubElement(root.find("worldbody/body[@name='hand_base_link']"), 'freejoint', name='wrist_free')
|
||||
root.find('option').set('gravity','0 0 0')
|
||||
# Replay supplies all angles; physical mimic equality is not needed for mj_forward.
|
||||
tree.write(out / 'l20_moving.xml')
|
||||
print(json.dumps(report,indent=2))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,5 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
cd "$(dirname "$0")/.."
|
||||
env PYTHONPATH= LD_LIBRARY_PATH= third_party/Dyn-HaMR/.dynhamr/bin/python scripts/export_dynhamr_dex.py
|
||||
env PYTHONPATH= LD_LIBRARY_PATH= MUJOCO_GL=osmesa .dex/bin/python scripts/run_dex_leap.py "$@"
|
||||
@@ -0,0 +1,127 @@
|
||||
"""Retarget Dyn-HaMR to the supplied L20 with JSON coupling and moving wrist."""
|
||||
import os
|
||||
os.environ.setdefault('MUJOCO_GL','osmesa')
|
||||
os.environ.setdefault('OMP_NUM_THREADS','4')
|
||||
import argparse,json,time
|
||||
from pathlib import Path
|
||||
import xml.etree.ElementTree as ET
|
||||
import numpy as np
|
||||
import torch,mujoco
|
||||
from scipy.spatial.transform import Rotation
|
||||
from scipy.ndimage import gaussian_filter1d
|
||||
from dex_retargeting.retargeting_config import RetargetingConfig
|
||||
from l20_calibrated import ROOT,Calibration,CalibratedAdaptor,build_assets,palm_basis,CHAINS,TIPS
|
||||
|
||||
OUT=ROOT/'output/l20_2047635068'
|
||||
|
||||
def main():
|
||||
global OUT
|
||||
parser=argparse.ArgumentParser();parser.add_argument('--frames',type=int,default=0)
|
||||
parser.add_argument('--output-dir',type=Path,default=OUT)
|
||||
parser.add_argument('--consistent-objective',action=argparse.BooleanOptionalAction,default=True)
|
||||
args=parser.parse_args();OUT=args.output_dir.resolve()
|
||||
torch.set_num_threads(4)
|
||||
fixed,urdf=build_assets(OUT)
|
||||
cal=Calibration()
|
||||
config=dict(type='position',urdf_path=str(urdf),target_joint_names=cal.active,
|
||||
target_link_names=[f'landmark_{i:02d}' for i in range(21)],
|
||||
target_link_human_indices=list(range(21)),low_pass_alpha=-1,normal_delta=.002)
|
||||
retarget=RetargetingConfig.from_dict(config.copy()).build()
|
||||
if args.consistent_objective:
|
||||
from l20_consistent_optimizer import ConsistentPositionOptimizer
|
||||
retarget.optimizer.__class__=ConsistentPositionOptimizer
|
||||
robot=retarget.optimizer.robot
|
||||
adaptor=CalibratedAdaptor(robot,cal);retarget.optimizer.set_kinematic_adaptor(adaptor)
|
||||
bounds=np.array([cal.bounds[n] for n in cal.active]);retarget.joint_limits=bounds
|
||||
retarget.optimizer.set_joint_limit(bounds,epsilon=0)
|
||||
neutral_q=np.zeros(robot.dof)
|
||||
for i,n in enumerate(cal.active):neutral_q[robot.get_joint_index(n)]=np.clip(cal.values[n][cal.raw['baseline_command_u8'][cal.tables[n]['motor_index']]],*bounds[i])
|
||||
neutral_q=adaptor.forward_qpos(neutral_q)
|
||||
retarget.set_qpos(neutral_q)
|
||||
sites=[robot.get_link_index(f'landmark_{i:02d}') for i in range(21)]
|
||||
def fk(q):
|
||||
robot.compute_forward_kinematics(q)
|
||||
return np.array([robot.get_link_pose(k)[:3,3] for k in sites])
|
||||
neutral=fk(neutral_q);basis_robot=palm_basis(neutral)
|
||||
human=np.load(ROOT/'output/dex_2047635068/human_joints.npz')
|
||||
joints=human['joints'][:args.frames or None]
|
||||
targets=[];wrist_rot=[]
|
||||
for t,p in enumerate(joints):
|
||||
basis=palm_basis(p)
|
||||
align=basis@basis_robot.T
|
||||
aligned=(p-p[0])@align
|
||||
target=neutral.copy()
|
||||
for chain in CHAINS:
|
||||
for a,b in zip(chain[:-1],chain[1:]):
|
||||
direction=aligned[b]-aligned[a]
|
||||
target[b]=target[a]+direction/np.linalg.norm(direction)*np.linalg.norm(neutral[b]-neutral[a])
|
||||
targets.append(target)
|
||||
wrist_rot.append(Rotation.from_rotvec(human['root_orient'][t]).as_matrix()@align)
|
||||
targets=np.array(targets);wrist_rot=np.array(wrist_rot)
|
||||
qraw=[];codes=[];start=time.monotonic()
|
||||
for t,target in enumerate(targets):
|
||||
q=retarget.retarget(target)
|
||||
qraw.append(q.copy());codes.append(retarget.optimizer.opt.last_optimize_result())
|
||||
if t%200==0: print(f'L20 dex {t}/{len(targets)}',flush=True)
|
||||
qraw=np.array(qraw)
|
||||
active=qraw[:,adaptor.idx_pin2target]
|
||||
active=np.clip(gaussian_filter1d(active,1,axis=0),bounds[:,0],bounds[:,1])
|
||||
qpos=[]
|
||||
for a in active:
|
||||
q=np.zeros(robot.dof);q[adaptor.idx_pin2target]=a;qpos.append(adaptor.forward_qpos(q))
|
||||
qpos=np.array(qpos);actual=np.array([fk(q) for q in qpos])
|
||||
commands=np.array([cal.command(dict(zip(robot.dof_joint_names,q))) for q in qpos])
|
||||
decoded=np.array([cal.decode(c,robot.dof_joint_names) for c in commands])
|
||||
wrist_world=human['wrist_world'][:len(qpos)]
|
||||
# A single rigid transform makes the initial palm upright for visualization.
|
||||
scene_rot=wrist_rot[0].T
|
||||
wrist_pos=(wrist_world-wrist_world[0])@scene_rot.T+np.array([0,0,.2])
|
||||
wrist_scene=np.einsum('ij,tjk->tik',scene_rot,wrist_rot)
|
||||
quat=Rotation.from_matrix(wrist_scene).as_quat()[:,[3,0,1,2]]
|
||||
for i in range(1,len(quat)):
|
||||
if quat[i]@quat[i-1]<0:quat[i]*=-1
|
||||
np.savez_compressed(OUT/'motion.npz',qpos=qpos,qpos_raw=qraw,joint_names=robot.dof_joint_names,
|
||||
active_qpos=active,active_joint_names=cal.active,command_u8=commands,decoded_qpos=decoded,
|
||||
wrist_pos=wrist_pos,wrist_quat_wxyz=quat,wrist_world=wrist_world,wrist_world_R=wrist_rot,
|
||||
scene_rotation=scene_rot,scene_translation=np.array([0,0,.2])-scene_rot@wrist_world[0],
|
||||
targets=targets,actual=actual,fps=30,time=np.arange(len(qpos))/30)
|
||||
np.savetxt(OUT/'trajectory.csv',np.c_[np.arange(len(qpos))/30,wrist_pos,quat,qpos],delimiter=',',
|
||||
header=','.join(['time_s','wrist_x','wrist_y','wrist_z','qw','qx','qy','qz']+robot.dof_joint_names),comments='')
|
||||
np.savetxt(OUT/'commands_u8.csv',np.c_[np.arange(len(qpos)),commands],delimiter=',',fmt='%d',
|
||||
header=','.join(['frame']+[f'slot_{i}' for i in range(20)]),comments='')
|
||||
limits=np.array([cal.urdf_limits[n] for n in robot.dof_joint_names])
|
||||
violation=float(np.maximum(limits[:,0]-qpos,0).max()+np.maximum(qpos-limits[:,1],0).max())
|
||||
assert violation<1e-6 and np.isfinite(qpos).all()
|
||||
err=np.linalg.norm(actual[:,TIPS]-targets[:,TIPS],axis=-1)*1000
|
||||
# Compare independent FK libraries, with all 21 nonlinear-coupled angles supplied.
|
||||
model=mujoco.MjModel.from_xml_path(str(fixed));data=mujoco.MjData(model)
|
||||
addresses=[model.jnt_qposadr[mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_JOINT,n)] for n in robot.dof_joint_names]
|
||||
ids=[mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_SITE,f'landmark_{i:02d}') for i in range(21)]
|
||||
fk_error=0
|
||||
for t in np.linspace(0,len(qpos)-1,min(20,len(qpos)),dtype=int):
|
||||
data.qpos[addresses]=qpos[t];mujoco.mj_forward(model,data)
|
||||
fk_error=max(fk_error,float(np.abs(data.site_xpos[ids]-actual[t]).max()))
|
||||
assert fk_error<1e-5
|
||||
report=dict(frames=len(qpos),fps=30,independent_joints=16,urdf_joints=21,
|
||||
calibration_model=cal.raw['model'],source_directory_name='L20',
|
||||
calibration_interpretation='q_urdf=angle_rad[command]+urdf_zero_offset_rad; passive joints share motor command; not hardware verified',
|
||||
reserved_command_slots=[11,12,13,14],tip_error_mean_mm=float(err.mean()),tip_error_p95_mm=float(np.percentile(err,95)),
|
||||
tip_error_finger_mm=dict(zip(['thumb','index','middle','ring','pinky'],err.mean(0).tolist())),
|
||||
wrist_translation_range_m=np.ptp(wrist_pos,axis=0).tolist(),
|
||||
wrist_rotation_max_from_initial_rad=float(np.linalg.norm(Rotation.from_matrix(wrist_scene).as_rotvec(),axis=1).max()),
|
||||
urdf_limit_violation_rad=violation,quantization_max_error_rad=float(np.abs(decoded-qpos).max()),
|
||||
pinocchio_mujoco_fk_error_m=fk_error,seconds=time.monotonic()-start,
|
||||
optimizer_status_counts={str(k):int(v) for k,v in zip(*np.unique(codes,return_counts=True))},
|
||||
consistent_objective=args.consistent_objective,
|
||||
object_contact_stage='not applied in this hand-only retargeting run')
|
||||
(OUT/'retarget_validation.json').write_text(json.dumps(report,indent=2))
|
||||
(OUT/'dex_config.json').write_text(json.dumps(config,indent=2))
|
||||
# Moving wrist replay model; contact scene construction is a separate stage.
|
||||
tree=ET.parse(fixed);root=tree.getroot()
|
||||
body=root.find("worldbody/body[@name='hand_base_link']")
|
||||
ET.SubElement(body,'freejoint',name='wrist_free')
|
||||
root.find('option').set('gravity','0 0 0')
|
||||
tree.write(OUT/'l20_moving.xml')
|
||||
print(json.dumps(report,indent=2),flush=True)
|
||||
|
||||
if __name__=='__main__':main()
|
||||
@@ -0,0 +1,165 @@
|
||||
"""Offline right-hand vector retargeting and MuJoCo kinematic replay."""
|
||||
import os
|
||||
os.environ.setdefault('MUJOCO_GL', 'osmesa')
|
||||
os.environ.setdefault('OMP_NUM_THREADS', '4')
|
||||
import argparse
|
||||
import json
|
||||
import time
|
||||
from pathlib import Path
|
||||
import xml.etree.ElementTree as ET
|
||||
import numpy as np
|
||||
import torch
|
||||
import mujoco
|
||||
import imageio.v2 as imageio
|
||||
from PIL import Image, ImageDraw
|
||||
from dex_retargeting.constants import RobotName, RetargetingType, HandType, get_default_config_path, OPERATOR2MANO_RIGHT
|
||||
from dex_retargeting.retargeting_config import RetargetingConfig
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
OUT = ROOT / 'output/dex_2047635068'
|
||||
ASSETS = ROOT / 'third_party/dex-assets/robots/hands'
|
||||
|
||||
def operator_frame(joints):
|
||||
# Same palm frame construction as upstream SingleHandDetector, without MediaPipe.
|
||||
points = joints[[0, 5, 9]]
|
||||
x = points[0] - points[2]
|
||||
points = points - points.mean(axis=0)
|
||||
_, _, v = np.linalg.svd(points)
|
||||
normal = v[2]
|
||||
x -= x.dot(normal) * normal
|
||||
x /= np.linalg.norm(x)
|
||||
z = np.cross(x, normal)
|
||||
if z.dot(points[1] - points[2]) < 0:
|
||||
normal *= -1
|
||||
z *= -1
|
||||
frame = np.stack([x, normal, z], axis=1)
|
||||
assert np.allclose(frame.T @ frame, np.eye(3), atol=1e-5)
|
||||
return frame @ OPERATOR2MANO_RIGHT
|
||||
|
||||
def load_scene(urdf):
|
||||
tree = ET.parse(urdf)
|
||||
root = tree.getroot()
|
||||
ext = ET.SubElement(root, 'mujoco')
|
||||
ET.SubElement(ext, 'compiler', discardvisual='false', fusestatic='false', strippath='false')
|
||||
for mesh in root.iter('mesh'):
|
||||
mesh.set('filename', str(urdf.parent / mesh.get('filename')))
|
||||
adapted = OUT / 'leap_replay.urdf'
|
||||
tree.write(adapted)
|
||||
model = mujoco.MjModel.from_xml_path(str(adapted))
|
||||
scene = OUT / 'scene.xml'
|
||||
mujoco.mj_saveLastXML(str(scene), model)
|
||||
tree = ET.parse(scene)
|
||||
root = tree.getroot()
|
||||
visual = ET.SubElement(root, 'visual')
|
||||
ET.SubElement(visual, 'global', offwidth='1280', offheight='480')
|
||||
ET.SubElement(visual, 'headlight', ambient='0.5 0.5 0.5', diffuse='0.7 0.7 0.7')
|
||||
ET.SubElement(root.find('worldbody'), 'light', pos='0 -1 1', dir='0 1 -1', directional='true')
|
||||
tree.write(scene)
|
||||
model = mujoco.MjModel.from_xml_path(str(scene))
|
||||
# Replay is kinematic. Retain only visual geometry in the viewer.
|
||||
model.geom_rgba[:, 3] = 1
|
||||
# LEAP fingertip pads are supplied only as collision meshes in this asset.
|
||||
# Display these meshes too, while keeping collision primitives hidden.
|
||||
model.geom_group[model.geom_type == mujoco.mjtGeom.mjGEOM_MESH] = 1
|
||||
return model
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument('--frames', type=int, default=0)
|
||||
parser.add_argument('--no-render', action='store_true')
|
||||
args = parser.parse_args()
|
||||
torch.set_num_threads(4)
|
||||
human = np.load(OUT / 'human_joints.npz')
|
||||
joints = human['joints'][:args.frames or None]
|
||||
RetargetingConfig.set_default_urdf_dir(ASSETS)
|
||||
config_path = get_default_config_path(RobotName.leap, RetargetingType.vector, HandType.right)
|
||||
cfg = RetargetingConfig.load_from_file(config_path)
|
||||
(OUT / 'retargeting_config.yml').write_text(Path(config_path).read_text())
|
||||
retarget = cfg.build()
|
||||
robot = retarget.optimizer.robot
|
||||
index = retarget.optimizer.target_link_human_indices
|
||||
tip_ids = [robot.get_link_index(x) for x in cfg.target_task_link_names]
|
||||
base_id = robot.get_link_index('base')
|
||||
qs, targets, actual, codes = [], [], [], []
|
||||
start = time.monotonic()
|
||||
for i, j in enumerate(joints):
|
||||
local = (j - j[:1]) @ operator_frame(j)
|
||||
ref = local[index[1]] - local[index[0]]
|
||||
q = retarget.retarget(ref)
|
||||
robot.compute_forward_kinematics(q)
|
||||
base = robot.get_link_pose(base_id)[:3, 3]
|
||||
tips = np.array([robot.get_link_pose(k)[:3, 3] for k in tip_ids])
|
||||
qs.append(q); targets.append(ref * cfg.scaling_factor + base); actual.append(tips)
|
||||
codes.append(retarget.optimizer.opt.last_optimize_result())
|
||||
if i % 200 == 0:
|
||||
print(f'retarget {i}/{len(joints)}', flush=True)
|
||||
qpos, targets, actual = map(np.array, (qs, targets, actual))
|
||||
elapsed = time.monotonic() - start
|
||||
limits = robot.joint_limits
|
||||
assert np.isfinite(qpos).all()
|
||||
assert np.all(qpos >= limits[:, 0] - 1e-6) and np.all(qpos <= limits[:, 1] + 1e-6)
|
||||
np.savez_compressed(OUT / 'leap_right_qpos.npz', qpos=qpos, joint_names=retarget.joint_names,
|
||||
fps=30, time=np.arange(len(qpos))/30, target_tips=targets,
|
||||
actual_tips=actual, joint_limits=limits, source_track=human['source_track'])
|
||||
np.savetxt(OUT / 'leap_right_qpos.csv', np.c_[np.arange(len(qpos))/30, qpos], delimiter=',',
|
||||
header=','.join(['time_s'] + retarget.joint_names), comments='')
|
||||
errors = np.linalg.norm(actual-targets, axis=2)*1000
|
||||
stats = dict(frames=len(qpos), fps=30, dof=qpos.shape[1], finite=True, joint_limits_pass=True,
|
||||
retarget_seconds=elapsed, tip_error_mean_mm=float(errors.mean()),
|
||||
tip_error_p95_mm=float(np.percentile(errors,95)),
|
||||
tip_error_by_finger_mm=dict(zip(['thumb','index','middle','ring'],errors.mean(0).tolist())),
|
||||
max_frame_joint_step_rad=float(np.max(np.abs(np.diff(qpos,axis=0)))),
|
||||
optimizer_status_counts={str(k):int(v) for k,v in zip(*np.unique(codes,return_counts=True))},
|
||||
scope='Fixed-wrist kinematic replay; no object, contact, dynamics, or real-robot validation.')
|
||||
(OUT / 'validation.json').write_text(json.dumps(stats, indent=2))
|
||||
print(json.dumps(stats, indent=2), flush=True)
|
||||
if args.no_render:
|
||||
return
|
||||
model = load_scene(ASSETS / 'leap_hand/leap_hand_right.urdf')
|
||||
data = mujoco.MjData(model)
|
||||
addresses = [model.jnt_qposadr[mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_JOINT,n)] for n in retarget.joint_names]
|
||||
assert len(set(addresses)) == len(addresses) == model.nq
|
||||
tip_bodies = [mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_BODY,n) for n in cfg.target_task_link_names]
|
||||
assert min(tip_bodies) >= 0
|
||||
fk_errors = []
|
||||
for i in np.linspace(0,len(qpos)-1,min(20,len(qpos)),dtype=int):
|
||||
data.qpos[addresses] = qpos[i]
|
||||
mujoco.mj_forward(model,data)
|
||||
fk_errors.append(np.max(np.abs(data.xpos[tip_bodies]-actual[i])))
|
||||
assert max(fk_errors) < 1e-5, f'Pinocchio/MuJoCo FK mismatch: {max(fk_errors)}'
|
||||
stats['pinocchio_mujoco_max_fk_error_m'] = float(max(fk_errors))
|
||||
renderer = mujoco.Renderer(model, height=480, width=640)
|
||||
cameras=[]
|
||||
for azimuth in [135, 225]:
|
||||
cam=mujoco.MjvCamera(); cam.lookat[:]=model.stat.center;cam.distance=.65;cam.azimuth=azimuth;cam.elevation=15
|
||||
cameras.append(cam)
|
||||
option=mujoco.MjvOption()
|
||||
option.geomgroup[0]=0 # URDF collision geometry; show visual group 1 only.
|
||||
writer=imageio.get_writer(OUT/'leap_right_replay.mp4',fps=30,codec='libx264',quality=8)
|
||||
try:
|
||||
for i,q in enumerate(qpos):
|
||||
data.qpos[addresses]=q; mujoco.mj_forward(model,data)
|
||||
panels=[]
|
||||
for cam in cameras:
|
||||
renderer.update_scene(data,camera=cam,scene_option=option)
|
||||
for p in targets[i]:
|
||||
scene=renderer.scene
|
||||
mujoco.mjv_initGeom(scene.geoms[scene.ngeom],mujoco.mjtGeom.mjGEOM_SPHERE,
|
||||
np.array([.004]*3),p,np.eye(3).ravel(),np.array([1,.25,.1,1]))
|
||||
scene.ngeom+=1
|
||||
panels.append(renderer.render().copy())
|
||||
img=Image.fromarray(np.concatenate(panels,axis=1)); draw=ImageDraw.Draw(img)
|
||||
draw.rectangle([0,0,1280,46],fill=(20,25,32))
|
||||
draw.text((12,8),f'LEAP right | frame {i+1}/{len(qpos)} | red dots: scaled human fingertips | fixed wrist',fill='white')
|
||||
draw.text((12,26),f'Mean fingertip error: {errors[i].mean():.1f} mm | kinematic replay',fill='white')
|
||||
writer.append_data(np.asarray(img))
|
||||
if i in [0,len(qpos)//2,len(qpos)-1]: img.save(OUT/f'preview_{i:04d}.png')
|
||||
if i % 200==0: print(f'render {i}/{len(qpos)}',flush=True)
|
||||
finally:
|
||||
writer.close(); renderer.close()
|
||||
stats['rendered_frames']=len(qpos)
|
||||
(OUT/'validation.json').write_text(json.dumps(stats,indent=2))
|
||||
print('COMPLETE',flush=True)
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,44 @@
|
||||
"""Resume the requested egocentric video inference with existing local models."""
|
||||
import json
|
||||
import os
|
||||
from pathlib import Path
|
||||
import subprocess
|
||||
import time
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
OUT = ROOT / 'output/15886123_handflow'
|
||||
OUT.mkdir(parents=True, exist_ok=True)
|
||||
env = os.environ.copy()
|
||||
env.update(PYTHONPATH=str(ROOT), LD_LIBRARY_PATH='', OMP_NUM_THREADS='4',
|
||||
MKL_NUM_THREADS='4', OPENBLAS_NUM_THREADS='4',
|
||||
TORCH_HOME=str(ROOT / '.torch_cache'), HF_HOME=str(ROOT / '.hf_cache'),
|
||||
HF_HUB_OFFLINE='1', TRANSFORMERS_OFFLINE='1',
|
||||
TORCH_FORCE_NO_WEIGHTS_ONLY_LOAD='1', PYTHONUNBUFFERED='1',
|
||||
VIPE_PYTHON=str(ROOT / '.venv/bin/python'),
|
||||
HAMER_CKPT=str(ROOT / 'third_party/hamer/_DATA/hamer_ckpts/checkpoints/hamer.ckpt'),
|
||||
DETECTOR_CKPT=str(ROOT / 'weights/detector.pt'),
|
||||
MANO_ROOT=str(ROOT / 'third_party/hamer/_DATA/data/mano'),
|
||||
HANDFLOW_NORMALIZATION_STATS=str(ROOT / 'weights/normalization_stats.npz'))
|
||||
cmd = [str(ROOT / '.venv/bin/python'), '-u', 'scripts/demo.py',
|
||||
'--input', '/home/timessage/下载/15886123.mp4',
|
||||
'--fm_ckpt', str(ROOT / 'weights/handflow_denoiser.pt'),
|
||||
'--intrinsics', 'auto', '--side', 'right', '--rotation_smooth_window', '5',
|
||||
'--output_dir', str(OUT), '--save_npz', str(OUT / 'handflow_results.npz')]
|
||||
|
||||
def status(stage, **extra):
|
||||
(OUT / 'status.json').write_text(json.dumps(dict(stage=stage, time=time.strftime('%Y-%m-%d %H:%M:%S'), **extra), indent=2))
|
||||
print(stage, flush=True)
|
||||
|
||||
try:
|
||||
for attempt in range(2):
|
||||
if (OUT / 'handflow_results.npz').exists():
|
||||
break
|
||||
status('INFERENCE', pass_index=attempt + 1, command=cmd)
|
||||
with (OUT / f'inference_pass_{attempt + 1}.log').open('a') as log:
|
||||
subprocess.run(cmd, cwd=ROOT, env=env, stdout=log, stderr=subprocess.STDOUT, check=True)
|
||||
if not (OUT / 'handflow_results.npz').exists():
|
||||
raise RuntimeError('Inference exited without final arrays after cache restart')
|
||||
status('INFERENCE_FINISHED_PENDING_VALIDATION')
|
||||
except Exception as exc:
|
||||
status('FAILED', error=str(exc))
|
||||
raise
|
||||
@@ -0,0 +1,10 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
SPIDER_WORKSPACE="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
export PYTHONPATH=""
|
||||
export LD_LIBRARY_PATH=""
|
||||
export WARP_CACHE_PATH="$SPIDER_WORKSPACE/.spider_cache/warp"
|
||||
export TORCHINDUCTOR_CACHE_DIR="$SPIDER_WORKSPACE/.spider_cache/torchinductor"
|
||||
export MUJOCO_GL="${MUJOCO_GL:-osmesa}"
|
||||
cd "$SPIDER_WORKSPACE/third_party/spider"
|
||||
exec "$SPIDER_WORKSPACE/.spider/bin/python" examples/run_mjwp_fast.py "$@"
|
||||
@@ -0,0 +1,18 @@
|
||||
"""Run upstream SPIDER contact guidance on the prepared L20 task."""
|
||||
import os
|
||||
from pathlib import Path
|
||||
ROOT=Path(__file__).resolve().parents[1]
|
||||
os.environ['WARP_CACHE_PATH']=str(ROOT/'.spider_cache/warp')
|
||||
os.environ['TORCHINDUCTOR_CACHE_DIR']=str(ROOT/'.spider_cache/torchinductor')
|
||||
os.environ.setdefault('MUJOCO_GL','osmesa')
|
||||
os.environ.setdefault('OMP_NUM_THREADS','4')
|
||||
import sys,json,importlib.util
|
||||
sys.path.insert(0,str(ROOT/'third_party/spider'))
|
||||
import torch
|
||||
torch.set_num_threads(4)
|
||||
from spider.config import Config
|
||||
spec=importlib.util.spec_from_file_location('spider_original_runner',ROOT/'third_party/spider/examples/run_mjwp.py')
|
||||
module=importlib.util.module_from_spec(spec);sys.modules[spec.name]=module;spec.loader.exec_module(module)
|
||||
config=Config(**json.loads((ROOT/'output/spider_l20_contact/config.json').read_text()))
|
||||
module.main(config)
|
||||
print('SPIDER_RUN_COMPLETE',flush=True)
|
||||
@@ -0,0 +1,39 @@
|
||||
"""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))
|
||||
@@ -0,0 +1,149 @@
|
||||
"""Offline whole-sequence temporal regularization with explicit distortion budgets."""
|
||||
import argparse
|
||||
import json
|
||||
import shutil
|
||||
from pathlib import Path
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
import mujoco
|
||||
import numpy as np
|
||||
from scipy import sparse
|
||||
from scipy.sparse.linalg import splu
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
|
||||
|
||||
def main():
|
||||
p = argparse.ArgumentParser()
|
||||
p.add_argument('--source', type=Path, default=ROOT/'output/l20_15886123')
|
||||
p.add_argument('--output', type=Path, default=ROOT/'output/l20_15886123_stable')
|
||||
a = p.parse_args()
|
||||
a.output.mkdir(parents=True, exist_ok=True)
|
||||
with np.load(a.source/'motion.npz') as f:
|
||||
m = {k:f[k] for k in f.files}
|
||||
n = len(m['qpos'])
|
||||
D2 = sparse.diags([np.ones(n-2), -2*np.ones(n-2), np.ones(n-2)], [0,1,2], shape=(n-2,n),format='csc')
|
||||
D3 = sparse.diags([-np.ones(n-3),3*np.ones(n-3),-3*np.ones(n-3),np.ones(n-3)], [0,1,2,3],shape=(n-3,n),format='csc')
|
||||
eye = sparse.eye(n,format='csc')
|
||||
names, active = list(m['joint_names']), list(m['active_joint_names'])
|
||||
ai = [names.index(k) for k in active]
|
||||
original = ET.parse(ROOT/'third_party/l20_assets/L20/RIGHT/linkerhand_g20_right.urdf').getroot()
|
||||
limits = {j.get('name'):np.array([float(j.find('limit').get(k)) for k in ['lower','upper']]) for j in original.findall('joint') if j.get('type')!='fixed'}
|
||||
low, high = np.array([limits[k] for k in active]).T.copy()
|
||||
mimic = {}
|
||||
for j in original.findall('joint'):
|
||||
mi = j.find('mimic')
|
||||
if mi is None:
|
||||
continue
|
||||
par, sc, off = mi.get('joint'),float(mi.get('multiplier','1')),float(mi.get('offset','0'))
|
||||
mimic[j.get('name')] = par,sc,off
|
||||
lo,hi = np.sort((limits[j.get('name')]-off)/sc)
|
||||
k = active.index(par)
|
||||
low[k],high[k] = max(low[k],lo),min(high[k],hi)
|
||||
|
||||
def smooth(x, weight, bounds=None):
|
||||
# Sample-domain objective ||x-y||² + ||D²x||² + weight*||D³x||².
|
||||
A = eye + D2.T@D2 + weight*(D3.T@D3)
|
||||
result = splu(A).solve(x)
|
||||
if bounds is not None:
|
||||
lo,hi = bounds
|
||||
# Explicit post-solve projection; evaluate smoothness after projection.
|
||||
# This is not claimed to solve the box-constrained temporal QP exactly.
|
||||
result = np.clip(result,lo,hi)
|
||||
return result
|
||||
|
||||
def d2(x):
|
||||
return float(np.sqrt(np.mean(np.diff(x[30:],n=2,axis=0)**2)))
|
||||
|
||||
rot = Rotation.from_quat(m['wrist_quat_wxyz'][:,[1,2,3,0]])
|
||||
rv = (rot[0].inv()*rot).as_rotvec()
|
||||
assert np.linalg.norm(rv,axis=1).max() < np.pi/2
|
||||
choices, sweep = {}, {}
|
||||
# Budgets are engineering choices relative to prior output, not true-motion tolerances.
|
||||
components = dict(fingers=(m['qpos'][:,ai],(low,high),np.deg2rad(8)),
|
||||
wrist_position=(m['wrist_pos'],None,.020),
|
||||
wrist_rotation=(rv,None,np.deg2rad(6)))
|
||||
for key,(x,bounds,budget) in components.items():
|
||||
rows, candidates = [], []
|
||||
for weight in [100.,300.,1000.,3000.,10000.,30000.]:
|
||||
y = smooth(x,weight,bounds)
|
||||
if key=='fingers':
|
||||
dev = float(np.abs(y-x).max())
|
||||
elif key=='wrist_rotation':
|
||||
dev = float(np.linalg.norm((rot.inv()*(rot[0]*Rotation.from_rotvec(y))).as_rotvec(),axis=1).max())
|
||||
else:
|
||||
dev = float(np.linalg.norm(y-x,axis=1).max())
|
||||
ratio = d2(y)/d2(x)
|
||||
row = dict(weight_d2=1.,weight_d3=weight,max_deviation=dev,budget=float(budget),
|
||||
second_difference_ratio=ratio,within_budget=bool(dev<=budget))
|
||||
rows.append(row)
|
||||
if dev<=budget:
|
||||
candidates.append((ratio,y,row))
|
||||
assert candidates, key
|
||||
_,chosen,row = min(candidates,key=lambda v:v[0])
|
||||
choices[key] = chosen
|
||||
sweep[key] = dict(selected=row,candidates=rows)
|
||||
print(key,row,flush=True)
|
||||
before = {k:m[k].copy() for k in ['qpos','wrist_pos','wrist_quat_wxyz','actual']}
|
||||
q = m['qpos'].copy()
|
||||
q[:,ai] = choices['fingers']
|
||||
for name,(parent,sc,off) in mimic.items():
|
||||
q[:,names.index(name)] = sc*q[:,names.index(parent)]+off
|
||||
m['qpos'] = q
|
||||
m['active_qpos'] = q[:,ai]
|
||||
m['wrist_pos'] = choices['wrist_position']
|
||||
newrot = rot[0]*Rotation.from_rotvec(choices['wrist_rotation'])
|
||||
m['wrist_quat_wxyz'] = newrot.as_quat()[:,[3,0,1,2]]
|
||||
for k,v in before.items():
|
||||
m['before_stabilization_'+k] = v
|
||||
m['stabilization_method'] = 'whole-sequence data fidelity + second/third difference regularization; component distortion budgets'
|
||||
m['stabilization_source'] = str(a.source.resolve())
|
||||
for filename in ['l20_moving.xml','l20_right.xml','l20_dex.urdf','dex_config.json']:
|
||||
shutil.copy2(a.source/filename,a.output/filename)
|
||||
model = mujoco.MjModel.from_xml_path(str(a.output/'l20_right.xml'))
|
||||
data = mujoco.MjData(model)
|
||||
addr = [model.jnt_qposadr[model.joint(k).id] for k in names]
|
||||
sites = [model.site(f'landmark_{i:02d}').id for i in range(21)]
|
||||
actual = []
|
||||
for row in q:
|
||||
data.qpos[addr] = row
|
||||
mujoco.mj_forward(model,data)
|
||||
actual.append(data.site_xpos[sites].copy())
|
||||
m['actual'] = np.array(actual)
|
||||
np.savez_compressed(a.output/'motion.npz',**m)
|
||||
np.savetxt(a.output/'trajectory.csv',np.c_[m['time'],m['wrist_pos'],m['wrist_quat_wxyz'],q],delimiter=',',
|
||||
header=','.join(['time_s','wrist_x','wrist_y','wrist_z','qw','qx','qy','qz']+names),comments='')
|
||||
lim = np.array([limits[k] for k in names])
|
||||
violation = float(max(np.maximum(lim[:,0]-q,0).max(),np.maximum(q-lim[:,1],0).max()))
|
||||
assert violation < 1e-9
|
||||
mimic_error = max(float(np.abs(q[:,names.index(k)]-sc*q[:,names.index(par)]-off).max()) for k,(par,sc,off) in mimic.items())
|
||||
assert mimic_error < 1e-10
|
||||
|
||||
def metrics(q,pos,quat,actual):
|
||||
r = Rotation.from_quat(quat[:,[1,2,3,0]])[30:]
|
||||
w = (r[:-1].inv()*r[1:]).as_rotvec()
|
||||
tips = np.linalg.norm(actual[:,[4,8,12,16,20]]-m['targets'][:,[4,8,12,16,20]],axis=-1)*1000
|
||||
return dict(joint_second_difference_rms_deg=np.rad2deg(d2(q)),
|
||||
joint_max_step_deg=float(np.rad2deg(np.abs(np.diff(q[30:],axis=0)).max())),
|
||||
wrist_second_difference_rms_mm=d2(pos)*1000,
|
||||
wrist_max_step_mm=float(np.linalg.norm(np.diff(pos[30:],axis=0),axis=1).max()*1000),
|
||||
wrist_rotation_velocity_difference_rms_deg=float(np.rad2deg(np.sqrt(np.mean(np.diff(w,axis=0)**2)))),
|
||||
wrist_rotation_max_step_deg=float(np.rad2deg(np.linalg.norm(w,axis=1).max())),
|
||||
wrist_range_m=np.ptp(pos,axis=0).tolist(),
|
||||
tip_error_mean_mm=float(tips.mean()),tip_error_p95_mm=float(np.percentile(tips,95)))
|
||||
report = dict(frames=n,fps=float(m['fps']),evaluation_start_frame=30,
|
||||
before=metrics(**dict(q=before['qpos'],pos=before['wrist_pos'],quat=before['wrist_quat_wxyz'],actual=before['actual'])),
|
||||
after=metrics(q,m['wrist_pos'],m['wrist_quat_wxyz'],m['actual']),
|
||||
selection=sweep,urdf_limit_violation_rad=violation,linear_mimic_error_rad=mimic_error,
|
||||
max_joint_change_deg=float(np.rad2deg(np.abs(q-before['qpos']).max())),
|
||||
max_wrist_change_mm=float(np.linalg.norm(m['wrist_pos']-before['wrist_pos'],axis=1).max()*1000),
|
||||
max_wrist_rotation_change_deg=float(np.rad2deg(np.linalg.norm((rot.inv()*newrot).as_rotvec(),axis=1).max())),
|
||||
uses_future_frames=True,boundary='Lower temporal derivatives do not prove true-motion accuracy; no contact/dynamics validation')
|
||||
assert all(np.isfinite(m[k]).all() for k in ['qpos','wrist_pos','wrist_quat_wxyz','actual'])
|
||||
(a.output/'stabilization_validation.json').write_text(json.dumps(report,indent=2))
|
||||
print(json.dumps({k:v for k,v in report.items() if k!='selection'},indent=2))
|
||||
|
||||
|
||||
if __name__=='__main__':
|
||||
main()
|
||||
@@ -0,0 +1,18 @@
|
||||
"""Decode and verify the complete exported LEAP replay."""
|
||||
import json
|
||||
import subprocess
|
||||
from pathlib import Path
|
||||
|
||||
root = Path(__file__).resolve().parents[1] / 'output/dex_2047635068'
|
||||
video = root / 'leap_right_replay.mp4'
|
||||
result = subprocess.run(['ffprobe', '-v', 'error', '-select_streams', 'v:0',
|
||||
'-count_frames', '-show_entries',
|
||||
'stream=width,height,avg_frame_rate,nb_read_frames',
|
||||
'-of', 'json', str(video)], check=True, capture_output=True, text=True)
|
||||
stream = json.loads(result.stdout)['streams'][0]
|
||||
assert int(stream['nb_read_frames']) == 1333
|
||||
assert stream['avg_frame_rate'] == '30/1'
|
||||
assert (stream['width'], stream['height']) == (1280, 480)
|
||||
subprocess.run(['ffmpeg', '-v', 'error', '-xerror', '-i', str(video), '-f', 'null', '-'], check=True)
|
||||
(root / 'video_validation.json').write_text(json.dumps(dict(stream=stream, full_decode_pass=True), indent=2))
|
||||
print('PASS: 1333 frames, 30 fps, 1280x480, full decode')
|
||||
@@ -0,0 +1,32 @@
|
||||
"""Validate full registered trajectory and decode both deliverable videos."""
|
||||
import json, subprocess
|
||||
import numpy as np
|
||||
from l20_calibrated import Calibration, ROOT
|
||||
|
||||
out=ROOT/'output/l20_2047635068/bottle_grasp'
|
||||
m=np.load(out/'registered_motion.npz'); c=Calibration()
|
||||
names=list(m['joint_names']); q=m['qpos']
|
||||
limits=np.array([c.urdf_limits[n] for n in names])
|
||||
coupling=max(float(np.max(np.abs(q[:,names.index(n)]-np.interp(q[:,names.index(a)],x,y)))) for n,(a,x,y) in c.curves.items())
|
||||
report=dict(frames=len(q),finite=all(bool(np.isfinite(m[k]).all()) for k in ['qpos','wrist_pos','wrist_quat_wxyz','bottle_pos','bottle_quat_wxyz']),
|
||||
limit_violations=int(np.sum((q<limits[:,0]-1e-8)|(q>limits[:,1]+1e-8))),
|
||||
max_passive_coupling_error_rad=coupling,
|
||||
max_penetration_mm=float(m['penetration_m'].max()*1000),
|
||||
frames_above_1_5_mm=int(np.sum(m['penetration_m']>.0015)),
|
||||
wrist_axis_range_m=np.ptp(m['wrist_pos'],axis=0).tolist(),
|
||||
max_joint_step_rad=float(np.abs(np.diff(q,axis=0)).max()),
|
||||
frames_with_joint_step_above_0_2_rad=int(np.sum(np.max(np.abs(np.diff(q,axis=0)),axis=1)>.2)),
|
||||
max_wrist_step_m=float(np.linalg.norm(np.diff(m['wrist_pos'],axis=0),axis=1).max()),
|
||||
max_command_quantization_error_rad=float(np.max(np.abs(np.array([c.decode(cmd,names) for cmd in m['command_u8']])-q))),videos={})
|
||||
for filename,expected in [('contact_registration.mp4',len(q)),('physics_test.mp4',len(np.load(out/'physics_motion.npz')['qpos']))]:
|
||||
path=out/filename
|
||||
probe=json.loads(subprocess.check_output(['ffprobe','-v','error','-count_frames','-select_streams','v:0','-show_entries','stream=width,height,nb_read_frames,r_frame_rate','-of','json',str(path)]))['streams'][0]
|
||||
subprocess.run(['ffmpeg','-v','error','-i',str(path),'-f','null','-'],check=True)
|
||||
assert int(probe['nb_read_frames'])==expected
|
||||
report['videos'][filename]=dict(probe,full_decode_passed=True)
|
||||
report['geometry_checks_passed']=report['finite'] and report['limit_violations']==0 and coupling<1e-8 and report['frames_above_1_5_mm']==0
|
||||
report['physical_retention_passed']=json.loads((out/'physics_validation.json').read_text())['retained']
|
||||
report['execution_ready']=False
|
||||
report['execution_limitations']=['Local pose jumps remain; no joint velocity constrained optimization','Free object retention test failed','Object trajectory is an inferred registration target']
|
||||
(out/'delivery_validation.json').write_text(json.dumps(report,indent=2))
|
||||
print(json.dumps(report,indent=2))
|
||||
@@ -0,0 +1,29 @@
|
||||
"""Independent narrow-phase contact, coupling, floor and pose verification."""
|
||||
import os
|
||||
os.environ.setdefault('MUJOCO_GL','osmesa')
|
||||
from pathlib import Path
|
||||
import json,xml.etree.ElementTree as E
|
||||
import numpy as np,mujoco
|
||||
OUT=Path(__file__).resolve().parents[1]/'output/l20_full_replay'
|
||||
x=np.load(OUT/'motion.npz');m=mujoco.MjModel.from_xml_path(str(OUT/'scene.xml'));d=mujoco.MjData(m)
|
||||
penetration=[];counts=[];floor_count=0;coupling=0.;limit=0.
|
||||
for row in x['qpos']:
|
||||
d.qpos[:]=row;mujoco.mj_forward(m,d)
|
||||
ds=[]
|
||||
for c in d.contact:
|
||||
types={int(m.geom_contype[c.geom1]),int(m.geom_contype[c.geom2])}
|
||||
if types=={1,2}:ds.append(float(c.dist))
|
||||
if 4 in types:floor_count+=1
|
||||
penetration.append(max(0,-min(ds,default=0)));counts.append(len(ds))
|
||||
for eq in range(m.neq):
|
||||
j1,j2=m.eq_obj1id[eq],m.eq_obj2id[eq];v=row[m.jnt_qposadr[j2]]
|
||||
expected=sum(m.eq_data[eq,k]*v**k for k in range(5))
|
||||
coupling=max(coupling,abs(row[m.jnt_qposadr[j1]]-expected))
|
||||
for j in range(m.njnt):
|
||||
if m.jnt_limited[j]:
|
||||
v=row[m.jnt_qposadr[j]];limit=max(limit,m.jnt_range[j,0]-v,v-m.jnt_range[j,1])
|
||||
r=json.loads((OUT/'validation.json').read_text());r['geom_distance_max_penetration_mm']=float(np.max(x['penetration_m'])*1000)
|
||||
r.update(max_all_penetration_mm=float(max(penetration)*1000),max_holding_penetration_mm=float(max(penetration[160:])*1000),holding_frames_with_contact=int(np.count_nonzero(counts[160:])),holding_frames=len(counts)-160,floor_contacts=floor_count,linear_mimic_max_error_rad=float(coupling),joint_limit_violation_rad=float(limit),contact_verification='mj_forward contact manifold independently checked every frame; geomDistance has isolated inconsistent spikes')
|
||||
(OUT/'validation.json').write_text(json.dumps(r,indent=2));np.savez_compressed(OUT/'contact_verification.npz',penetration_m=penetration,contact_count=counts)
|
||||
assert np.isfinite(x['qpos']).all() and coupling<1e-8 and limit<1e-8 and floor_count==0
|
||||
print(json.dumps(r,indent=2))
|
||||
@@ -0,0 +1,36 @@
|
||||
"""Check SPIDER imports, dependency versions and a two-world GPU contact simulation."""
|
||||
import os
|
||||
from pathlib import Path
|
||||
ROOT=Path(__file__).resolve().parents[1]
|
||||
os.environ['WARP_CACHE_PATH']=str(ROOT/'.spider_cache/warp')
|
||||
os.environ['TORCHINDUCTOR_CACHE_DIR']=str(ROOT/'.spider_cache/torchinductor')
|
||||
os.environ.setdefault('MUJOCO_GL','osmesa')
|
||||
import json,sys,importlib,importlib.metadata as metadata,tomllib
|
||||
import numpy as np
|
||||
from packaging.requirements import Requirement
|
||||
import torch,mujoco,mujoco_warp as mw,warp as wp
|
||||
|
||||
report={'python':sys.version,'compatibility_mode':'Python 3.11 with reused Torch; upstream declares Python >=3.12','versions':{},'imports':{},'requirements':{}}
|
||||
reqs=tomllib.loads((ROOT/'third_party/spider/pyproject.toml').read_text())['project']['dependencies']
|
||||
for text in reqs:
|
||||
r=Requirement(text);v=metadata.version(r.name);report['versions'][r.name]=v;report['requirements'][r.name]=v in r.specifier
|
||||
for name in ['spider.config','spider.simulators.mjwp','spider.optimizers.sampling_fast','spider.viewers','spider.preprocess.ik','open3d','pymeshlab','mink']:
|
||||
importlib.import_module(name);report['imports'][name]=True
|
||||
report['torch_path']=torch.__file__;report['cuda_available']=torch.cuda.is_available()
|
||||
assert report['cuda_available'],'GPU test requires access outside the device-restricted sandbox'
|
||||
report['gpu']=torch.cuda.get_device_name(0)
|
||||
assert float((torch.ones(8,device='cuda')*2).sum())==16
|
||||
wp.init()
|
||||
xml='<mujoco><option timestep="0.002" iterations="5"/><worldbody><geom type="plane" size="1 1 .1"/><body pos="0 0 .105"><freejoint/><geom type="sphere" size=".1" mass=".1"/></body></worldbody></mujoco>'
|
||||
m=mujoco.MjModel.from_xml_string(xml);d=mujoco.MjData(m);mujoco.mj_forward(m,d)
|
||||
with wp.ScopedDevice('cuda:0'):
|
||||
wm=mw.put_model(m);wd=mw.put_data(m,d,nworld=2,nconmax=32,njmax=128)
|
||||
for _ in range(60):mw.step(wm,wd)
|
||||
wp.synchronize()
|
||||
q=wd.qpos.numpy();time=wd.time.numpy();contacts=wd.nacon.numpy()
|
||||
assert np.isfinite(q).all() and np.all(q[:,2]>.085) and np.all(q[:,2]<.105),q
|
||||
assert np.allclose(time,.12,atol=1e-5),time
|
||||
report['gpu_physics']={'worlds':2,'steps':60,'time_s':time.tolist(),'sphere_height_m':q[:,2].tolist(),'contacts':contacts.tolist(),'passed':True}
|
||||
assert all(report['requirements'].values())
|
||||
out=ROOT/'output/spider_install';out.mkdir(exist_ok=True)
|
||||
(out/'validation.json').write_text(json.dumps(report,indent=2));print(json.dumps(report,indent=2),flush=True)
|
||||
@@ -0,0 +1,81 @@
|
||||
"""Independently validate saved video/robot outputs, including full video decode."""
|
||||
import argparse
|
||||
import json
|
||||
from pathlib import Path
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
import cv2
|
||||
import mujoco
|
||||
import numpy as np
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
|
||||
|
||||
def main():
|
||||
p = argparse.ArgumentParser()
|
||||
p.add_argument('--inference', type=Path, required=True)
|
||||
p.add_argument('--retarget', type=Path, required=True)
|
||||
a = p.parse_args()
|
||||
h = np.load(a.inference / 'handflow_results.npz')
|
||||
m = np.load(a.retarget / 'motion.npz')
|
||||
count, fps = len(h['pose']), float(h['fps'])
|
||||
assert len(m['qpos']) == count and float(m['fps']) == fps
|
||||
names = list(m['joint_names'])
|
||||
model = mujoco.MjModel.from_xml_path(str(a.retarget / 'l20_moving.xml'))
|
||||
data = mujoco.MjData(model)
|
||||
addr = [model.jnt_qposadr[model.joint(n).id] for n in names]
|
||||
wrist = model.jnt_qposadr[model.joint('wrist_free').id]
|
||||
sites = [model.site(f'landmark_{i:02d}').id for i in range(21)]
|
||||
limits = []
|
||||
mimic_error = 0.
|
||||
urdf = ET.parse(ROOT / 'third_party/l20_assets/L20/RIGHT/linkerhand_g20_right.urdf').getroot()
|
||||
for n in names:
|
||||
j = urdf.find(f"joint[@name='{n}']")
|
||||
limits.append([float(j.find('limit').get(k)) for k in ['lower','upper']])
|
||||
mi = j.find('mimic')
|
||||
if mi is not None:
|
||||
expected = m['qpos'][:,names.index(mi.get('joint'))] * float(mi.get('multiplier','1')) + float(mi.get('offset','0'))
|
||||
mimic_error = max(mimic_error, float(np.abs(expected-m['qpos'][:,names.index(n)]).max()))
|
||||
limits = np.array(limits)
|
||||
violation = float(max(np.maximum(limits[:,0]-m['qpos'],0).max(),np.maximum(m['qpos']-limits[:,1],0).max()))
|
||||
fk_error = 0.
|
||||
from scipy.spatial.transform import Rotation
|
||||
rotations = Rotation.from_quat(m['wrist_quat_wxyz'][:,[1,2,3,0]]).as_matrix()
|
||||
for t in range(count):
|
||||
data.qpos[addr] = m['qpos'][t]
|
||||
data.qpos[wrist:wrist+3] = m['wrist_pos'][t]
|
||||
data.qpos[wrist+3:wrist+7] = m['wrist_quat_wxyz'][t]
|
||||
mujoco.mj_forward(model,data)
|
||||
expected = m['actual'][t] @ rotations[t].T + m['wrist_pos'][t]
|
||||
fk_error = max(fk_error,float(np.abs(data.site_xpos[sites]-expected).max()))
|
||||
csv = np.loadtxt(a.retarget/'trajectory.csv',delimiter=',',skiprows=1)
|
||||
expected_csv = np.c_[m['time'],m['wrist_pos'],m['wrist_quat_wxyz'],m['qpos']]
|
||||
assert np.allclose(csv,expected_csv,atol=1e-12)
|
||||
assert np.isfinite(expected_csv).all() and violation < 1e-8 and mimic_error < 1e-10 and fk_error < 1e-5
|
||||
video_paths = [a.inference/'overlay.mp4',a.inference/'ortho_third_person.mp4',a.retarget/'l20_moving_replay.mp4']
|
||||
if (a.retarget/'comparison.mp4').exists():
|
||||
video_paths.append(a.retarget/'comparison.mp4')
|
||||
videos = []
|
||||
for path in video_paths:
|
||||
cap = cv2.VideoCapture(str(path))
|
||||
vf = cap.get(cv2.CAP_PROP_FPS)
|
||||
decoded = 0
|
||||
while True:
|
||||
ok, image = cap.read()
|
||||
if not ok:
|
||||
break
|
||||
assert image is not None
|
||||
decoded += 1
|
||||
cap.release()
|
||||
assert decoded == count and abs(vf-fps)<1e-3, (path,decoded,vf)
|
||||
videos.append(dict(path=str(path.resolve()),decoded_frames=decoded,fps=vf))
|
||||
report = dict(status='COMPLETE',frames=count,fps=fps,duration_seconds=count/fps,
|
||||
finite=True,urdf_limit_violation_rad=violation,linear_mimic_error_rad=mimic_error,
|
||||
independent_world_fk_max_error_m=fk_error,checked_frames=count,csv_matches_npz=True,
|
||||
videos=videos,boundary='Kinematic hand motion only; monocular estimates; no object or hardware validation')
|
||||
(a.retarget/'delivery_validation.json').write_text(json.dumps(report,indent=2))
|
||||
print(json.dumps(report,indent=2))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
Reference in New Issue
Block a user