liyang
ae28d55f81
Source: RGB-D -> Dyn-HaMR -> L20 retargeting -> FoundationPose -> reference repair -> SPIDER, documented in docs/PIPELINE_LATEST.md and docs/SETUP_AND_WEIGHTS.md. Adds FoundationPose and nvdiffrast upstream snapshots, requirements/pipeline_venv.txt and the FoundationPose weight manifest/downloader. Assets (Git LFS): weights/ (WiLoR detector, HandFlow denoiser, UniDepth-L), FoundationPose checkpoints, HaMeR checkpoint, Dyn-HaMR HMP model and BMC constraints, L20 URDF/meshes, the 20260915_171525 D405 recording and the two box CADs. MANO models are not redistributed (third_party/hamer/_DATA/data/mano/README.txt). Environments, caches and run outputs excluded. Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
57 lines
6.9 KiB
Python
57 lines
6.9 KiB
Python
"""Audit saved SPIDER states; render source, kinematic reference, and physical rollout."""
|
|
import os
|
|
os.environ.setdefault('MUJOCO_GL','osmesa')
|
|
from pathlib import Path
|
|
import json,re,sys
|
|
import numpy as np,mujoco,cv2,imageio.v2 as imageio
|
|
from scipy.spatial.transform import Rotation as R
|
|
ROOT=Path(__file__).resolve().parents[1];OUT=ROOT/'output/foundationpose_spider_20260915';TASK=OUT/'datasets/processed/current/l20/bimanual/boxes';TRIAL=TASK/'0'
|
|
r=np.load(TRIAL/'trajectory_kinematic_act.npz');a=np.load(TRIAL/'trajectory_mjwp_act.npz');q=a['qpos'].reshape(-1,66);ctrl=a['ctrl'].reshape(-1,56);times=a['time'].ravel();ref=np.load(OUT/'reference_video_rate.npz');fp=np.load(OUT/'foundationpose_objects.npz')
|
|
assert np.isfinite(q).all(),'Nonfinite SPIDER result; cannot render as valid motion'
|
|
m=mujoco.MjModel.from_xml_path(str(TASK/'scene_act.xml'));d=mujoco.MjData(m);dr=mujoco.MjData(m);assert not m.actuator_gainprm[-12:].any() and not m.actuator_biasprm[-12:].any()
|
|
# Evaluate at video times, preserving actual step timestamps rather than nominal array indices.
|
|
ix=np.argmin(abs(times[:,None]-ref['time'][None,:]),axis=0);qv=q[ix];bv=np.load(OUT/'reference_before_contact_fit.npz')['qpos'];sid=[m.site(f'{s}_hand_{f}_track').id for s in ['right','left'] for f in ['thumb','index','middle','ring','pinky']]
|
|
def metrics(rows):
|
|
pen=[];contacts=[];gaps=[];err=[[],[]];centererr=[[],[]];ang=[[],[]];lim=[];mim=[]
|
|
for f,row in enumerate(rows):
|
|
d.qpos[:]=row;mujoco.mj_forward(m,d);dr.qpos[:]=ref['qpos'][f];mujoco.mj_kinematics(m,dr)
|
|
selected=[c for c in d.contact if {int(m.geom_contype[c.geom[0]]),int(m.geom_contype[c.geom[1]])}=={1,2}]
|
|
pen.append(max([max(0.,-c.dist) for c in selected],default=0)*1000);contacts.append(len(selected))
|
|
mask=ref['contact'][f].astype(bool);ds=np.linalg.norm(d.site_xpos[sid]-ref['contact_pos'][f],axis=1);gaps.extend(ds[mask]*1000)
|
|
for k,s in enumerate(['right','left']):
|
|
oid=m.body(s+'_object').id;cs=m.site(s+'_object_track').id;centererr[k].append(np.linalg.norm(d.site_xpos[cs]-dr.site_xpos[cs])*1000);err[k].append(np.linalg.norm(d.xpos[oid]-dr.xpos[oid])*1000);ang[k].append(R.from_matrix(d.xmat[oid].reshape(3,3)@dr.xmat[oid].reshape(3,3).T).magnitude()*180/np.pi)
|
|
inds=np.flatnonzero(m.jnt_limited);v=row[m.jnt_qposadr[inds]];lim.append(max(np.maximum(m.jnt_range[inds,0]-v,0).max(),np.maximum(v-m.jnt_range[inds,1],0).max()))
|
|
for j in range(m.neq):
|
|
j1,j2=m.eq_obj1id[j],m.eq_obj2id[j];coef=m.eq_data[j,:5];expected=sum(coef[k]*row[m.jnt_qposadr[j2]]**k for k in range(5));mim.append(abs(row[m.jnt_qposadr[j1]]-expected))
|
|
result=dict(finite=bool(np.isfinite(rows).all()),sampled_video_frames=len(rows),hand_object_contact_frames=int(np.sum(np.array(contacts)>0)),max_hand_object_penetration_mm=float(np.max(pen)),median_frame_max_penetration_mm=float(np.median(pen)),inferred_contact_target_gap_mean_mm=float(np.mean(gaps)) if gaps else None,max_joint_limit_violation_rad=float(max(lim)),max_mimic_residual_rad=float(max(mim)))
|
|
for k,name in enumerate(['upper','lower']):
|
|
mask=fp[name+'_valid'];e=np.array(err[k]);aa=np.array(ang[k]);result[name]=dict(observed_frames=int(mask.sum()),observed_position_error_mean_mm=float(e[mask].mean()),observed_position_error_max_mm=float(e[mask].max()),observed_rotation_error_mean_deg=float(aa[mask].mean()),all_frames_position_error_mean_mm=float(e.mean()),observed_centroid_error_mean_mm=float(np.array(centererr[k])[mask].mean()),observed_centroid_error_max_mm=float(np.array(centererr[k])[mask].max()))
|
|
return result,np.array(pen),np.array(contacts)
|
|
report={};report['original_hands_new_objects'],_,_=metrics(bv);report['kinematic_contact_warmstart'],_,_=metrics(ref['qpos']);report['spider'],pen,counts=metrics(qv)
|
|
if (OUT/'independent_gpu_replay.npz').exists():
|
|
replay=np.load(OUT/'independent_gpu_replay.npz')
|
|
for key in replay.files:
|
|
rows=replay[key][ix]
|
|
report['independent_gpu_'+key]=metrics(rows)[0] if np.isfinite(rows).all() else {'finite':False}
|
|
report['independent_replay_integrity']=json.loads((OUT/'independent_gpu_replay.json').read_text())
|
|
report.update(full_steps=len(q),duration_s=float(times[-1]),zero_object_assistance=True,validity_note='Video-depth consistency is not external pose ground truth; lower reference after176 is held and excluded as a new target; either hand may contact red.',collision_note='Penetration measured using approximate convex-decomposed collision meshes, not exact CAD signed distance.',contact_note='Masks are geometric hypotheses, not measured contact labels.',all_states_finite=bool(np.isfinite(q).all()))
|
|
log=(OUT/'spider_full.log').read_text();report['completion_marker']='SPIDER_RUN_COMPLETE' in log;report['nonfinite_candidate_warning_count']=len(re.findall('NaNs or infs in rews',log))
|
|
report['tracking_20mm_gate_passed']=all(report['spider'][n]['observed_position_error_max_mm']<20 for n in ['upper','lower'])
|
|
(OUT/'validation.json').write_text(json.dumps(report,indent=2));np.savez_compressed(OUT/'comparison_motion.npz',original=bv,reference=ref['qpos'],spider=qv,time=ref['time'],spider_full_qpos=q,spider_full_ctrl=ctrl,spider_full_time=times)
|
|
np.savetxt(OUT/'spider_trajectory.csv',np.c_[times,q,ctrl],delimiter=',',header=','.join(['time_s']+[m.joint(i).name for i in range(m.njnt)]+['ctrl_'+m.actuator(i).name for i in range(m.nu)]),comments='')
|
|
if '--skip-render' in sys.argv:
|
|
print(json.dumps(report,indent=2));sys.exit(0)
|
|
m.vis.headlight.ambient[:]=.7;m.vis.quality.offsamples=1;renderer=mujoco.Renderer(m,height=360,width=640);opt=mujoco.MjvOption();opt.sitegroup[:]=0;opt.geomgroup[3:]=0;cid=m.camera('source_camera').id
|
|
cap=cv2.VideoCapture(str(ROOT/'docs/20260915_171525/color.mp4'));writer=imageio.get_writer(OUT/'original_foundationpose_spider.mp4',fps=30,codec='libx264',quality=8,macro_block_size=1)
|
|
for f in range(len(qv)):
|
|
ok,img=cap.read();assert ok;panels=[cv2.cvtColor(cv2.resize(img,(640,360)),cv2.COLOR_BGR2RGB)]
|
|
cam=ref['camera_world_from_cv'][f];m.cam_pos[cid]=cam[:3,3];quat=R.from_matrix(cam[:3,:3]@np.diag([1,-1,-1])).as_quat();m.cam_quat[cid]=quat[[3,0,1,2]]
|
|
for row in [ref['qpos'][f],qv[f]]:
|
|
d.qpos[:]=row;mujoco.mj_forward(m,d);renderer.update_scene(d,camera='source_camera',scene_option=opt);panels.append(renderer.render().copy())
|
|
panel=np.concatenate(panels,1);cv2.rectangle(panel,(0,0),(1920,45),(20,25,30),-1)
|
|
for j,label in enumerate(['Recorded RGB-D','FoundationPose + contact IK reference','Actual SPIDER: free objects, zero assistance']):cv2.putText(panel,label,(j*640+8,18),cv2.FONT_HERSHEY_SIMPLEX,.47,(255,255,255),1)
|
|
label=f'frame {f}/351 | red observed={bool(fp["upper_valid"][f])}, blue observed={bool(fp["lower_valid"][f])} | SPIDER penetration={pen[f]:.1f}mm, contacts={counts[f]}'
|
|
cv2.putText(panel,label,(8,38),cv2.FONT_HERSHEY_SIMPLEX,.45,(255,255,255),1);writer.append_data(panel)
|
|
if f in [0,80,160,240,351]:cv2.imwrite(str(OUT/f'comparison_{f:04d}.jpg'),cv2.cvtColor(panel,cv2.COLOR_RGB2BGR))
|
|
writer.close();renderer.close();cap.release();print(json.dumps(report,indent=2),flush=True)
|