Update to 2026-09-17 pipeline snapshot; add weights, L20 assets and recording via Git LFS

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>

scripts/align_collision_floor.py

@@ -0,0 +1,8 @@
+"""Remove initial CAD/table interpenetration before unassisted physics; record the offset."""
+from pathlib import Path
+import numpy as np,mujoco,xml.etree.ElementTree as E,json
+O=Path(__file__).resolve().parents[1]/'output/collision_fix_20260915';T=O/'datasets/processed/current/l20/bimanual/boxes';p=T/'scene_act.xml';m=mujoco.MjModel.from_xml_path(str(p));d=mujoco.MjData(m);d.qpos[:]=np.load(T/'0/trajectory_kinematic_act.npz')['qpos'][0];mujoco.mj_fwdPosition(m,d);z=[]
+for g in range(m.ngeom):
+ if m.geom_contype[g]!=2:continue
+ mid=m.geom_dataid[g];v=m.mesh_vert[m.mesh_vertadr[mid]:m.mesh_vertadr[mid]+m.mesh_vertnum[mid]];world=v@d.geom_xmat[g].reshape(3,3).T+d.geom_xpos[g];z.append(world[:,2].min())
+height=float(min(z)-.0005);tree=E.parse(p);floor=tree.getroot().find("worldbody/geom[@name='right_floor']");old=float(floor.get('pos').split()[2]);floor.set('pos',f'0 0 {height}');tree.write(p);report={'old_height_m':old,'new_height_m':height,'offset_m':height-old,'reason':'No object/table initial overlap. Simulation alignment correction, not a new measured plane; object poses unchanged.'};(O/'floor_alignment.json').write_text(json.dumps(report,indent=2));print(report)

scripts/audit_collision_fix.py

@@ -0,0 +1,42 @@
+"""Independent before/after full-link collision, limits, continuity, and CAD surface probes."""
+import os
+os.environ['OPENBLAS_NUM_THREADS']='1';os.environ['OMP_NUM_THREADS']='1'
+from pathlib import Path
+import json,numpy as np,mujoco,trimesh,open3d as o3d
+from scipy.spatial.transform import Rotation as R
+from red_box_distance import signed_distance
+ROOT=Path(__file__).resolve().parents[1];O=Path(os.environ.get('HF_FIX_OUT',ROOT/'output/collision_fix_20260915'));OLD=Path(os.environ.get('HF_FIX_OLD',ROOT/'output/foundationpose_spider_20260915'));T=O/'datasets/processed/current/l20/bimanual/boxes';m=mujoco.MjModel.from_xml_path(str(T/'scene_act.xml'));d=mujoco.MjData(m);ref=np.load(O/'reference_video_rate.npz');before=np.load(OLD/'reference_video_rate.npz');q=ref['qpos'];sites=[m.site(s+'_hand_'+f+'_track').id for s in ['right','left'] for f in ['thumb','index','middle','ring','pinky']]
+def audit(rows):
+ pen=[];gap=[];byhand=[[],[]];lim=[];mimic=[]
+ for f,row in enumerate(rows):
+  d.qpos[:]=row;mujoco.mj_fwdPosition(m,d);ds=[[],[]]
+  for c in d.contact:
+   g0,g1=map(int,c.geom)
+   if {int(m.geom_contype[g0]),int(m.geom_contype[g1])} not in ([{1,2},{1,4}] if os.environ.get('HF_TABLE','0')=='1' else [{1,2}]):continue
+   h=g0 if m.geom_contype[g0]==1 else g1;side=0 if m.geom(h).name.startswith('right_') else 1;ds[side].append(c.dist)
+  for j in range(2):byhand[j].append(max(0,-min(ds[j]))*1000 if ds[j] else 0.)
+  pen.append(max(byhand[0][-1],byhand[1][-1]));on=ref['contact'][f].astype(bool);gap.extend(np.linalg.norm(d.site_xpos[sites][on]-ref['contact_pos'][f][on],axis=1)*1000)
+  ids=np.flatnonzero(m.jnt_limited);v=row[m.jnt_qposadr[ids]];lim.append(max(np.maximum(m.jnt_range[ids,0]-v,0).max(),np.maximum(v-m.jnt_range[ids,1],0).max()))
+  for e in range(m.neq):
+   j1,j2=m.eq_obj1id[e],m.eq_obj2id[e];poly=m.eq_data[e,:5];x=row[m.jnt_qposadr[j2]];mimic.append(abs(row[m.jnt_qposadr[j1]]-sum(poly[k]*x**k for k in range(5))))
+ result=dict(frames=len(rows),all_finite=bool(np.isfinite(rows).all()),max_hand_object_penetration_mm=float(max(pen)),median_frame_max_penetration_mm=float(np.median(pen)),frames_over_0_5mm=int(np.sum(np.array(pen)>.5)),inferred_contact_gap_mean_mm=float(np.mean(gap)),inferred_contact_gap_p95_mm=float(np.percentile(gap,95)),max_joint_limit_violation_rad=float(max(lim)),max_mimic_error_rad=float(max(mimic)))
+ for i,(side,st) in enumerate([('right',0),('left',27)]):
+  result[side]={'max_penetration_mm':float(max(byhand[i])),'max_wrist_step_mm':float(np.linalg.norm(np.diff(rows[:,st:st+3],axis=0),axis=1).max()*1000),'max_wrist_rotation_step_deg':float((R.from_euler('XYZ',rows[:-1,st+3:st+6]).inv()*R.from_euler('XYZ',rows[1:,st+3:st+6])).magnitude().max()*180/np.pi)}
+ return result,np.array(pen)
+report={};report['before_same_new_collision_model'],bp=audit(before['qpos']);report['corrected_reference'],ap=audit(q)
+report['object_qpos_unchanged']=bool(np.array_equal(q[:,-12:],before['qpos'][:,-12:]));report['wrist_correction_mm']={s:{'median':float(np.median(np.linalg.norm(q[:,j:j+3]-before['qpos'][:,j:j+3],axis=1))*1000),'max':float(np.max(np.linalg.norm(q[:,j:j+3]-before['qpos'][:,j:j+3],axis=1))*1000)} for s,j in [('right',0),('left',27)]}
+# Sample the original visible link meshes, independently of the CoACD collision mesh.
+probes=[]
+for g in range(m.ngeom):
+ n=m.geom(g).name or ''
+ if '_visual_' not in n or not n.startswith(('right_','left_')):continue
+ mid=m.geom_dataid[g];v=m.mesh_vert[m.mesh_vertadr[mid]:m.mesh_vertadr[mid]+m.mesh_vertnum[mid]];faces=m.mesh_face[m.mesh_faceadr[mid]:m.mesh_faceadr[mid]+m.mesh_facenum[mid]];cent=v[faces].mean(1);pts=np.r_[v[np.linspace(0,len(v)-1,min(64,len(v)),dtype=int)],cent[np.linspace(0,len(cent)-1,min(64,len(cent)),dtype=int)]];probes.append((g,pts))
+report['cad_probe_scope']=f'{sum(len(p) for g,p in probes)} deterministic original-link surface samples per frame, all 352 frames; sampled test, not exhaustive mesh intersection.'
+for name,side in [('upper','right'),('lower','left')]:
+ mesh=trimesh.load(O/'collision_v2'/f'{name}_closed.ply');assert mesh.is_watertight;sc=o3d.t.geometry.RaycastingScene();sc.add_triangles(o3d.t.geometry.TriangleMesh.from_legacy(o3d.geometry.TriangleMesh(o3d.utility.Vector3dVector(mesh.vertices),o3d.utility.Vector3iVector(mesh.faces))));obj=m.body(side+'_object').id;values=[]
+ for row in q:
+  d.qpos[:]=row;mujoco.mj_kinematics(m,d);points=np.concatenate([v@d.geom_xmat[g].reshape(3,3).T+d.geom_xpos[g] for g,v in probes]);local=(points-d.xpos[obj])@d.xmat[obj].reshape(3,3);dist=signed_distance(sc,mesh,local);values.append(max(0,-dist.min())*1000)
+ report[name+'_exact_CAD_surface_probes']={'max_inside_mm':float(max(values)),'frames_over_1mm':int(np.sum(np.array(values)>1))};print(name,'probe',report[name+'_exact_CAD_surface_probes'],flush=True)
+if (T/'0/trajectory_mjwp_act.npz').exists() and np.load(T/'0/trajectory_mjwp_act.npz')['time'].max()>=ref['time'][-1]-.05:
+ a=np.load(T/'0/trajectory_mjwp_act.npz');rows=a['qpos'].reshape(-1,66);times=a['time'].ravel();ix=np.argmin(abs(times[:,None]-ref['time'][None,:]),axis=0);report['spider_physics'],_=audit(rows[ix]);report['spider_physics']['duration_s']=float(times[-1]);report['spider_physics']['steps']=len(rows)
+report['interpolation']=json.loads((O/'interpolation_validation.json').read_text());(O/'validation.json').write_text(json.dumps(report,indent=2));np.savez_compressed(O/'collision_comparison.npz',before=before['qpos'],after=q,time=ref['time'],before_penetration_mm=bp,after_penetration_mm=ap);print(json.dumps(report,indent=2))

scripts/audit_collision_physics.py

@@ -0,0 +1,16 @@
+"""Inspect every executed SPIDER state, using CPU collision detection independently."""
+import os
+os.environ['OPENBLAS_NUM_THREADS']='1';os.environ['OMP_NUM_THREADS']='1'
+from pathlib import Path
+import numpy as np,mujoco,json
+from scipy.spatial.transform import Rotation as R
+ROOT=Path(__file__).resolve().parents[1];O=Path(os.environ.get('SPIDER_TASK_OUT',str(ROOT/'output/collision_fix_20260915')));T=O/'datasets/processed/current/l20/bimanual/boxes';m=mujoco.MjModel.from_xml_path(str(T/'scene_act.xml'));d=mujoco.MjData(m);dr=mujoco.MjData(m);a=np.load(T/'0/trajectory_mjwp_act.npz');ref=np.load(T/'0/trajectory_kinematic_act.npz');qref=ref['qpos'];fp=np.load(ROOT/'output/foundationpose_spider_20260915/foundationpose_objects.npz');q=a['qpos'].reshape(-1,66);times=a['time'].ravel();assert np.isfinite(q).all();pen=[];counts=[];activecounts=[];errors=[[],[]];angles=[[],[]]
+for f,row in enumerate(q):
+ d.qpos[:]=row;mujoco.mj_fwdPosition(m,d);i=min(int(round(times[f]/.0025)),len(qref)-1);dr.qpos[:]=qref[i];mujoco.mj_kinematics(m,dr);cs=[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 cs],default=0)*1000);counts.append(sum(c.dist<.001 for c in cs));activecounts.append(sum(c.dist<c.includemargin for c in cs))
+ for k,s in enumerate(['right','left']):
+  site=m.site(s+'_object_track').id;obj=m.body(s+'_object').id;errors[k].append(np.linalg.norm(d.site_xpos[site]-dr.site_xpos[site])*1000);angles[k].append(R.from_matrix(d.xmat[obj].reshape(3,3)@dr.xmat[obj].reshape(3,3).T).magnitude()*180/np.pi)
+ids=np.flatnonzero(m.jnt_limited);v=q[:,m.jnt_qposadr[ids]];limits=float(max(np.maximum(m.jnt_range[ids,0]-v,0).max(),np.maximum(v-m.jnt_range[ids,1],0).max()));report={'steps':len(q),'duration_s':float(times[-1]),'all_finite':bool(np.isfinite(q).all()),'max_penetration_mm':float(max(pen)),'median_step_max_penetration_mm':float(np.median(pen)),'steps_over_1mm':int(np.sum(np.array(pen)>1)),'steps_with_hand_object_contact':int(np.sum(np.array(counts)>0)),'steps_with_active_hand_object_constraint':int(np.sum(np.array(activecounts)>0)),'contact_count_definition':'within 1 mm; active constraints separately include configured contact activation distance','max_joint_limit_violation_rad':limits,'zero_object_actuator_gains':bool(not m.actuator_gainprm[-12:].any() and not m.actuator_biasprm[-12:].any())}
+videoindex=np.minimum(np.round(times*30).astype(int),351)
+for k,n in enumerate(['upper','lower']):
+ mask=fp[n+'_valid'][videoindex];ee=np.array(errors[k]);aa=np.array(angles[k]);report[n]={'observed_centroid_error_mean_mm':float(ee[mask].mean()),'observed_centroid_error_max_mm':float(ee[mask].max()),'observed_rotation_error_mean_deg':float(aa[mask].mean())}
+report['grasp_tracking_20mm_passed']=all(report[n]['observed_centroid_error_max_mm']<20 for n in ['upper','lower']);(O/'physics_validation.json').write_text(json.dumps(report,indent=2));np.savez_compressed(O/'physics_motion.npz',qpos=q,ctrl=a['ctrl'].reshape(-1,56),time=times,max_penetration_mm=pen,contact_counts=counts,active_contact_counts=activecounts,object_centroid_error_mm=np.array(errors).T,object_rotation_error_deg=np.array(angles).T);print(json.dumps(report,indent=2))

scripts/audit_red_depth_metrics.py

@@ -0,0 +1,24 @@
+"""Independent distance sign audit and paired metric recomputation from saved geometry."""
+from pathlib import Path
+import numpy as np,trimesh,open3d as o3d,json
+from red_box_distance import signed_distance
+ROOT=Path(__file__).resolve().parents[1];out=ROOT/'output/depth_ablation_red_20260916';a=np.load(out/'foreground_left_depth_comparison.npz');m=trimesh.load(out/'red_box_closed.ply',process=False)
+scene=o3d.t.geometry.RaycastingScene();scene.add_triangles(o3d.t.geometry.TriangleMesh.from_legacy(o3d.geometry.TriangleMesh(o3d.utility.Vector3dVector(m.vertices),o3d.utility.Vector3iVector(m.faces))))
+rows=json.loads((out/'foreground_left_frame_metrics.json').read_text());report=json.loads((out/'comparison_metrics.json').read_text())
+for i in np.flatnonzero(a['paired']):
+ T=a['object_pose'][i];v=a['verts_before'][i];j=a['joints_before'][i];groups=[np.argsort(np.linalg.norm(v-j[k],axis=1))[:20] for k in [4,8,12,16,20]]
+ for label in ['before','after']:
+  local=(a['verts_'+label][i]-T[:3,3])@T[:3,:3];sd=signed_distance(scene,m,local)
+  gap=[float(np.min(np.abs(sd[g]))*1000) for g in groups]
+  rows[i][label]={'penetrating_vertex_fraction':float((sd<-.001).mean()),'max_vertex_penetration_mm':float(max(0,-sd.min())*1000),'nearest_fingertip_surface_gap_mm':min(gap),'fingertip_surface_gaps_mm':gap}
+ if i%60==0:print('audited',i,flush=True)
+s=report['hands']['foreground_left']
+for label in ['before','after']:
+ for key in ['penetrating_vertex_fraction','max_vertex_penetration_mm','nearest_fingertip_surface_gap_mm']:
+  x=np.array([rows[i][label][key] for i in np.flatnonzero(a['paired'])]);s[label][key]={'median':float(np.median(x)),'p95':float(np.percentile(x,95)),'mean':float(np.mean(x))}
+report['distance_sign']='Unsigned nearest surface distance with solid-angle winding number; outside bounding box explicitly exterior. Ray parity produced false inside classifications for this CAD and was replaced.'
+# Geometry and data consistency invariants.
+d=a['translation_camera'];assert np.max(np.abs(a['verts_after']-a['verts_before']-d[:,None,:]))<1e-6
+assert np.max(np.abs(a['joints_after']-a['joints_before']-d[:,None,:]))<1e-6
+assert np.isfinite(a['verts_after']).all();assert len(a['verts_before'])==315
+(out/'foreground_left_frame_metrics.json').write_text(json.dumps(rows,indent=2));(out/'comparison_metrics.json').write_text(json.dumps(report,indent=2));print(json.dumps(s,indent=2))

scripts/audit_smooth_table_result.py

@@ -0,0 +1,34 @@
+"""Audit filtered/projected reference for table and hand-object penetration."""
+from pathlib import Path
+import json
+import numpy as np
+import mujoco
+
+ROOT = Path(__file__).resolve().parents[1]
+O = ROOT / "output/desktop_smooth_fix_20260915"
+z = np.load(O / "reference_video_rate.npz")
+m = mujoco.MjModel.from_xml_path(str(O / "scene_act.xml")); d = mujoco.MjData(m)
+floor = next(i for i in range(m.ngeom) if "_floor" in (m.geom(i).name or ""))
+floor_z = float(m.geom_pos[floor][2])
+result = {"frames": len(z["qpos"]), "floor_z_m": floor_z, "max_table_penetration_mm": 0.0,
+          "max_hand_object_penetration_mm": 0.0, "frames_table_over_0_5mm": 0,
+          "frames_hand_object_over_0_5mm": 0}
+for row in z["qpos"]:
+    d.qpos[:] = row; mujoco.mj_forward(m, d)
+    floor_min = {"right": 1.0, "left": 1.0}
+    for g in range(m.ngeom):
+        n = m.geom(g).name or ""
+        if not n.startswith(("right_", "left_")) or m.geom_dataid[g] < 0:
+            continue
+        side = n.split("_")[0]; mid = int(m.geom_dataid[g]); a = int(m.mesh_vertadr[mid]); b = a + int(m.mesh_vertnum[mid])
+        v = m.mesh_vert[a:b: max(1, (b-a)//128)]
+        world = v @ d.geom_xmat[g].reshape(3,3).T + d.geom_xpos[g]
+        floor_min[side] = min(floor_min[side], float(world[:,2].min()-floor_z))
+    tp = max(0.0, -min(floor_min.values())) * 1000
+    hp = 0.0
+    for c in d.contact:
+        a,b = int(c.geom[0]), int(c.geom[1]); ta,tb = int(m.geom_contype[a]),int(m.geom_contype[b])
+        if {ta,tb} == {1,2}: hp = max(hp, max(0.0,-float(c.dist))*1000)
+    result["max_table_penetration_mm"] = max(result["max_table_penetration_mm"],tp); result["max_hand_object_penetration_mm"] = max(result["max_hand_object_penetration_mm"],hp)
+    result["frames_table_over_0_5mm"] += tp > .5; result["frames_hand_object_over_0_5mm"] += hp > .5
+(O/"smoothing_table_validation.json").write_text(json.dumps(result,indent=2)); print(json.dumps(result,indent=2))

scripts/audit_spider_dynamic_surfaces.py

@@ -0,0 +1,18 @@
+"""Check original visual hand surfaces against exact closed CAD in dynamic rollout."""
+import os
+os.environ['OPENBLAS_NUM_THREADS']='1';os.environ['OMP_NUM_THREADS']='1'
+from pathlib import Path
+import json,numpy as np,mujoco,trimesh,open3d as o3d
+from red_box_distance import signed_distance
+ROOT=Path(__file__).resolve().parents[1];O=Path(os.environ.get('SPIDER_TASK_OUT',str(ROOT/'output/spider_dynamics_fix_20260915')));m=mujoco.MjModel.from_xml_path(str(O/'datasets/processed/current/l20/bimanual/boxes/scene_act.xml'));d=mujoco.MjData(m);a=np.load(O/'physics_motion.npz');t=np.load(O/'reference_video_rate.npz')['time'];ix=np.argmin(abs(a['time'][:,None]-t[None,:]),axis=0);q=a['qpos'][ix];probes=[]
+for g in range(m.ngeom):
+ n=m.geom(g).name or ''
+ if '_visual_' not in n or not n.startswith(('right_','left_')):continue
+ mid=m.geom_dataid[g];v=m.mesh_vert[m.mesh_vertadr[mid]:m.mesh_vertadr[mid]+m.mesh_vertnum[mid]];faces=m.mesh_face[m.mesh_faceadr[mid]:m.mesh_faceadr[mid]+m.mesh_facenum[mid]];cent=v[faces].mean(1);pts=np.r_[v[np.linspace(0,len(v)-1,min(64,len(v)),dtype=int)],cent[np.linspace(0,len(cent)-1,min(64,len(cent)),dtype=int)]];probes.append((g,pts))
+report={'sampled_frames':len(q),'hand_surface_samples_per_frame':sum(len(p) for g,p in probes),'scope':'Original visible hand meshes vs closed CAD; deterministic surface sampling, not exhaustive intersection or continuous-time proof.'}
+for name,side in [('upper','right'),('lower','left')]:
+ mesh=trimesh.load(ROOT/'output/collision_fix_20260915/collision_v2'/f'{name}_closed.ply');assert mesh.is_watertight;sc=o3d.t.geometry.RaycastingScene();sc.add_triangles(o3d.t.geometry.TriangleMesh.from_legacy(o3d.geometry.TriangleMesh(o3d.utility.Vector3dVector(mesh.vertices),o3d.utility.Vector3iVector(mesh.faces))));obj=m.body(side+'_object').id;values=[]
+ for row in q:
+  d.qpos[:]=row;mujoco.mj_kinematics(m,d);points=np.concatenate([v@d.geom_xmat[g].reshape(3,3).T+d.geom_xpos[g] for g,v in probes]);local=(points-d.xpos[obj])@d.xmat[obj].reshape(3,3);dist=signed_distance(sc,mesh,local);values.append(max(0,-dist.min())*1000)
+ report[name]={'max_inside_mm':float(max(values)),'frames_over_1mm':int(np.sum(np.array(values)>1))};print(name,report[name],flush=True)
+(O/'dynamic_surface_validation.json').write_text(json.dumps(report,indent=2))

scripts/audit_yesterday_geometry.py

@@ -0,0 +1,24 @@
+"""Independent full-rate collision audit and exact closed-red-mesh landmark probes."""
+import os
+os.environ.setdefault('MUJOCO_GL','osmesa')
+from pathlib import Path
+import json,numpy as np,mujoco,trimesh,open3d as o3d
+from red_box_distance import signed_distance
+ROOT=Path(__file__).resolve().parents[1];O=ROOT/'output/foundationpose_spider_20260915';T=O/'datasets/processed/current/l20/bimanual/boxes'
+m=mujoco.MjModel.from_xml_path(str(T/'scene_act.xml'));d=mujoco.MjData(m);r=np.load(O/'comparison_motion.npz');q=r['spider_full_qpos'];pens=[];counts=[]
+for row in q:
+ d.qpos[:]=row;mujoco.mj_fwdPosition(m,d)
+ selected=[c for c in d.contact if {int(m.geom_contype[c.geom[0]]),int(m.geom_contype[c.geom[1]])}=={1,2}]
+ pens.append(max([max(0.,-c.dist) for c in selected],default=0)*1000);counts.append(len(selected))
+mesh=trimesh.load(ROOT/'output/depth_ablation_red_20260916/red_box_closed.ply');assert mesh.is_watertight
+sc=o3d.t.geometry.RaycastingScene();sc.add_triangles(o3d.t.geometry.TriangleMesh.from_legacy(o3d.geometry.TriangleMesh(o3d.utility.Vector3dVector(mesh.vertices),o3d.utility.Vector3iVector(mesh.faces))))
+ids=[i for i in range(m.nsite) if 'landmark_' in m.site(i).name or '_hand_' in m.site(i).name];assert len(ids)==42
+obj=m.body('right_object').id;report={}
+for key in ['original','reference','spider']:
+ points=[]
+ for row in r[key]:
+  d.qpos[:]=row;mujoco.mj_kinematics(m,d);points.append((d.site_xpos[ids]-d.xpos[obj])@d.xmat[obj].reshape(3,3))
+ dist=signed_distance(sc,mesh,np.array(points).reshape(-1,3)).reshape(-1,len(ids));report[key]={'frames_with_landmark_inside_red_over_1mm':int((dist.min(1)<-.001).sum()),'max_red_landmark_inside_depth_mm':float(max(0.,-dist.min())*1000)}
+report['full_rate_spider_collision']={'steps':len(q),'max_hand_object_penetration_mm':float(max(pens)),'median_step_max_penetration_mm':float(np.median(pens)),'hand_object_contact_steps':int((np.array(counts)>0).sum())}
+report['scope']='42 kinematic landmarks are point probes, not complete hand mesh intersection. Exact red sign uses winding on topology-repaired watertight CAD; full-rate contacts use CoACD approximation.'
+(O/'geometry_audit.json').write_text(json.dumps(report,indent=2));np.savez_compressed(O/'full_rate_contacts.npz',time=r['spider_full_time'],max_penetration_mm=pens,contact_counts=counts);print(json.dumps(report,indent=2))

scripts/build_box_collision_v2.py

@@ -0,0 +1,24 @@
+"""Repair CAD T junctions without geometry changes, then decompose without a 20-hull cap."""
+from pathlib import Path
+import numpy as np,trimesh,json,coacd
+ROOT=Path(__file__).resolve().parents[1];O=ROOT/'output/collision_fix_20260915';C=O/'collision_v2';C.mkdir(parents=True,exist_ok=True)
+report={};coacd.set_log_level('warn')
+for name,file in [('upper','上半.stl'),('lower','下半.stl')]:
+ old=trimesh.load(ROOT/'docs'/file);m=old.copy();splits=0
+ for iteration in range(100):
+  count=np.bincount(m.edges_unique_inverse);edges=m.edges_unique[count==1];changed=False;v=m.vertices
+  for a,b in edges:
+   ab=v[b]-v[a];t=(v-v[a])@ab/(ab@ab);distance=np.linalg.norm(v-(v[a]+t[:,None]*ab),axis=1);inside=np.flatnonzero((t>1e-7)&(t<1-1e-7)&(distance<1e-8))
+   if len(inside)==0:continue
+   faces=m.faces.tolist();hit=[i for i,f in enumerate(faces) if a in f and b in f]
+   if len(hit)!=1:continue
+   face=faces.pop(hit[0]);k=next(k for k in range(3) if face[k] in [a,b] and face[(k+1)%3] in [a,b]);a,b,third=face[k],face[(k+1)%3],face[(k+2)%3];inside=sorted(inside,key=lambda x:np.linalg.norm(v[x]-v[a]));chain=[a,*inside,b];faces.extend([[int(x),int(y),int(third)] for x,y in zip(chain[:-1],chain[1:])]);m=trimesh.Trimesh(v.copy(),faces,process=False);splits+=1;changed=True;break
+  if not changed:break
+ assert np.array_equal(old.vertices,m.vertices) and abs(old.area-m.area)<1e-8 and abs(old.volume-m.volume)<1e-8
+ print(name,'closed',m.is_watertight,'splits',splits,'boundary',int(np.sum(np.bincount(m.edges_unique_inverse)==1)),flush=True)
+ assert m.is_watertight and m.is_winding_consistent
+ m.export(C/(name+'_closed.ply'))
+ parts=coacd.run_coacd(coacd.Mesh(m.vertices,m.faces),threshold=.03,max_convex_hull=96,preprocess_mode='off',resolution=1500,mcts_nodes=10,mcts_iterations=60,mcts_max_depth=3,merge=True,seed=0)
+ for i,(v,f) in enumerate(parts):trimesh.Trimesh(v,f,process=False).export(C/f'{name}_{i:03d}.obj')
+ report[name]={'parts':len(parts),'watertight':bool(m.is_watertight),'t_junction_splits':splits,'surface_area_change':float(m.area-old.area),'volume_change':float(m.volume-old.volume),'cad_volume':float(m.volume),'convex_sum_volume':float(sum(trimesh.Trimesh(v,f,process=False).volume for v,f in parts)),'normalized_concavity_threshold':.03,'part_cap':96}
+ (C/'manifest.json').write_text(json.dumps(report,indent=2));print(name,report[name],flush=True)

scripts/build_palm_collision_v2.py

@@ -0,0 +1,11 @@
+from pathlib import Path
+import xml.etree.ElementTree as E,trimesh,coacd,json
+R=Path(__file__).resolve().parents[1];O=R/'output/collision_fix_20260915/hand_collision';O.mkdir(exist_ok=True)
+r=E.parse(R/'output/foundationpose_spider_20260915/datasets/processed/current/l20/bimanual/boxes/scene_act.xml').getroot();report={};coacd.set_log_level('warn')
+for x in r.findall('asset/mesh'):
+ n=x.get('name')
+ if not any(n==s+'_'+part+'_mesh_0' for s in ['right','left'] for part in ['hand_base_link','thumb_metacarpals']):continue
+ m=trimesh.load(x.get('file'));parts=coacd.run_coacd(coacd.Mesh(m.vertices,m.faces),threshold=.015,max_convex_hull=16,preprocess_mode='auto',preprocess_resolution=60,resolution=1000,mcts_nodes=10,mcts_iterations=40,mcts_max_depth=3,merge=True,seed=0)
+ for i,(v,f) in enumerate(parts):trimesh.Trimesh(v,f,process=False).export(O/f'{n}_{i:03d}.obj')
+ report[n]={'parts':len(parts),'original_hull_volume':float(m.convex_hull.volume),'parts_volume_sum':float(sum(trimesh.Trimesh(v,f,process=False).volume for v,f in parts)),'nonwatertight_source':not m.is_watertight}
+ (O/'manifest.json').write_text(json.dumps(report,indent=2));print(n,report[n],flush=True)

scripts/check_spider_dynamics_replay.py

@@ -0,0 +1,19 @@
+"""Compare saved controls in fresh GPU worlds and CPU, without optimization."""
+import os,json
+from pathlib import Path
+ROOT=Path(__file__).resolve().parents[1]
+os.environ['WARP_CACHE_PATH']=str(ROOT/'.spider_cache/warp');os.environ['OMP_NUM_THREADS']='4'
+import numpy as np,mujoco,mujoco_warp as mw,warp as wp
+O=Path(os.environ.get('SPIDER_TASK_OUT',str(ROOT/'output/spider_dynamics_fix_20260915')));T=O/'datasets/processed/current/l20/bimanual/boxes'
+a=np.load(T/'0/trajectory_mjwp_act.npz');r=np.load(T/'0/trajectory_kinematic_act.npz');rq,rv,rc=(r[k] for k in ['qpos','qvel','ctrl']);u=a['ctrl'].reshape(-1,56);expected=a['qpos'].reshape(-1,66)
+m=mujoco.MjModel.from_xml_path(str(T/'scene_act.xml'));settings=json.loads((O/'physics_parameters.json').read_text());m.opt.iterations=settings['solver_iterations'];m.opt.ls_iterations=settings['ls_iterations'];m.opt.timestep=.0025;m.opt.integrator=mujoco.mjtIntegrator.mjINT_IMPLICITFAST;m.opt.o_solref[:]=[.02,1];m.opt.o_solimp[:]=[0,.95,.03,.5,2]
+assert not m.actuator_gainprm[-12:].any() and not m.actuator_biasprm[-12:].any()
+d=mujoco.MjData(m);d.qpos[:]=rq[0].astype(np.float32);d.qvel[:]=rv[0].astype(np.float32);d.ctrl[:]=rc[0].astype(np.float32);mujoco.mj_step(m,d)
+wp.init();gpu=[];cpu=[]
+with wp.ScopedDevice('cuda:0'):
+ wm=mw.put_model(m);wd=mw.put_data(m,d,nworld=64,nconmax=1024,njmax=3072)
+ with wp.ScopedCapture() as capture:mw.step(wm,wd)
+ for i,ctrl in enumerate(u):
+  wd.ctrl.assign(np.tile(ctrl.astype(np.float32),(64,1)));wp.capture_launch(capture.graph);wp.synchronize();gpu.append(wd.qpos.numpy()[0].copy());d.ctrl[:]=ctrl;mujoco.mj_step(m,d);cpu.append(d.qpos.copy())
+gpu=np.array(gpu);cpu=np.array(cpu);diff=np.abs(gpu-expected);reports={'steps':len(u),'all_finite':bool(np.isfinite(gpu).all() and np.isfinite(cpu).all()),'zero_object_assistance':True,'gpu_first_step_max_qpos_difference':float(diff[0].max()),'gpu_max_qpos_difference':float(diff.max()),'cpu_gpu_max_qpos_difference':float(np.abs(cpu-gpu).max()),'note':'qpos differences mix meters and radians; 64 fresh duplicate worlds, same model/initial state/controls'}
+np.savez_compressed(O/'control_replay.npz',gpu=gpu,cpu=cpu,expected=expected,ctrl=u,time=a['time'].ravel());(O/'control_replay.json').write_text(json.dumps(reports,indent=2));print(json.dumps(reports,indent=2))

scripts/check_spider_state_restore.py

@@ -0,0 +1,19 @@
+"""Test whether speculative rollout state can affect the next executed step."""
+import os,sys,json
+from pathlib import Path
+ROOT=Path(__file__).resolve().parents[1];O=ROOT/'output/spider_dynamics_fix_20260915';T=O/'datasets/processed/current/l20/bimanual/boxes'
+os.environ['WARP_CACHE_PATH']=str(ROOT/'.spider_cache/warp');sys.path.insert(0,str(ROOT/'third_party/spider'))
+import numpy as np,torch,mujoco,warp as wp
+from spider.config import Config
+from spider.simulators import mjwp as s
+c=Config(device='cuda:0',num_samples=64,nconmax_per_env=1024,njmax_per_env=3072,sim_dt=.0025,embodiment_type='bimanual');c.model_path=str(T/'scene_act.xml');c.object_actuator_ids=list(range(44,56));c.npair=0
+setup=s.setup_mj_model
+def model(c):
+ m=setup(c);m.opt.iterations=80;return m
+s.setup_mj_model=model
+r=np.load(T/'0/trajectory_kinematic_act.npz');ref=tuple(torch.tensor(r[k],device='cuda:0',dtype=torch.float32) for k in ['qpos','qvel','ctrl','contact','contact_pos']);env=s.setup_env(c,ref);u=ref[2][0];state=s.save_state(env);s.step_env(c,env,u);wp.synchronize();expected=env.data_wp.qpos.numpy().copy();s.load_state(env,state)
+for _ in range(80):s.step_env(c,env,u)
+s.load_state(env,state);s.step_env(c,env,u);wp.synchronize();after=env.data_wp.qpos.numpy().copy();report={'state_only_max_qpos_difference':float(np.max(abs(expected-after)))}
+s.load_state(env,state);params=s.save_env_params(c,env);s.load_env_params(c,env,{'kp':np.ones(12,dtype=np.float32)*10,'kd':np.ones(12,dtype=np.float32)})
+for _ in range(80):s.step_env(c,env,u)
+s.load_state(env,state);s.load_env_params(c,env,params);s.step_env(c,env,u);wp.synchronize();after=env.data_wp.qpos.numpy().copy();report['state_and_parameters_max_qpos_difference']=float(np.max(abs(expected-after)));(O/'state_restore_check.json').write_text(json.dumps(report,indent=2));print(report)

scripts/compare_red_hand_depth.py

@@ -0,0 +1,75 @@
+"""Paired depth-only translation ablation; fixed hand articulation and object trajectory."""
+import os
+os.environ.setdefault('OMP_NUM_THREADS','4')
+import sys,json
+from pathlib import Path
+ROOT=Path(__file__).resolve().parents[1];sys.path.insert(0,str(ROOT));sys.path.insert(0,str(ROOT/'third_party/FoundationPose'))
+import numpy as np,cv2,torch,trimesh,open3d as o3d,nvdiffrast.torch as dr
+from Utils import nvdiffrast_render,make_mesh_tensors
+from utils.mano_utils import MANOForwardKinematics
+from red_box_distance import signed_distance
+OUT=ROOT/'output/depth_ablation_red_20260916';SRC=ROOT/'docs/20260916_104026'
+torch.set_num_threads(4)
+meta=json.loads((SRC/'intrinsics.json').read_text());K=np.array([[meta['fx'],0,meta['cx']],[0,meta['fy'],meta['cy']],[0,0,1.]])
+obj=np.load(ROOT/'output/foundationpose_red_20260916/poses.npz');P=obj['T_camera_from_object'][:315];objvalid=obj['accepted'][:315]
+mesh=trimesh.load(OUT/'red_box_closed.ply',process=False);assert mesh.is_watertight
+scene=o3d.t.geometry.RaycastingScene();scene.add_triangles(o3d.t.geometry.TriangleMesh.from_legacy(o3d.geometry.TriangleMesh(o3d.utility.Vector3dVector(mesh.vertices),o3d.utility.Vector3iVector(mesh.faces))))
+ctx=dr.RasterizeCudaContext();eye=torch.eye(4,device='cuda')[None]
+fk=MANOForwardKinematics(str(ROOT/'third_party/hamer/_DATA/data/mano'),torch.device('cpu'))
+yy,xx=np.indices((480,848));train=((xx//8+yy//8)%2)==0
+all_data={};report={'design':'Same RGB-only HandFlow output, fixed STL/object pose, articulation/shape unchanged. Single robust camera-ray translation using depth. No added smoothing or interpolation. Fit and evaluation use disjoint 8x8 image checkerboard cells.','limits':['Visible skin selected with a color heuristic; correspondence and occlusion errors remain.','Held-out cells belong to the same depth sensor and frame, not independent ground truth.','Vertex signed distances do not detect every triangle-triangle collision.','Contact labels and force ground truth unavailable; nearest fingertip gap is not proof of contact.'],'hands':{}}
+def render(v,faces):
+ mt={'pos':torch.as_tensor(v,device='cuda',dtype=torch.float32),'faces':torch.as_tensor(faces,device='cuda',dtype=torch.int32),'vnormals':torch.zeros((len(v),3),device='cuda'),'vertex_color':torch.ones((len(v),3),device='cuda')}
+ with torch.inference_mode():_,d,_=nvdiffrast_render(K=K,H=480,W=848,ob_in_cams=eye,glctx=ctx,mesh_tensors=mt)
+ return d[0].cpu().numpy()
+def signed(v,T):
+ local=(v-T[:3,3])@T[:3,:3]
+ return signed_distance(scene,mesh,local)
+def stat(x):
+ x=np.asarray(x);x=x[np.isfinite(x)];return {'median':float(np.median(x)),'p95':float(np.percentile(x,95)),'mean':float(np.mean(x))} if len(x) else None
+for side in ['foreground_left']:
+ src=np.load(OUT/'left_mirrored'/'handflow_results.npz');v=src['verts_cam'];n=len(v);assert n==315
+ pose=torch.tensor(src['pose']);betas=torch.tensor(src['betas']);trans=torch.tensor(src['trans']);
+ if betas.ndim==1:betas=betas[None].expand(n,-1)
+ elif len(betas)==1:betas=betas.expand(n,-1)
+ j=fk.joints(pose,betas,trans,['right']*n).numpy()/1000
+ reproduced=fk.verts(pose,betas,trans,['right']*n).numpy()/1000;assert np.max(np.abs(reproduced-v))<1e-5
+ # Restore mirrored-right geometry to physical left; reverse face winding under reflection.
+ v=v.copy();v[:,:,0]*=-1;j[:,:,0]*=-1
+ faces=np.ascontiguousarray(src['faces'][:,[0,2,1]])
+ corrected=v.copy();jc=j.copy();deltas=np.zeros((n,3));supported=np.zeros(n,bool);rows=[];cap=cv2.VideoCapture(str(SRC/'color.mp4'))
+ for t in range(n):
+  ok,b=cap.read();assert ok;z=cv2.imread(str(SRC/'depth'/f'{t:06d}.png'),-1).astype('float32')*meta['depth_scale_m']
+  r={'frame':t,'detected':bool(src['pred_valid'][t]),'object_accepted':bool(objvalid[t]),'depth_supported':False}
+  if src['pred_valid'][t]:
+   d0=render(v[t],faces);skin=cv2.inRange(cv2.cvtColor(b,cv2.COLOR_BGR2YCrCb),np.array([0,133,77]),np.array([255,173,127]))>0
+   hsv=cv2.cvtColor(b,cv2.COLOR_BGR2HSV);red=((hsv[:,:,0]<10)|(hsv[:,:,0]>170))&(hsv[:,:,1]>115)
+   skin &= ~red
+   interior=cv2.erode((d0>.1).astype('uint8'),np.ones((5,5),np.uint8))>0
+   mask=skin&interior&(z>.1)&(z<.85)
+   fit=mask&train;errors=z[fit]-d0[fit];delta=float(np.median(errors)) if len(errors) else 0.;mad=float(np.median(np.abs(errors-delta))) if len(errors) else 1.
+   accept=len(errors)>=100 and mad<.025 and abs(delta)<.25 and j[t,0,2]>.1
+   if accept:
+    deltas[t]=j[t,0]/j[t,0,2]*delta;corrected[t]+=deltas[t];jc[t]+=deltas[t];supported[t]=True
+   d1=render(corrected[t],faces) if accept else d0
+   test=mask&(~train)&(d1>.1)
+   r.update(depth_supported=bool(accept),fit_pixels=int(fit.sum()),fit_mad_mm=mad*1000,shift_z_mm=float(deltas[t,2]*1000),heldout_pixels=int(test.sum()),depth_error_before_mm=float(np.median(np.abs(d0[test]-z[test]))*1000) if test.any() else None,depth_error_after_mm=float(np.median(np.abs(d1[test]-z[test]))*1000) if test.any() else None)
+   if objvalid[t]:
+    # Use nearest surface vertices to each MANO fingertip; preserve exactly the same vertices in A/B.
+    tips=[4,8,12,16,20];groups=[np.argsort(np.linalg.norm(v[t]-j[t,k],axis=1))[:20] for k in tips]
+    for label,verts in [('before',v[t]),('after',corrected[t])]:
+     sd=signed(verts,P[t]);gap=[float(np.min(np.abs(sd[g]))*1000) for g in groups]
+     r[label]={'penetrating_vertex_fraction':float((sd<-.001).mean()),'max_vertex_penetration_mm':float(max(0,-sd.min())*1000),'nearest_fingertip_surface_gap_mm':min(gap),'fingertip_surface_gaps_mm':gap}
+  rows.append(r)
+  if t%60==0:print(side,t,flush=True)
+ cap.release();paired=np.array([r['depth_supported'] and r['object_accepted'] and 'before' in r for r in rows]);held=[r for r in rows if r['depth_supported'] and r.get('heldout_pixels',0)>=100]
+ summary={'detected_frames':int(src['pred_valid'].sum()),'depth_supported_frames':int(supported.sum()),'paired_object_frames':int(paired.sum()),'heldout_depth_frames':len(held),'translation_norm_mm':stat(np.linalg.norm(deltas[supported],axis=1)*1000),'before':{},'after':{}}
+ triple=paired[:-2]&paired[1:-1]&paired[2:]
+ for label,joints in [('before',j),('after',jc)]:
+  summary[label]['heldout_depth_error_mm']=stat([r['depth_error_'+label+'_mm'] for r in held])
+  for key in ['penetrating_vertex_fraction','max_vertex_penetration_mm','nearest_fingertip_surface_gap_mm']:summary[label][key]=stat([rows[t][label][key] for t in np.flatnonzero(paired)])
+  second=np.diff(joints[:,0],n=2,axis=0)[triple];summary[label]['wrist_second_difference_rms_mm']=float(np.sqrt(np.mean(np.sum(second**2,axis=-1)))*1000) if len(second) else None
+ summary['jitter_valid_triples']=int(triple.sum());report['hands'][side]=summary
+ np.savez_compressed(OUT/f'{side}_depth_comparison.npz',verts_before=v,verts_after=corrected,joints_before=j,joints_after=jc,faces=faces,translation_camera=deltas,depth_supported=supported,detected=src['pred_valid'],paired=paired,K=K,object_pose=P,object_accepted=objvalid)
+ (OUT/f'{side}_frame_metrics.json').write_text(json.dumps(rows,indent=2))
+(OUT/'comparison_metrics.json').write_text(json.dumps(report,indent=2));print(json.dumps(report,indent=2))

scripts/configure_collision_physics.py

@@ -0,0 +1,14 @@
+"""Apply the selected bounded-drive contact model after geometric preparation."""
+from pathlib import Path
+import json,xml.etree.ElementTree as E
+O=Path(__file__).resolve().parents[1]/'output/collision_fix_20260915';p=O/'datasets/processed/current/l20/bimanual/boxes/scene_act.xml';tree=E.parse(p);r=tree.getroot();r.find('option').set('iterations','80');r.find('option').set('ls_iterations','50')
+for g in r.findall('.//geom'):
+ if g.get('contype') in ['1','2']:g.set('solimp','.999 .9999 .0001 .5 2');g.set('solref','.005 1')
+for a in r.findall('actuator/position'):
+ n=a.get('name')
+ if '_object_' in n:continue
+ if '_hand_pos_' in n:kp,kv,limit=500,30,20
+ elif '_hand_rot_' in n:kp,kv,limit=15,2,2
+ else:kp,kv,limit=4,.15,.1
+ a.attrib.update(kp=str(kp),kv=str(kv),forcerange=f'{-limit} {limit}')
+tree.write(p);(O/'physics_parameters.json').write_text(json.dumps({'solver_iterations':80,'ls_iterations':50,'wrist_position_kp':500,'wrist_position_kv':30,'wrist_force_cap_N':20,'wrist_rotation_kp':15,'wrist_rotation_kv':2,'wrist_torque_cap_Nm':2,'finger_kp':4,'finger_kv':.15,'finger_torque_cap_Nm':.1,'contact_solimp':[.999,.9999,.0001,.5,2],'contact_solref':[.005,1],'note':'Simulation drive assumptions, not measured hardware ratings. Chosen after full-clip CPU control-replay comparison.'},indent=2))

scripts/configure_spider_stable_contact.py

@@ -0,0 +1,9 @@
+"""Apply contact settings selected by the matched CPU/GPU short replay probe."""
+from pathlib import Path
+import json,xml.etree.ElementTree as ET
+O=Path(__file__).resolve().parents[1]/'output/spider_dynamics_fix_20260915';p=O/'datasets/processed/current/l20/bimanual/boxes/scene_act.xml';tree=ET.parse(p)
+for g in tree.findall('.//geom'):
+ g.set('solimp','.9 .95 .001 .5 2');g.set('solref','.02 1')
+ if g.get('contype') in ['1','2']:g.set('margin','.004');g.set('gap','.002')
+tree.write(p)
+s=json.loads((O/'physics_parameters.json').read_text());s.update(contact_solimp=[.9,.95,.001,.5,2],contact_solref=[.02,1],contact_activation_distance_m=.002,note='Contact regularization and 2 mm activation distance selected from GPU/CPU short replay. Simulation parameters, not measured hardware.');(O/'physics_parameters.json').write_text(json.dumps(s,indent=2))

scripts/configure_spider_task.py

@@ -0,0 +1,29 @@
+"""Build a SPIDER task directory from a kinematic reference directory and apply the validated physics settings.
+Env: SPIDER_SRC (reference dir with datasets/.../boxes, reference_video_rate.npz, config.json), SPIDER_TASK_OUT (target).
+Settings follow output/spider_dynamics_fix_20260915/physics_parameters.json (simulation assumptions, not hardware)."""
+import os, json, shutil, xml.etree.ElementTree as E
+from pathlib import Path
+ROOT = Path(__file__).resolve().parents[1]; SRC = Path(os.environ['SPIDER_SRC']); OUT = Path(os.environ['SPIDER_TASK_OUT'])
+T = OUT / 'datasets/processed/current/l20/bimanual/boxes'; (T / '0').mkdir(parents=True, exist_ok=True)
+for rel in ['datasets/processed/current/l20/bimanual/boxes/scene_act.xml', 'datasets/processed/current/l20/bimanual/boxes/task_info.json', 'datasets/processed/current/l20/bimanual/boxes/0/trajectory_kinematic_act.npz', 'reference_video_rate.npz']:
+    shutil.copy2(SRC / rel, OUT / rel)
+cfg = json.loads((SRC / 'config.json').read_text()); cfg['dataset_dir'] = str(OUT / 'datasets'); cfg.update(json.loads(os.environ.get('SPIDER_CONFIG_OVERRIDES', '{}'))); (OUT / 'config.json').write_text(json.dumps(cfg, indent=2))
+for name in ['collision_v2', 'hand_collision']:
+    if not (OUT / name).exists(): os.symlink(os.path.relpath(ROOT / 'output/collision_fix_20260915' / name, OUT), OUT / name)
+p = T / 'scene_act.xml'; tree = E.parse(p); r = tree.getroot()
+r.find('option').set('iterations', '80'); r.find('option').set('ls_iterations', '50')
+for g in r.findall('.//geom'):
+    g.set('solimp', '.9 .95 .001 .5 2'); g.set('solref', '.02 1')
+    if g.get('contype') in ['1', '2', '4']: g.set('margin', '.004'); g.set('gap', '.002')
+for a in r.findall('actuator/position'):
+    n = a.get('name')
+    if '_object_' in n: continue
+    if '_hand_pos_' in n: kp, kv, limit = 500, 30, 20
+    elif '_hand_rot_' in n: kp, kv, limit = 15, 2, 2
+    else: kp, kv, limit = float(os.environ.get('HF_FINGER_KP', '4')), float(os.environ.get('HF_FINGER_KV', '.15')), float(os.environ.get('HF_FINGER_TORQUE', '.1'))
+    a.attrib.update(kp=str(kp), kv=str(kv), forcerange=f'{-limit} {limit}')
+tree.write(p)
+settings = {'solver_iterations': 80, 'ls_iterations': 50, 'wrist_position_kp': 500, 'wrist_position_kv': 30, 'wrist_force_cap_N': 20, 'wrist_rotation_kp': 15, 'wrist_rotation_kv': 2, 'wrist_torque_cap_Nm': 2, 'finger_kp': float(os.environ.get('HF_FINGER_KP', '4')), 'finger_kv': float(os.environ.get('HF_FINGER_KV', '.15')), 'finger_torque_cap_Nm': float(os.environ.get('HF_FINGER_TORQUE', '.1')),
+            'contact_solimp': [.9, .95, .001, .5, 2], 'contact_solref': [.02, 1], 'contact_activation_distance_m': .002, 'table_collides_with_hands': True,
+            'note': 'Same contact regularization and 2 mm activation as spider_dynamics_fix_20260915 (selected by GPU/CPU short replay); table now a hand collider. Simulation parameters, not measured hardware.', 'source_reference': str(SRC)}
+(OUT / 'physics_parameters.json').write_text(json.dumps(settings, indent=2)); print('configured', OUT)

scripts/constrain_rgbd_hand.py

@@ -0,0 +1,70 @@
+"""Use observed skin-surface depth to correct camera-space hand translation.
+
+This is a robust translation fit, not depth-derived skeleton ground truth.
+"""
+from pathlib import Path
+import json
+import argparse
+import numpy as np
+import cv2
+from scipy.ndimage import gaussian_filter1d
+from scipy.spatial.transform import Rotation
+
+ROOT=Path(__file__).resolve().parents[1];BASE=ROOT/'output/20260915_171525'
+SRC=ROOT/'docs/20260915_171525'
+parser=argparse.ArgumentParser();parser.add_argument('--left',action='store_true');args=parser.parse_args()
+folder='handflow_left' if args.left else 'handflow'
+suffix='_left' if args.left else ''
+h=np.load(BASE/folder/'handflow_results.npz');j=dict(np.load(BASE/folder/'human_joints.npz'))
+meta=json.loads((SRC/'intrinsics.json').read_text());camera=np.load(BASE/'rgbd_camera.npz')
+K=np.array([meta[k] for k in ['fx','fy','cx','cy']]);N=len(h['pose'])
+c2w=camera['c2w'];old_camera=h['c2w']
+wrist_cam=np.einsum('tji,tj->ti',old_camera[:,:3,:3],j['wrist_world']-old_camera[:,:3,3])
+shifts=[];records=[];valid=[];cap=cv2.VideoCapture(str(SRC/'color.mp4'))
+for t,verts in enumerate(h['verts_cam']):
+    ok,color=cap.read();assert ok
+    dep=cv2.imread(str(SRC/'depth'/f'{t:06d}.png'),-1).astype(float)*meta['depth_scale_m']
+    ycc=cv2.cvtColor(color,cv2.COLOR_BGR2YCrCb)
+    skin=cv2.inRange(ycc,np.array([0,133,77]),np.array([255,173,127]))>0
+    uv=np.rint(verts[:,:2]/verts[:,2,None]*K[:2]+K[2:]).astype(int)
+    keep=(verts[:,2]>.05)&(uv[:,0]>=2)&(uv[:,0]<color.shape[1]-2)&(uv[:,1]>=2)&(uv[:,1]<color.shape[0]-2)
+    # Keep nearest projected vertex per 4x4 pixel bin, reducing hidden-surface bias.
+    closest={}
+    for index in np.flatnonzero(keep):
+        u,v=uv[index];key=(u//4,v//4)
+        if key not in closest or verts[index,2]<verts[closest[key],2]:closest[key]=index
+    errors=[]
+    for index in closest.values():
+        u,v=uv[index]
+        if not skin[v,u]:continue
+        patch=dep[v-1:v+2,u-1:u+2];values=patch[(patch>.1)&(patch<.85)]
+        if len(values)<5 or np.ptp(values)>.04:continue
+        errors.append(float(np.median(values)-verts[index,2]))
+    dz=float(np.median(errors)) if errors else 0.
+    mad=float(np.median(np.abs(np.asarray(errors)-dz))) if errors else 1.
+    accepted=bool(len(errors)>=25 and mad<.025 and abs(dz)<.25 and h['pred_valid'][t])
+    shifts.append(dz);valid.append(accepted)
+    records.append(dict(frame=t,samples=len(errors),median_depth_residual_m=dz,mad_m=mad,accepted=accepted))
+cap.release();valid=np.asarray(valid);idx=np.flatnonzero(valid)
+assert len(idx)>=N*.5, f'Insufficient depth support: {len(idx)}/{N}'
+filled=np.interp(np.arange(N),idx,np.asarray(shifts)[idx]);smooth=gaussian_filter1d(filled,2)
+delta=wrist_cam/wrist_cam[:,2,None]*smooth[:,None]
+corrected=wrist_cam+delta
+world=np.einsum('tij,tj->ti',c2w[:,:3,:3],corrected)+c2w[:,:3,3]
+root_world=Rotation.from_matrix(c2w[:,:3,:3]@Rotation.from_rotvec(h['pose'][:,:3]).as_matrix()).as_rotvec()
+j.update(wrist_world=world,root_orient=root_world,time=camera['time'],
+         before_depth_wrist_camera=wrist_cam,depth_translation_camera=delta,
+         depth_valid=valid,depth_correction_interpolated=~valid,
+         detection_valid=j['detection_valid']&valid&camera['valid'],
+         source=str((BASE/folder/'handflow_results.npz').resolve()),
+         metric_scale_provenance='D405 depth_scale from recording metadata; skin-surface translation fit, model size unchanged; RGBD static-background odometry world. Not calibrated motion ground truth.')
+np.savez_compressed(BASE/f'human_joints_rgbd{suffix}.npz',**j)
+np.savez_compressed(BASE/f'depth_hand_fit{suffix}.npz',raw_depth_shift=shifts,applied_depth_shift=smooth,
+                    accepted=valid,wrist_camera=corrected,wrist_world=world,c2w=c2w,
+                    corrected_verts_camera=h['verts_cam']+delta[:,None,:])
+report=dict(frames=N,depth_supported_frames=int(valid.sum()),rejected_depth_frames=np.flatnonzero(~valid).tolist(),
+            median_abs_raw_depth_residual_m=float(np.median(np.abs(np.asarray(shifts)[valid]))),
+            median_abs_remaining_sample_residual_m=float(np.median(np.abs(np.asarray(shifts)[valid]-smooth[valid]))),
+            applied_depth_shift_range_m=[float(smooth.min()),float(smooth.max())],frames_detail=records,
+            boundary='Translation-only fit to selected visible skin depth; self-occlusion and skin-mask errors remain. No finger-joint depth truth. Rejected depth corrections interpolated and marked invalid.')
+(BASE/f'depth_fit_validation{suffix}.json').write_text(json.dumps(report,indent=2));print(json.dumps({k:v for k,v in report.items() if k!='frames_detail'},indent=2))

scripts/correct_dynhamr_rgbd.py

@@ -0,0 +1,80 @@
+"""Offline, rotation-safe smoothing and conservative visible-surface RGB-D fit."""
+import sys,json
+from pathlib import Path
+import numpy as np
+import torch,cv2
+from scipy.spatial.transform import Rotation
+from scipy.ndimage import gaussian_filter1d
+ROOT=Path(__file__).resolve().parents[1]
+sys.path.insert(0,str(ROOT/'third_party/Dyn-HaMR/dyn-hamr'))
+from body_model import MANO,run_mano
+BASE=ROOT/'output/20260915_171525_dynhamr'; OUT=BASE/'corrected'; OUT.mkdir(exist_ok=True)
+SRC=ROOT/'docs/20260915_171525'; meta=json.loads((SRC/'intrinsics.json').read_text())
+a=dict(np.load(BASE/'optimization/prior/20260915_171525_000000_world_results.npz')); N=352
+torch.set_num_threads(4)
+model=MANO(model_path=str(ROOT/'third_party/Dyn-HaMR/_DATA/data/mano'),batch_size=704,pose2rot=True)
+def geometry(d):
+ with torch.no_grad():
+  o=run_mano(model,*[torch.tensor(d[k]).float() for k in ['trans','root_orient','pose_body','is_right','betas']])
+ return o['joints'].numpy(),o['vertices'].numpy()
+def smooth_rotation(v,sigma=1.2):
+ shape=v.shape; v=v.reshape(N,-1,3); out=np.empty_like(v)
+ for j in range(v.shape[1]):
+  r=Rotation.from_rotvec(v[:,j])
+  for t in range(N):
+   ix=np.arange(max(0,t-4),min(N,t+5)); w=np.exp(-.5*((ix-t)/sigma)**2)
+   out[t,j]=r[ix].mean(weights=w).as_rotvec()
+ return out.reshape(shape)
+j0,v0=geometry(a); d={k:v.copy() for k,v in a.items()}
+for b in range(2):
+ d['root_orient'][b]=smooth_rotation(a['root_orient'][b]); d['pose_body'][b]=smooth_rotation(a['pose_body'][b])
+# Correct translations in world coordinates; left MANO translation has reflected X.
+R=a['cam_R'][0]; ct=a['cam_t'][0]; K=np.array([meta[k] for k in ['fx','fy','cx','cy']])
+vc=np.einsum('tij,btvj->btvi',R,v0)+ct[None,:,None,:]
+wc=np.einsum('tij,btj->bti',R,j0[:,:,0])+ct[None,:,:]
+raw=np.zeros((2,N)); valid=np.zeros((2,N),bool); counts=np.zeros((2,N),int)
+cap=cv2.VideoCapture(str(SRC/'color.mp4'))
+for t in range(N):
+ ok,c=cap.read(); assert ok
+ dep=cv2.imread(str(SRC/'depth'/f'{t:06d}.png'),-1)*meta['depth_scale_m']
+ skin=cv2.inRange(cv2.cvtColor(c,cv2.COLOR_BGR2YCrCb),np.array([0,133,77]),np.array([255,173,127]))>0
+ for b in range(2):
+  v=vc[b,t]; uv=np.rint(v[:,:2]/v[:,2,None]*K[:2]+K[2:]).astype(int); closest={}
+  for i in np.flatnonzero((v[:,2]>.1)&(uv[:,0]>=2)&(uv[:,0]<846)&(uv[:,1]>=2)&(uv[:,1]<478)):
+   x,y=uv[i]; key=(x//4,y//4)
+   if key not in closest or v[i,2]<v[closest[key],2]: closest[key]=i
+  errors=[]
+  for i in closest.values():
+   x,y=uv[i]
+   if not skin[y,x]:continue
+   patch=dep[y-1:y+2,x-1:x+2]; z=patch[(patch>.1)&(patch<.85)]
+   if len(z)>=5 and np.ptp(z)<.02:errors.append(np.median(z)-v[i,2])
+  counts[b,t]=len(errors)
+  if errors:
+   dz=np.median(errors); mad=np.median(np.abs(np.array(errors)-dz));raw[b,t]=dz
+   valid[b,t]=len(errors)>=25 and mad<.01 and abs(dz)<.025
+cap.release()
+# Only directly supported neighborhoods receive a depth update. No long-gap extrapolation.
+shifts=np.zeros_like(raw)
+for b in range(2):
+ numerator=gaussian_filter1d(raw[b]*valid[b],2); support=gaussian_filter1d(valid[b].astype(float),2)
+ shifts[b]=np.where(support>.6,numerator/np.maximum(support,1e-6),0)
+ shifts[b]=gaussian_filter1d(shifts[b],1)
+ delta_c=wc[b]/wc[b,:,2,None]*shifts[b,:,None]
+ delta_w=np.einsum('tji,tj->ti',R,delta_c)
+ delta_w[:,0]*=2*a['is_right'][b]-1
+ d['trans'][b]=gaussian_filter1d(a['trans'][b]+delta_w,1.5,axis=0)
+j1,v1=geometry(d)
+report={'frames':N,'method':'Rotation weighted means on SO(3), sigma 1.2 frames; translation Gaussian sigma 1.5 frames; conservative skin-depth residual <=25mm and MAD<10mm, support-weighted local correction','offline_uses_future_frames':True,'depth_fit_is_surface_estimate_not_ground_truth':True,'hands':{}}
+for b in range(2):
+ side='right' if a['is_right'][b,0]>.5 else 'left'
+ def rms2(x):return float(np.sqrt(np.mean(np.sum(np.diff(x,n=2,axis=0)**2,axis=-1)))*1000)
+ deviation=np.linalg.norm(j1[b,:,0]-j0[b,:,0],axis=-1)*1000
+ angles=(Rotation.from_rotvec(a['root_orient'][b]).inv()*Rotation.from_rotvec(d['root_orient'][b])).magnitude()*180/np.pi
+ report['hands'][side]={'depth_accepted_frames':int(valid[b].sum()),'depth_shift_max_mm':float(abs(shifts[b]).max()*1000),'wrist_second_difference_rms_mm_before':rms2(j0[b,:,0]),'wrist_second_difference_rms_mm_after':rms2(j1[b,:,0]),'wrist_change_max_mm':float(deviation.max()),'wrist_change_p95_mm':float(np.percentile(deviation,95)),'root_change_max_deg':float(angles.max())}
+ assert np.isfinite(j1[b]).all() and deviation.max()<40 and angles.max()<15
+phase=OUT/'prior';phase.mkdir(exist_ok=True)
+np.savez_compressed(phase/'20260915_171525_000000_world_results.npz',**d)
+np.savez_compressed(OUT/'depth_fit.npz',raw_shift=raw,accepted=valid,applied_shift=shifts,sample_count=counts)
+np.savez_compressed(OUT/'comparison_joints.npz',before=j0,after=j1)
+(OUT/'validation.json').write_text(json.dumps(report,indent=2));print(json.dumps(report,indent=2))

scripts/decompose_yesterday_boxes.py

@@ -0,0 +1,13 @@
+from pathlib import Path
+import numpy as np,trimesh,coacd,json
+ROOT=Path(__file__).resolve().parents[1];out=ROOT/'output/foundationpose_spider_20260915/collision';out.mkdir(exist_ok=True)
+coacd.set_log_level('warn');report={}
+for name,file in [('upper','上半.stl'),('lower','下半.stl')]:
+ m=trimesh.load(ROOT/'docs'/file)
+ if name=='upper':m=trimesh.load(ROOT/'output/depth_ablation_red_20260916/red_box_closed.ply',process=False)
+ print(name,'watertight',m.is_watertight,flush=True)
+ parts=coacd.run_coacd(coacd.Mesh(m.vertices,m.faces),threshold=.03,max_convex_hull=20,preprocess_mode='auto',preprocess_resolution=60,resolution=1500,mcts_nodes=10,mcts_iterations=60,mcts_max_depth=3,merge=True,seed=0)
+ for i,(v,f) in enumerate(parts):trimesh.Trimesh(v,f,process=False).export(out/f'{name}_{i:03d}.obj')
+ report[name]={'parts':len(parts),'mesh_volume_m3':m.volume,'hull_volume_sum_m3':sum(trimesh.Trimesh(v,f,process=False).volume for v,f in parts),'approximation':'CoACD convex decomposition, threshold .03; not exact CAD collision'}
+ print(name,report[name],flush=True)
+(out/'manifest.json').write_text(json.dumps(report,indent=2))

scripts/diagnose_dynhamr_rgbd.py

@@ -0,0 +1,58 @@
+"""Read-only trajectory diagnosis; frame differences are not ground-truth errors."""
+import json
+import sys
+from pathlib import Path
+import numpy as np
+import torch
+from scipy.spatial.transform import Rotation
+ROOT = Path(__file__).resolve().parents[1]
+sys.path.insert(0, str(ROOT/'third_party/Dyn-HaMR/dyn-hamr'))
+from body_model import MANO, run_mano
+from vis.tools import smooth_results
+torch.set_num_threads(4)
+BASE = ROOT/'output/20260915_171525_dynhamr'
+OUT = BASE/'diagnosis'
+OUT.mkdir(exist_ok=True)
+def stats(x):
+    x=np.asarray(x)
+    return dict(rms=float(np.sqrt(np.mean(x*x))), p95=float(np.percentile(x,95)), maximum=float(np.max(x)))
+def positions(x):
+    d=np.linalg.norm(np.diff(x,axis=0),axis=-1)*1000
+    dd=np.linalg.norm(np.diff(x,n=2,axis=0),axis=-1)*1000
+    return dict(step_mm=stats(d), second_difference_mm=stats(dd), top_step_frames=(np.argsort(d)[-8:][::-1]+1).tolist())
+def rotations(x):
+    r=Rotation.from_rotvec(x.reshape(-1,3)).as_matrix().reshape(x.shape[:-1]+(3,3))
+    inc=r[1:]@np.swapaxes(r[:-1],-1,-2)
+    a=Rotation.from_matrix(inc.reshape(-1,3,3)).magnitude()*180/np.pi
+    return stats(a)
+model=MANO(model_path=str(ROOT/'third_party/Dyn-HaMR/_DATA/data/mano'),batch_size=704,pose2rot=True)
+report={}
+saved={}
+for stage in ['smooth_fit','prior']:
+    path=sorted((BASE/'optimization'/stage).glob('*world_results.npz'))[-1]
+    arr=np.load(path)
+    for filtered in [False,True]:
+        d={k:torch.from_numpy(arr[k].copy()).float() for k in ['trans','root_orient','pose_body','is_right','betas']}
+        if filtered:
+            d['root_orient'],d['pose_body'],d['betas'],d['trans']=smooth_results(d['root_orient'],d['pose_body'],d['betas'],d['is_right'],d['trans'])
+        with torch.no_grad():
+            j=run_mano(model,d['trans'],d['root_orient'],d['pose_body'],d['is_right'],d['betas'])['joints'].numpy()
+        key=stage+('_render_filtered' if filtered else '_raw')
+        saved[key]=j
+        report[key]={}
+        for b in range(2):
+            side='right' if arr['is_right'][b,0]>.5 else 'left'
+            report[key][side]=dict(wrist_world=positions(j[b,:,0]),root_rotation_step_deg=rotations(d['root_orient'][b].numpy()),finger_rotation_step_deg=rotations(d['pose_body'][b].numpy().reshape(352,15,3)))
+        report[key]['relative_wrist']=positions(j[1,:,0]-j[0,:,0])
+        if stage=='prior':
+            R=arr['cam_R'][0]; t=arr['cam_t'][0]
+            for b in range(2):
+                side='right' if arr['is_right'][b,0]>.5 else 'left'
+                report[key][side]['wrist_camera']=positions(np.einsum('tij,tj->ti',R,j[b,:,0])+t)
+cam=np.load(ROOT/'output/20260915_171525/rgbd_camera.npz')
+report['camera_position']=positions(cam['c2w'][:,:3,3])
+report['camera_rotation_step_deg']=rotations(Rotation.from_matrix(cam['c2w'][:,:3,:3]).as_rotvec())
+report['caveat']='Frame differences include true motion; not a causal decomposition or accuracy metric. Render filtering replicated for static views; source camera render excludes translation filtering.'
+np.savez_compressed(OUT/'joints_diagnostic.npz',**saved,time=cam['time'])
+(OUT/'metrics.json').write_text(json.dumps(report,indent=2))
+print(json.dumps(report,indent=2))

scripts/diagnose_yesterday_hand_depth.py

@@ -0,0 +1,129 @@
+"""Held-out pixel depth residual of yesterday's (20260915_171525) Dyn-HaMR hand trajectory.
+
+Same method as scripts/compare_red_hand_depth.py: render MANO hand depth into the D405
+camera, select visible skin pixels, fit a single camera-ray translation on one half of an
+8x8 checkerboard, evaluate the residual on the other half. No smoothing, no articulation change.
+"""
+import os, sys, json
+os.environ.setdefault('OMP_NUM_THREADS', '4')
+from pathlib import Path
+ROOT = Path(__file__).resolve().parents[1]
+sys.path.insert(0, str(ROOT / 'third_party/Dyn-HaMR/dyn-hamr'))
+sys.path.insert(0, str(ROOT / 'third_party/FoundationPose'))
+import numpy as np, cv2, torch, nvdiffrast.torch as dr
+from body_model import MANO, run_mano
+from Utils import nvdiffrast_render
+
+SRC = ROOT / 'docs/20260915_171525'
+BASE = ROOT / 'output/20260915_171525_dynhamr'
+PRIOR = BASE / 'corrected/prior/20260915_171525_000000_world_results.npz'
+OUT = ROOT / 'output/hand_depth_diagnosis_20260915'; OUT.mkdir(exist_ok=True)
+torch.set_num_threads(4)
+meta = json.loads((SRC / 'intrinsics.json').read_text())
+K = np.array([[meta['fx'], 0, meta['cx']], [0, meta['fy'], meta['cy']], [0, 0, 1.]])
+H, W = 480, 848
+N = 352
+
+p = dict(np.load(PRIOR))
+model = MANO(model_path=str(ROOT / 'third_party/Dyn-HaMR/_DATA/data/mano'), batch_size=2 * N, pose2rot=True)
+with torch.no_grad():
+    o = run_mano(model, *[torch.tensor(p[k]).float() for k in ['trans', 'root_orient', 'pose_body', 'is_right', 'betas']])
+J = o['joints'].numpy(); V = o['vertices'].numpy()          # (2,N,J,3), (2,N,778,3) world, metres
+faces = None
+for attr in ['faces', 'faces_tensor']:
+    f = getattr(model, attr, None)
+    if f is None: f = getattr(getattr(model, 'bm', None), attr, None)
+    if f is not None:
+        faces = np.asarray(f.cpu() if torch.is_tensor(f) else f).astype(np.int32); break
+assert faces is not None and faces.max() < V.shape[2], 'MANO faces not found'
+R = p['cam_R'][0]; ct = p['cam_t'][0]
+Vc = np.einsum('tij,btvj->btvi', R, V) + ct[None, :, None, :]
+Jc = np.einsum('tij,btkj->btki', R, J) + ct[None, :, None, :]
+sides = ['right' if p['is_right'][b, 0] > .5 else 'left' for b in range(2)]
+
+ctx = dr.RasterizeCudaContext(); eye = torch.eye(4, device='cuda')[None]
+ft = torch.as_tensor(faces, device='cuda', dtype=torch.int32)
+def render(v):
+    mt = {'pos': torch.as_tensor(v, device='cuda', dtype=torch.float32), 'faces': ft,
+          'vnormals': torch.zeros((len(v), 3), device='cuda'), 'vertex_color': torch.ones((len(v), 3), device='cuda')}
+    with torch.inference_mode():
+        _, d, _ = nvdiffrast_render(K=K, H=H, W=W, ob_in_cams=eye, glctx=ctx, mesh_tensors=mt)
+    return d[0].cpu().numpy()
+
+yy, xx = np.indices((H, W)); train = ((xx // 8 + yy // 8) % 2) == 0
+def stat(x):
+    x = np.asarray([v for v in x if v is not None], float); x = x[np.isfinite(x)]
+    return {'n': int(len(x)), 'median': float(np.median(x)), 'p5': float(np.percentile(x, 5)), 'p95': float(np.percentile(x, 95)), 'mean': float(np.mean(x))} if len(x) else None
+
+rows = {s: [] for s in sides}; deltas = np.zeros((2, N, 3)); supported = np.zeros((2, N), bool)
+cap = cv2.VideoCapture(str(SRC / 'color.mp4')); keep = {0, 60, 120, 175, 240, 300, 351}
+for t in range(N):
+    ok, bgr = cap.read(); assert ok
+    z = cv2.imread(str(SRC / 'depth' / f'{t:06d}.png'), -1).astype('float32') * meta['depth_scale_m']
+    ycc = cv2.cvtColor(bgr, cv2.COLOR_BGR2YCrCb); hsv = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV)
+    skin = cv2.inRange(ycc, np.array([0, 133, 77]), np.array([255, 173, 127])) > 0
+    red = ((hsv[:, :, 0] < 10) | (hsv[:, :, 0] > 170)) & (hsv[:, :, 1] > 115)
+    blue = (hsv[:, :, 0] > 95) & (hsv[:, :, 0] < 130) & (hsv[:, :, 1] > 115)
+    skin &= ~red & ~blue
+    d0 = [render(Vc[b, t]) for b in range(2)]
+    vis = []
+    for b in range(2):
+        other = d0[1 - b]
+        occluded = (other > .1) & (other < d0[b])
+        interior = cv2.erode((d0[b] > .1).astype('uint8'), np.ones((5, 5), np.uint8)) > 0
+        mask = skin & interior & (z > .1) & (z < .85) & ~occluded
+        fit = mask & train; err = z[fit] - d0[b][fit]
+        delta = float(np.median(err)) if len(err) else 0.; mad = float(np.median(np.abs(err - delta))) if len(err) else 1.
+        wz = float(Jc[b, t, 0, 2])
+        accept = len(err) >= 100 and mad < .025 and abs(delta) < .25 and wz > .1
+        r = {'frame': t, 'side': sides[b], 'wrist_z_m': wz, 'mask_pixels': int(mask.sum()), 'fit_pixels': int(fit.sum()),
+             'fit_mad_mm': mad * 1000, 'fit_shift_z_mm': delta * 1000, 'depth_supported': bool(accept),
+             'passes_yesterday_gate': bool(len(err) >= 25 and mad < .010 and abs(delta) < .025)}
+        test0 = mask & ~train & (d0[b] > .1)
+        r['heldout_pixels'] = int(test0.sum())
+        r['signed_residual_before_mm'] = float(np.median(z[test0] - d0[b][test0]) * 1000) if test0.any() else None
+        r['abs_residual_before_mm'] = float(np.median(np.abs(z[test0] - d0[b][test0])) * 1000) if test0.any() else None
+        d1 = d0[b]
+        if accept:
+            deltas[b, t] = Jc[b, t, 0] / Jc[b, t, 0, 2] * delta; supported[b, t] = True
+            d1 = render(Vc[b, t] + deltas[b, t])
+            test1 = mask & ~train & (d1 > .1)
+            r['abs_residual_after_mm'] = float(np.median(np.abs(z[test1] - d1[test1])) * 1000) if test1.any() else None
+            r['translation_norm_mm'] = float(np.linalg.norm(deltas[b, t]) * 1000)
+        else:
+            r['abs_residual_after_mm'] = None; r['translation_norm_mm'] = None
+        rows[sides[b]].append(r); vis.append((d0[b], d1))
+    if t in keep:
+        img = bgr.copy()
+        for b, col in enumerate([(255, 128, 0), (0, 200, 255)]):
+            for d, thick in [(vis[b][0], 1), (vis[b][1], 2)]:
+                cnts, _ = cv2.findContours((d > .1).astype('uint8'), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+                cv2.drawContours(img, cnts, -1, col, thick)
+        cv2.putText(img, f'frame {t}  thin=before thick=after  orange={sides[0]} yellow={sides[1]}', (8, 20), cv2.FONT_HERSHEY_SIMPLEX, .5, (255, 255, 255), 1)
+        cv2.imwrite(str(OUT / f'overlay_{t:04d}.jpg'), img)
+    if t % 50 == 0: print('frame', t, flush=True)
+cap.release()
+
+summary = {'method': 'Same as depth_ablation_red_20260916: rendered MANO depth vs D405 depth on visible skin pixels; single camera-ray translation fit on checkerboard half, held-out on other half. Hands rendered jointly, mutual occlusion excluded.',
+           'input': str(PRIOR.relative_to(ROOT)), 'frames': N, 'hands': {}}
+seg = [(0, 117), (118, 235), (236, 351)]
+for b, s in enumerate(sides):
+    rs = rows[s]; sup = [r for r in rs if r['depth_supported']]
+    summary['hands'][s] = {
+        'depth_supported_frames': len(sup),
+        'frames_passing_yesterday_gate': int(sum(r['passes_yesterday_gate'] for r in rs)),
+        'fit_pixels': stat([r['fit_pixels'] for r in rs]),
+        'fit_mad_mm': stat([r['fit_mad_mm'] for r in rs]),
+        'wrist_z_m': stat([r['wrist_z_m'] for r in rs]),
+        'heldout_abs_residual_before_mm': stat([r['abs_residual_before_mm'] for r in rs]),
+        'heldout_signed_residual_before_mm': stat([r['signed_residual_before_mm'] for r in rs]),
+        'heldout_abs_residual_after_mm': stat([r['abs_residual_after_mm'] for r in sup]),
+        'fit_shift_z_mm': stat([r['fit_shift_z_mm'] for r in sup]),
+        'translation_norm_mm': stat([r['translation_norm_mm'] for r in sup]),
+        'frames_shift_over_25mm': int(sum(abs(r['fit_shift_z_mm']) > 25 for r in sup)),
+        'shift_z_by_segment_mm': {f'{a}-{c}': stat([r['fit_shift_z_mm'] for r in sup if a <= r['frame'] <= c]) for a, c in seg},
+    }
+np.savez_compressed(OUT / 'hand_depth_fit.npz', translation_camera=deltas, depth_supported=supported, sides=np.array(sides), K=K)
+(OUT / 'frame_metrics.json').write_text(json.dumps(rows, indent=1))
+(OUT / 'summary.json').write_text(json.dumps(summary, indent=2))
+print(json.dumps(summary, indent=2, ensure_ascii=False))

scripts/download_foundationpose_weights.py

@@ -0,0 +1,33 @@
+"""Download and verify the FoundationPose checkpoints listed in configs/foundationpose_weights_manifest.json.
+
+Files go to third_party/FoundationPose/weights/<run>/{config.yml, model_best.pth}; resumable (Range requests), SHA-256 checked.
+The manifest URLs point at a community Hugging Face mirror (official Google Drive quota was exceeded when the snapshot was made);
+replace `url` entries with the official links if you have them. Usage: .venv/bin/python scripts/download_foundationpose_weights.py
+"""
+import hashlib, json, time
+from pathlib import Path
+import requests
+
+ROOT = Path(__file__).resolve().parents[1]
+manifest = json.loads((ROOT / 'configs/foundationpose_weights_manifest.json').read_text())
+dest = ROOT / 'third_party/FoundationPose/weights'
+for item in manifest['files']:
+    path = dest / item['path']; path.parent.mkdir(parents=True, exist_ok=True); tmp = path.with_suffix(path.suffix + '.part')
+    if path.exists() and path.stat().st_size == item['size'] and hashlib.sha256(path.read_bytes()).hexdigest() == item['sha256']:
+        print('ok      ', item['path']); continue
+    for attempt in range(5):
+        try:
+            offset = tmp.stat().st_size if tmp.exists() else 0
+            print(f'download {item["path"]} from byte {offset}', flush=True)
+            with requests.get(item['url'], headers={'Range': f'bytes={offset}-{item["size"] - 1}'}, stream=True, timeout=(20, 60)) as r:
+                r.raise_for_status()
+                if offset and r.status_code != 206: offset = 0
+                with tmp.open('ab' if offset else 'wb') as f:
+                    for chunk in r.iter_content(1024 * 1024): f.write(chunk)
+            if tmp.stat().st_size == item['size']: tmp.replace(path); break
+        except requests.RequestException as e:
+            print('retry:', type(e).__name__, flush=True); time.sleep(3)
+    assert path.exists() and path.stat().st_size == item['size'], f'incomplete: {path}'
+    digest = hashlib.sha256(path.read_bytes()).hexdigest(); assert digest == item['sha256'], f'sha256 mismatch: {path}'
+    print('verified', item['path'], flush=True)
+(dest / 'download_manifest.json').write_text(json.dumps(manifest, indent=2)); print('all FoundationPose weights present in', dest)

scripts/evaluate_yesterday_spider.py

@@ -0,0 +1,56 @@
+"""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)

scripts/export_camera_delivery.py

@@ -0,0 +1,214 @@
+"""Package the 1333-frame replay cameras, source cameras and frame mapping."""
+import os
+os.environ.setdefault('MUJOCO_GL', 'osmesa')
+import csv
+import hashlib
+import json
+from pathlib import Path
+import shutil
+import subprocess
+import tarfile
+
+import cv2
+import mujoco
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+ROOT = Path(__file__).resolve().parents[1]
+OUT = ROOT/'output/camera_delivery_1333'
+
+
+def save_json(path, value):
+    path.write_text(json.dumps(value, ensure_ascii=False, indent=2)+'\n')
+
+
+def pose(R, t):
+    result = np.broadcast_to(np.eye(4), (*np.asarray(R).shape[:-2],4,4)).copy()
+    result[..., :3,:3] = R
+    result[..., :3,3] = t
+    return result
+
+
+def probe(path):
+    return json.loads(subprocess.check_output([
+        'ffprobe','-v','error','-select_streams','v:0','-show_streams',
+        '-show_frames','-show_entries',
+        'stream=width,height,avg_frame_rate,time_base,nb_frames,start_time,duration:frame=best_effort_timestamp,best_effort_timestamp_time',
+        '-of','json',str(path)]))
+
+
+def main():
+    OUT.mkdir(exist_ok=True)
+    replay = ROOT/'output/l20_full_replay'
+    original = Path('/home/timessage/下载/2047635068.mp4')
+    final = np.load(replay/'motion.npz')
+    temporal = np.load(ROOT/'output/l20_2047635068/jitter_audit/temporal/motion.npz')
+    reconstruction = np.load(ROOT/'output/results_2047635068/world_results.npz')
+    vipe_k = np.load(ROOT/'output/vipe_2047635068/intrinsics/2047635068.npz')
+    vipe_pose = np.load(ROOT/'output/vipe_2047635068/pose/2047635068.npz')
+    N = len(final['qpos'])
+    assert N == 1333 and np.array_equal(vipe_k['inds'],np.arange(N))
+    assert np.array_equal(vipe_pose['inds'],np.arange(N))
+    video_meta = {name:probe(path) for name,path in {
+        'original':original,'full_replay':replay/'full_replay.mp4',
+        'comparison':replay/'original_vs_mujoco.mp4'}.items()}
+    for value in video_meta.values():
+        assert len(value['frames']) == N
+    timestamps = {name:np.array([float(x['best_effort_timestamp_time']) for x in value['frames']])
+                  for name,value in video_meta.items()}
+    with (OUT/'frame_mapping.csv').open('w') as f:
+        writer=csv.writer(f)
+        writer.writerow(['hdf5_frame_0based','source_video_frame_0based','source_extracted_jpg',
+                         'source_pts_ticks','source_pts_s','hdf5_time_s','full_replay_pts_s','comparison_pts_s'])
+        for i in range(N):
+            writer.writerow([i,i,f'{i+1:06d}.jpg',video_meta['original']['frames'][i]['best_effort_timestamp'],
+                             timestamps['original'][i],final['time'][i],timestamps['full_replay'][i],timestamps['comparison'][i]])
+    # Reconstruct the fixed world transforms from the actual build script.
+    source_R = Rotation.from_quat(temporal['wrist_quat_wxyz'][:,[1,2,3,0]])
+    bottle_R = source_R * Rotation.from_euler('x',-np.pi/2)
+    bottle_p = temporal['wrist_pos'] + source_R.apply(np.tile([.12,-.065,.15],(N,1)))
+    align = bottle_R[600].inv()
+    shift = np.array([0,0,.3])-align.apply(bottle_p[600])
+    before_floor = align.apply(temporal['wrist_pos'])+shift+final['correction'][:,16:19]
+    residual = final['wrist_pos']-before_floor
+    floor_shift = residual.mean(axis=0)
+    assert np.max(np.abs(residual-floor_shift)) < 1e-9
+    assert np.max(np.abs(floor_shift[:2])) < 1e-9
+    final_from_temporal = pose(align.as_matrix(),shift+floor_shift)
+    temporal_from_dyn = pose(temporal['scene_rotation'],temporal['scene_translation'])
+    final_from_dyn = final_from_temporal @ temporal_from_dyn
+    track = int(np.load(ROOT/'output/dex_2047635068/human_joints.npz')['source_track'])
+    camera_from_dyn = pose(reconstruction['cam_R'][track],reconstruction['cam_t'][track])
+    camera_from_final = camera_from_dyn @ np.linalg.inv(final_from_dyn)
+    final_from_camera = np.linalg.inv(camera_from_final)
+    # world_results cam_t already contains world_scale; do not scale twice.
+    K4 = reconstruction['intrins'].astype(float)
+    K = np.array([[K4[0],0,K4[2]],[0,K4[1],K4[3]],[0,0,1.]])
+    np.savez_compressed(OUT/'source_camera.npz',
+                        frame_index=np.arange(N), time=final['time'], intrinsics_fx_fy_cx_cy=vipe_k['data'],
+                        K=K, camera_from_final_world=camera_from_final,
+                        final_world_from_camera=final_from_camera,
+                        camera_from_dynhamr_world=camera_from_dyn,
+                        final_world_from_dynhamr_world=final_from_dyn,
+                        vipe_world_from_camera_raw=vipe_pose['data'])
+    with (OUT/'source_intrinsics.csv').open('w') as f:
+        w=csv.writer(f);w.writerow(['frame_0based','fx','fy','cx','cy','width','height'])
+        for i,row in enumerate(vipe_k['data']):w.writerow([i,*row,1280,720])
+    with (OUT/'source_extrinsics.csv').open('w') as f:
+        w=csv.writer(f);w.writerow(['frame_0based']+[f'camera_from_final_world_{i}{j}' for i in range(4) for j in range(4)])
+        for i,T in enumerate(camera_from_final):w.writerow([i,*T.ravel()])
+    source_config = dict(width=1280,height=720,fps=30,K=K.tolist(),
+                         model='estimated pinhole',calibration='ViPE/Dyn-HaMR estimate, not measured calibration',
+                         distortion_coefficients=None,undistortion='No explicit lens undistortion found in this inference path; capture-device processing unknown.',
+                         preprocessing='Input video and extracted JPG are 1280x720; no full-image crop/resize observed in those inputs. Network-internal crops/resizes do not redefine this full-image K.',
+                         camera_axes='OpenCV: +X right, +Y down, +Z forward; pixel coordinates u right, v down',
+                         world_axes='Final scene.xml right-handed display world, Z display up; physical gravity not calibrated',
+                         transform_convention='column vectors: p_camera = camera_from_final_world @ p_final_world; inverse is final_world_from_camera',
+                         world_scale_already_applied=float(reconstruction['world_scale'].item()),
+                         intrinsics_max_variation=float(np.ptp(vipe_k['data'],axis=0).max()),
+                         warning='Contact registration changes hand motion per frame. This camera transform preserves the fixed world change, not inverse contact fitting. Exact original-video hand overlap is not guaranteed.')
+    save_json(OUT/'source_camera.json',source_config)
+    save_json(OUT/'world_transforms.json',dict(final_world_from_dynhamr_world=final_from_dyn.tolist(),
+                  temporal_world_from_dynhamr_world=temporal_from_dyn.tolist(),
+                  final_world_from_temporal_world=final_from_temporal.tolist(),
+                  recovered_floor_translation=floor_shift.tolist(),
+                  per_frame_contact_correction_included_in_camera=False,
+                  derivation='scripts/build_l20_full_replay.py: alignment using bottle orientation at frame 600, shift to [0,0,.3], then floor clearance. Floor shift recovered from saved wrist minus fixed transform minus saved contact correction, verified constant across all frames.'))
+    # Recover actual mono renderer cameras, including the moving close-up lookat.
+    m=mujoco.MjModel.from_xml_path(str(replay/'scene.xml'));d=mujoco.MjData(m)
+    option=mujoco.MjvOption();option.sitegroup[:]=0;option.geomgroup[3:]=0
+    scene=mujoco.MjvScene(m,maxgeom=10000)
+    cameras=[]
+    for distance,azimuth,elevation in [(1.05,135,-23),(.62,35,-12)]:
+        c=mujoco.MjvCamera();c.lookat[:]=final['camera_center'];c.distance=distance;c.azimuth=azimuth;c.elevation=elevation;cameras.append(c)
+    arrays={name:[] for name in ['wide_world_from_camera_cv','close_world_from_camera_cv','close_lookat','wide_gl_pos','close_gl_pos']}
+    for row in final['qpos']:
+        d.qpos[:]=row;mujoco.mj_forward(m,d)
+        cameras[1].lookat[:]=(d.xpos[m.body('hand_base_link').id]+d.xpos[m.body('right_object').id])/2
+        cameras[1].lookat[2]+=.06
+        arrays['close_lookat'].append(cameras[1].lookat.copy())
+        for name,c in zip(['wide','close'],cameras):
+            mujoco.mjv_updateScene(m,d,option,None,c,mujoco.mjtCatBit.mjCAT_ALL,scene)
+            gl=mujoco.mjv_averageCamera(scene.camera[0],scene.camera[1])
+            forward=gl.forward.astype(float);forward/=np.linalg.norm(forward)
+            right=np.cross(forward,gl.up);right/=np.linalg.norm(right)
+            up=np.cross(right,forward)
+            arrays[name+'_world_from_camera_cv'].append(pose(np.column_stack([right,-up,forward]),gl.pos))
+            arrays[name+'_gl_pos'].append(gl.pos.copy())
+    arrays={k:np.asarray(v) for k,v in arrays.items()}
+    fy=720/(2*np.tan(np.deg2rad(float(m.vis.global_.fovy))/2))
+    render_K=np.array([[fy,0,320],[0,fy,360],[0,0,1.]])
+    np.savez_compressed(OUT/'render_cameras.npz',frame_index=np.arange(N),time=final['time'],
+                        K_panel=render_K,**arrays,
+                        wide_camera_from_world_cv=np.linalg.inv(arrays['wide_world_from_camera_cv']),
+                        close_camera_from_world_cv=np.linalg.inv(arrays['close_world_from_camera_cv']))
+    save_json(OUT/'render_camera_config.json',dict(mujoco_version=mujoco.__version__,
+        panel_width=640,panel_height=720,full_video_width=1280,full_video_height=720,
+        vertical_fov_degrees=float(m.vis.global_.fovy),K_panel=render_K.tolist(),
+        pixel_convention='K uses continuous image-edge coordinates, pixel centers at (i+0.5,j+0.5); subtract 0.5 from cx/cy for integer pixel-center indexing.',
+        wide=dict(lookat=final['camera_center'].tolist(),distance=1.05,azimuth=135,elevation=-23,panel_x=0),
+        close=dict(lookat='(hand_base_link.xpos + right_object.xpos)/2 + [0,0,.06], per frame',distance=.62,azimuth=35,elevation=-12,panel_x=640),
+        stereo='monoscopic, average of MjvScene left/right GL cameras',
+        option=dict(sitegroup='all zero',geomgroup_3_and_above='zero'),
+        banners=dict(top_y=[0,64],bottom_y=[690,720]),
+        comparison_video=dict(width=1600,height=720,
+             original_panel='source scaled 1280x720 -> 960x540, offset [0,90]',
+             render_panel='full_replay crop x=640,y=64,w=640,h=626, then pad y=64 and place x=960',
+             source_image_to_comparison=[[.75,0,0],[0,.75,90],[0,0,1]],
+             close_panel_to_comparison=[[1,0,960],[0,1,0],[0,0,1]])))
+    # Check time correspondence, coordinate algebra and a rendered frame.
+    sample_frames=[0,160,600,1332]
+    pixel_errors=[]
+    cap=cv2.VideoCapture(str(original))
+    for i in sample_frames:
+        cap.set(cv2.CAP_PROP_POS_FRAMES,i);ok,im=cap.read();assert ok
+        jpg=cv2.imread(str(ROOT/f'third_party/Dyn-HaMR/test/images/2047635068/{i+1:06d}.jpg'))
+        assert jpg.shape==im.shape
+        pixel_errors.append(float(np.mean(np.abs(im.astype(float)-jpg.astype(float)))))
+    cap.release()
+    renderer=mujoco.Renderer(m,height=720,width=640)
+    render_errors=[]
+    cap=cv2.VideoCapture(str(replay/'full_replay.mp4'))
+    for i in [0,600,1332]:
+        d.qpos[:]=final['qpos'][i];mujoco.mj_forward(m,d)
+        cameras[1].lookat[:]=arrays['close_lookat'][i]
+        panels=[]
+        for c in cameras:
+            renderer.update_scene(d,camera=c,scene_option=option);panels.append(renderer.render().copy())
+        generated=np.concatenate(panels,axis=1)
+        cap.set(cv2.CAP_PROP_POS_FRAMES,i);ok,video=cap.read();assert ok
+        video=cv2.cvtColor(video,cv2.COLOR_BGR2RGB)
+        error=float(np.mean(np.abs(generated[65:689].astype(float)-video[65:689].astype(float))))
+        render_errors.append(error)
+        if i==0:cv2.imwrite(str(OUT/'render_check_frame_0000.png'),cv2.cvtColor(generated,cv2.COLOR_RGB2BGR))
+    renderer.close();cap.release()
+    assert max(render_errors)<8,render_errors
+    assert max(pixel_errors)<8,pixel_errors
+    points=np.column_stack([np.linspace(0,.1,N),np.ones(N)*.2,np.ones(N),np.ones(N)])
+    lhs=np.einsum('tij,tj->ti',camera_from_dyn,points)
+    rhs=np.einsum('tij,tj->ti',camera_from_final,points@final_from_dyn.T)
+    validation=dict(frames=N,source_to_hdf5_time_max_error_s=float(np.max(np.abs(timestamps['original']-final['time']))),
+                    world_transform_roundtrip_max_error=float(np.max(np.abs(lhs-rhs))),
+                    floor_shift_consistency_max_error_m=float(np.max(np.abs(residual-floor_shift))),
+                    source_jpg_sample_frames=sample_frames,source_jpg_mean_absolute_pixel_error=pixel_errors,
+                    rerender_checked_frames=[0,600,1332],rerender_mean_absolute_pixel_error=render_errors,
+                    cameras_finite=bool(np.isfinite(camera_from_final).all()),
+                    calibration_is_measured=False,source_distortion_calibration_available=False)
+    assert validation['source_to_hdf5_time_max_error_s']<1e-6
+    assert validation['world_transform_roundtrip_max_error']<1e-8
+    save_json(OUT/'validation.json',validation)
+    save_json(OUT/'video_metadata.json',{k:v['streams'][0] for k,v in video_meta.items()})
+    target=OUT/'replay';target.mkdir(exist_ok=True)
+    for name in ['scene.xml','motion.npz','full_replay.mp4','original_vs_mujoco.mp4']:
+        shutil.copy2(replay/name,target/name)
+    shutil.copytree(replay/'assets',target/'assets',dirs_exist_ok=True)
+    shutil.copy2(original,OUT/'original_2047635068.mp4')
+    shutil.copy2(ROOT/'scripts/play_l20_full.py',OUT/'play_l20_full.py')
+    shutil.copy2(replay/'make_comparison.sh',OUT/'make_comparison_original.sh')
+    shutil.copy2(ROOT/'scripts/build_l20_full_replay.py',OUT/'build_l20_full_replay_reference.py')
+    shutil.copy2(ROOT/'output/hdf5_delivery/right_1333/demonstrations_right.hdf5',OUT/'demonstrations_right.hdf5')
+    print(json.dumps(validation,indent=2))
+
+
+if __name__ == '__main__':
+    main()

scripts/export_dynhamr_bimanual_rgbd.py

@@ -0,0 +1,18 @@
+"""Export corrected Dyn-HaMR world-oriented, wrist-relative joints for both L20 hands."""
+from pathlib import Path
+import json,shutil,numpy as np
+ROOT=Path(__file__).resolve().parents[1]; BASE=ROOT/'output/20260915_171525_dynhamr'
+source=BASE/'corrected/prior/20260915_171525_000000_world_results.npz'
+a=np.load(source); joints=np.load(BASE/'corrected/comparison_joints.npz')['after']; camera=np.load(ROOT/'output/20260915_171525/rgbd_camera.npz')
+assert joints.shape==(2,352,21,3) and np.isfinite(joints).all()
+for b in range(2):
+ side='right' if a['is_right'][b,0]>.5 else 'left'
+ assert np.all(a['is_right'][b]==a['is_right'][b,0])
+ track=BASE/f'dataset/dynhamr/track_preds/20260915_171525/{b:03d}'
+ valid=np.array([(track/f'{t+1:06d}_keypoints.json').is_file() for t in range(352)])
+ np.savez_compressed(BASE/f'human_joints_{side}.npz',joints=joints[b]-joints[b,:,:1],wrist_world=joints[b,:,0],root_orient=np.zeros((352,3)),fps=30.,time=camera['time'],detection_valid=valid,source=str(source),side=side,coordinate_note='World-oriented wrist-relative joints from exact Dyn-HaMR run_mano, including left reflection; root rotation identity because joint directions already carry world orientation.',valid_semantics='Upstream keypoint file available; not per-joint confidence or depth acceptance.',depth_correction_applied=False)
+ print(side,'frames',len(joints[b]),'track available',valid.sum())
+for name in ['rgbd_camera.npz','object_poses.npz','object_fit_validation.json']:
+ shutil.copy2(ROOT/'output/20260915_171525'/name,BASE/name)
+shutil.copytree(ROOT/'output/20260915_171525/object_assets',BASE/'object_assets',dirs_exist_ok=True)
+(BASE/'object_assets/colors.json').write_text(json.dumps({'upper':{'source':'上半.stl','rgba':[.8,.08,.05,1]},'lower':{'source':'下半.stl','rgba':[.05,.15,.65,1]},'note':'STL geometry is colorless; colors stored in replay scene.xml material/rgba.'},ensure_ascii=False,indent=2))

scripts/fit_rgbd_objects.py

@@ -0,0 +1,93 @@
+"""Estimate two colored object poses from RGB-D; partial-surface ICP, not pose truth."""
+import os
+os.environ.setdefault('OMP_NUM_THREADS','4')
+from pathlib import Path
+import json
+import cv2
+import numpy as np
+import open3d as o3d
+from scipy.spatial.transform import Rotation,Slerp
+from scipy.ndimage import gaussian_filter1d
+
+ROOT=Path(__file__).resolve().parents[1];SRC=ROOT/'docs/20260915_171525';BASE=ROOT/'output/20260915_171525'
+meta=json.loads((SRC/'intrinsics.json').read_text());cam=np.load(BASE/'rgbd_camera.npz');N=len(cam['time'])
+o3d.utility.random.seed(7)
+models={}
+for name in ['upper','lower']:
+    mesh=o3d.io.read_triangle_mesh(str(BASE/'object_assets'/f'{name}.stl'))
+    cloud=mesh.sample_points_uniformly(6000);cloud=cloud.voxel_down_sample(.003)
+    models[name]=(cloud,np.asarray(mesh.vertices).mean(0),mesh.get_axis_aligned_bounding_box().get_center())
+cap=cv2.VideoCapture(str(SRC/'color.mp4'));clouds={n:[] for n in models};centers={n:[] for n in models}
+for t in range(N):
+    ok,im=cap.read();assert ok
+    hsv=cv2.cvtColor(im,cv2.COLOR_BGR2HSV);z=cv2.imread(str(SRC/'depth'/f'{t:06d}.png'),-1)*meta['depth_scale_m']
+    for name in models:
+        hue=hsv[:,:,0];mask=((hue<12)|(hue>170)) if name=='upper' else ((hue>90)&(hue<135))
+        mask=(mask&(hsv[:,:,1]>100)&(hsv[:,:,2]>35)&(z>.12)&(z<.85)).astype(np.uint8)
+        # Restrict to the tabletop work area, then largest coherent colored component.
+        mask[:80]=0
+        count,labels,stats,_=cv2.connectedComponentsWithStats(mask,8)
+        if count<=1 or stats[1:,cv2.CC_STAT_AREA].max()<300:
+            clouds[name].append(None);centers[name].append(None);continue
+        selected=1+np.argmax(stats[1:,cv2.CC_STAT_AREA]);mask=(labels==selected).astype(np.uint8)
+        contours,_=cv2.findContours(mask,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
+        if name=='lower' and contours:
+            hull=cv2.convexHull(max(contours,key=cv2.contourArea));cv2.fillConvexPoly(mask,hull,1)
+            ycc=cv2.cvtColor(im,cv2.COLOR_BGR2YCrCb)
+            mask[cv2.inRange(ycc,np.array([0,133,77]),np.array([255,173,127]))>0]=0
+            mask[(z<=.12)|(z>=.85)]=0
+        mask=cv2.erode(mask,np.ones((3,3),np.uint8))>0
+        v,u=np.nonzero(mask);zz=z[v,u];points=np.c_[(u-meta['cx'])/meta['fx']*zz,(v-meta['cy'])/meta['fy']*zz,zz]
+        points=points@cam['c2w'][t,:3,:3].T+cam['c2w'][t,:3,3]
+        pc=o3d.geometry.PointCloud(o3d.utility.Vector3dVector(points)).voxel_down_sample(.004)
+        clouds[name].append(pc);centers[name].append(np.median(np.asarray(pc.points),axis=0))
+cap.release()
+arrays={};report={}
+for name,(model,_,model_center) in models.items():
+    poses=[];good=[];records=[];previous=None;previous_center=None
+    frames=np.unique(np.r_[np.arange(0,N,3),N-1])
+    for t in frames:
+        target=clouds[name][t]
+        if target is None:
+            poses.append(np.eye(4) if previous is None else previous.copy());good.append(False);records.append(dict(frame=int(t),accepted=False));continue
+        points=np.asarray(target.points);center=centers[name][t]
+        seeds=[]
+        if previous is not None:
+            seed=previous.copy();seed[:3,3]+=center-previous_center;seeds.append(seed)
+            # Update observed face normal continuously, preserving the previous
+            # in-plane direction rather than reselecting a symmetric CAD yaw.
+            plane,inliers=target.segment_plane(.004,3,80)
+            normal=np.asarray(plane[:3]);normal/=np.linalg.norm(normal)
+            if normal@previous[:3,1]<0:normal=-normal
+            x=previous[:3,0]-normal*(normal@previous[:3,0]);x/=np.linalg.norm(x)
+            rr=np.column_stack([x,normal,np.cross(x,normal)])
+            normal_seed=np.eye(4);normal_seed[:3,:3]=rr;normal_seed[:3,3]=center-rr@model_center;seeds.append(normal_seed)
+        if previous is None:
+            _,axes=np.linalg.eigh(np.cov(points.T));normal=axes[:,0];x=axes[:,2];zaxis=np.cross(x,normal)
+            for sign in [1,-1]:
+                basis=np.column_stack([x,normal*sign,zaxis*sign])
+                for yaw in [0,np.pi/2,np.pi,3*np.pi/2]:
+                    R=basis@Rotation.from_euler('y',yaw).as_matrix();T=np.eye(4);T[:3,:3]=R;T[:3,3]=center-R@model_center;seeds.append(T)
+        candidates=[]
+        for seed in seeds:
+            fit=o3d.pipelines.registration.registration_icp(target,model,.04,np.linalg.inv(seed),o3d.pipelines.registration.TransformationEstimationPointToPoint(),o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=35))
+            candidates.append((fit.inlier_rmse+(.1*(1-fit.fitness)),fit))
+        _,fit=min(candidates,key=lambda x:x[0]);T=np.linalg.inv(fit.transformation)
+        rotation_step=0. if previous is None else float(Rotation.from_matrix(previous[:3,:3].T@T[:3,:3]).magnitude())
+        accepted=bool(fit.fitness>.8 and fit.inlier_rmse<.015 and np.isfinite(T).all() and rotation_step<.7)
+        if accepted:previous=T;previous_center=center
+        poses.append(T if accepted or previous is None else previous.copy());good.append(accepted)
+        records.append(dict(frame=int(t),accepted=accepted,fitness=float(fit.fitness),rmse_m=float(fit.inlier_rmse),rotation_step_rad=rotation_step))
+        if t%60==0:print(name,t,'rmse',fit.inlier_rmse,'fitness',fit.fitness,flush=True)
+    poses=np.asarray(poses);good=np.asarray(good);ix=np.flatnonzero(good)
+    assert len(ix)>2,(name,len(ix))
+    knots=frames[ix];ts=np.clip(np.arange(N),knots[0],knots[-1]);p=np.stack([np.interp(ts,knots,poses[ix,k,3]) for k in range(3)],axis=1)
+    rot=Slerp(knots,Rotation.from_matrix(poses[ix,:3,:3]))(ts)
+    arrays[name+'_position_world']=gaussian_filter1d(p,1,axis=0)
+    arrays[name+'_quaternion_xyzw']=rot.as_quat()
+    arrays[name+'_keyframes']=frames;arrays[name+'_keyframe_valid']=good
+    # Interpolation is explicit; accepted ICP is geometric fit, not unique pose identification.
+    report[name]=dict(keyframes=len(frames),accepted_keyframes=int(good.sum()),median_rmse_m=float(np.median([r['rmse_m'] for r in records if r['accepted']])),frames=records)
+np.savez_compressed(BASE/'object_poses.npz',time=cam['time'],**arrays)
+report['boundary']='Red=upper, blue=lower assumed from recording. Fixed meter-scale STL, partial colored visible-surface ICP; pose symmetry and occlusion ambiguity unresolved. Interpolated between keyframes; not contact labels or ground truth.'
+(BASE/'object_fit_validation.json').write_text(json.dumps(report,indent=2));print(json.dumps({n:{k:v for k,v in r.items() if k!='frames'} for n,r in report.items() if isinstance(r,dict)},indent=2))

scripts/fix_yesterday_hand_collision.py

@@ -0,0 +1,118 @@
+"""Collision-constrained L20 retargeting: analytic contact Jacobians, exact mimic, verified steps."""
+import os
+os.environ.setdefault('MUJOCO_GL','osmesa')
+os.environ['OPENBLAS_NUM_THREADS']='1';os.environ['OMP_NUM_THREADS']='1'
+from pathlib import Path
+import json,copy,argparse,xml.etree.ElementTree as E
+import numpy as np,mujoco,trimesh
+from scipy.optimize import minimize
+from scipy.spatial.transform import Rotation as Rotation
+import l20_model_source as source
+R=Path(__file__).resolve().parents[1];OLD=Path(os.environ.get('HF_FIX_OLD',R/'output/foundationpose_spider_20260915'));OUT=Path(os.environ.get('HF_FIX_OUT',R/'output/collision_fix_20260915'));T=OUT/'datasets/processed/current/l20/bimanual/boxes';(T/'0').mkdir(parents=True,exist_ok=True)
+p=argparse.ArgumentParser();p.add_argument('--legacy-geoms',action='store_true');p.add_argument('--pilot',action='store_true');args=p.parse_args()
+root=E.parse(OLD/'datasets/processed/current/l20/bimanual/boxes/scene_act.xml').getroot();assets=root.find('asset')
+if not args.legacy_geoms:
+ manifest=json.loads((OUT/'collision_v2/manifest.json').read_text());handmanifest=json.loads((OUT/'hand_collision/manifest.json').read_text());assert len(manifest)==2 and len(handmanifest)==4
+ for name,side in [('upper','right'),('lower','left')]:
+  body=root.find(f"worldbody/body[@name='{side}_object']")
+  for g in list(body):
+   if g.tag=='geom' and '_collision_' in g.get('name',''):body.remove(g)
+  for a in list(assets):
+   if a.get('name','').startswith(name+'_collision_'):assets.remove(a)
+  for i,path in enumerate(sorted((OUT/'collision_v2').glob(name+'_*.obj'))):
+   part=trimesh.load(path)
+   if abs(part.volume)<1e-12 or np.linalg.matrix_rank(part.vertices-part.vertices.mean(0),tol=1e-9)<3:continue
+   n=f'{name}_collision_{i}';E.SubElement(assets,'mesh',name=n,file=str(path),maxhullvert='128');E.SubElement(body,'geom',name=n,type='mesh',mesh=n,contype='2',conaffinity='7',friction='.8 .005 .001',condim='3',solref='.006 1',solimp='.95 .99 .001',mass='0',group='3')
+ for body in root.findall('.//body'):
+  for g in list(body.findall('geom')):
+   mesh=g.get('mesh')
+   if mesh not in handmanifest:continue
+   g.set('contype','0');g.set('conaffinity','0')
+   for i,path in enumerate(sorted((OUT/'hand_collision').glob(mesh+'_*.obj'))):
+    n=f'{mesh}_part_{i}';E.SubElement(assets,'mesh',name=n,file=str(path),maxhullvert='128');attrs=dict(g.attrib);attrs.update(name=n,mesh=n,contype='1',conaffinity='2',group='3',mass='0',solref='.006 1',solimp='.95 .99 .001');E.SubElement(body,'geom',**attrs)
+TABLE=os.environ.get('HF_TABLE','0')=='1';PAIRS=[{1,2},{1,4}] if TABLE else [{1,2}]
+if TABLE:
+ for fl in root.findall(".//geom[@name='right_floor']"):fl.set('conaffinity','3')   # hands (contype 1) now collide with the table
+# Keep contacts within 4 mm available to the solver, without early physical forces.
+for g in root.findall('.//geom'):
+ if int(g.get('contype','0')) in ([1,2,4] if TABLE else [1,2]):g.attrib.update(margin='.004',gap='.004',solref='.005 1',solimp='.95 .99 .001')
+scene=T/('scene_legacy.xml' if args.legacy_geoms else 'scene_act.xml');E.ElementTree(root).write(scene)
+m=mujoco.MjModel.from_xml_path(str(scene));d=mujoco.MjData(m);source.REPO_ROOT=R/'third_party/l20_assets'
+oldref=np.load(OLD/'reference_video_rate.npz');original=np.load(OLD/'reference_before_contact_fit.npz')['qpos'];q=oldref['qpos'].copy();cp=oldref['contact_pos'];contact=oldref['contact'];N=len(q);meta={}
+for side in ['right','left']:
+ k=source.HandKinematics(OLD/('model_'+side)/('l20_'+side+'.xml'),side);wn=[side+'_hand_'+s for s in ['pos_x','pos_y','pos_z','rot_x','rot_y','rot_z']];wa=np.array([m.jnt_qposadr[m.joint(n).id] for n in wn]);ja=np.array([m.jnt_qposadr[m.joint(side+'_'+n).id] for n in k.joint_names]);B=np.zeros((m.nv,22));B[wa,:6]=np.eye(6);B[ja,6:]=k.expansion
+ meta[side]=(k,wa,ja,B,[m.site(side+'_hand_'+f+'_track').id for f in source.FINGERS])
+# All hand/object geometry is used, rather than only tip points.
+def collision_rows(side,B,derivatives=True):
+ rows=[];dist=[]
+ for c in d.contact:
+  g0,g1=map(int,c.geom);types={int(m.geom_contype[g0]),int(m.geom_contype[g1])}
+  if types not in PAIRS:continue
+  hg=g0 if m.geom_contype[g0]==1 else g1
+  if not m.geom(hg).name.startswith(side+'_'):continue
+  dist.append(float(c.dist))
+  if derivatives:
+   jac=np.zeros((3,m.nv));mujoco.mj_jac(m,d,jac,None,c.pos,int(m.geom_bodyid[hg]));normal=c.frame[:3]*(1 if hg==g1 else -1);rows.append(normal@jac@B)
+ return np.asarray(rows).reshape(-1,22),np.array(dist)
+def put(x,base,k,wa,ja):
+ d.qpos[:]=base;d.qpos[wa]=x[:6];d.qpos[ja]=k.expand(x[6:]);mujoco.mj_fwdPosition(m,d)
+frames=[0,36,46,80,160,240,351] if args.pilot else range(N);report=[];previous={};margin=float(os.environ.get('HF_MARGIN','.0005'))
+for f in frames:
+ for si,side in enumerate(['right','left']):
+  k,wa,ja,B,sites=meta[side];base=q[f].copy();xorig=np.r_[original[f,wa],original[f,ja][k.independent_indices]];x=np.r_[q[f,wa],q[f,ja][k.independent_indices]]
+  low=np.r_[xorig[:3]-.15,xorig[3:6]-.65,k.lower];high=np.r_[xorig[:3]+.15,xorig[3:6]+.65,k.upper];x=np.clip(x,low,high)
+  target=cp[f,si*5:si*5+5];on=contact[f,si*5:si*5+5].astype(bool);prior=xorig.copy()
+  if side in previous and not args.pilot:
+   prior=np.clip(xorig+previous[side]*(1. if on.any() else .95),low,high);x=prior.copy()
+   prev=np.r_[q[f-1,wa],q[f-1,ja][k.independent_indices]]
+   temporal=np.r_[[.012]*3,[.18]*3,[.25]*16];low=np.maximum(low,prev-temporal);high=np.minimum(high,prev+temporal)
+   # Transport the previous hand with its currently contacted object to stay on the same side.
+   if on.any():
+    d.qpos[:]=base;mujoco.mj_kinematics(m,d);center=target[on].mean(0);choices=[54] if f>=177 else [54,60]
+    start=min(choices,key=lambda j:np.linalg.norm(center-d.site_xpos[m.site(('right' if j==54 else 'left')+'_object_track').id]))
+    old_obj=q[f-1,start:start+6];new_obj=q[f,start:start+6];delta=Rotation.from_euler('XYZ',new_obj[3:])*Rotation.from_euler('XYZ',old_obj[3:]).inv()
+    x[:3]=delta.apply(prev[:3]-old_obj[:3])+new_obj[:3]
+    e=(delta*Rotation.from_euler('XYZ',prev[3:6])).as_euler('XYZ');candidates=np.array([e,[e[0]+np.pi,np.pi-e[1],e[2]+np.pi]]);candidates+=2*np.pi*np.round((prev[3:6]-candidates)/(2*np.pi));x[3:6]=candidates[np.argmin(np.linalg.norm(candidates-prev[3:6],axis=1))];x[6:]=prev[6:]
+   x=np.clip(x,low,high)
+  put(x,base,k,wa,ja);_,dist=collision_rows(side,B,False);initial=max(0,-dist.min()) if len(dist) else 0
+  # Exit deep or contradictory overlap first; no object pose changes or mesh shrinking.
+  if initial>.004 and (f==0 or args.pilot):
+   seeds=[x.copy()];directions=np.array([[i,j,z] for i in [-1,0,1] for j in [-1,0,1] for z in [-1,0,1] if (i,j,z)!=(0,0,0)],float);directions/=np.linalg.norm(directions,axis=1)[:,None]
+   for radius in [.025,.05,.08,.12]:
+    for direction in directions:
+     seed=x.copy();seed[:3]=np.clip(x[:3]+direction*radius,low[:3],high[:3]);seeds.append(seed)
+   best=None
+   for seed in seeds:
+    put(seed,base,k,wa,ja);_,ds=collision_rows(side,B,False);pen=max(0,-ds.min()) if len(ds) else 0;gap=np.linalg.norm(d.site_xpos[sites][on]-target[on],axis=1).mean() if on.any() else 0
+    score=500*pen+np.linalg.norm(seed[:3]-xorig[:3])+gap*.3
+    if best is None or score<best[0]:best=(score,seed.copy())
+   x=best[1]
+  weights=np.r_[[float(os.environ.get('HF_WRIST_WEIGHT','25'))]*3,[1.5]*3,[.22]*16];bounds_step=np.r_[[.015]*3,[.10]*3,[.18]*16]
+  for it in range(45):
+   put(x,base,k,wa,ja);A,ds=collision_rows(side,B);J=[];res=[]
+   for tip in np.flatnonzero(on):
+    jac=np.zeros((3,m.nv));mujoco.mj_jacSite(m,d,jac,None,int(sites[tip]));J.extend((jac@B)/.012);res.extend((d.site_xpos[sites[tip]]-target[tip])/.012)
+   J=np.asarray(J).reshape(-1,22);res=np.array(res);H=np.diag(weights**2)+J.T@J;g=weights**2*(x-prior)+J.T@res
+   lo=np.maximum(low-x,-bounds_step);hi=np.minimum(high-x,bounds_step)
+   constraints=[] if not len(ds) else [dict(type='ineq',fun=lambda z:A@z+ds-margin,jac=lambda z:A)]
+   fit=minimize(lambda z:.5*z@H@z+g@z,np.zeros(22),jac=lambda z:H@z+g,bounds=list(zip(lo,hi)),constraints=constraints,method='SLSQP',options={'maxiter':80,'ftol':1e-9})
+   dx=fit.x;pen=max(0.,-ds.min()) if len(ds) else 0.;oldcost=.5*np.sum((weights*(x-prior))**2)+.5*(res@res)+1e5*pen
+   chosen=None
+   for alpha in [1.,.5,.25,.1]:
+    trial=np.clip(x+alpha*dx,low,high);put(trial,base,k,wa,ja);_,td=collision_rows(side,B,False);tp=max(0.,-td.min()) if len(td) else 0.;rr=(d.site_xpos[sites][on]-target[on]).ravel()/.012;cost=.5*np.sum((weights*(trial-prior))**2)+.5*(rr@rr)+1e5*tp
+    if cost<oldcost-1e-7 or (pen>.0001 and tp<pen*.8):chosen=trial;break
+   if chosen is None:break
+   x=chosen
+   if np.linalg.norm(alpha*dx)<1e-5:break
+  put(x,base,k,wa,ja);_,ds=collision_rows(side,B,False);final=max(0.,-ds.min()) if len(ds) else 0
+  q[f]=d.qpos.copy();previous[side]=x-xorig
+  report.append(dict(frame=int(f),side=side,initial_penetration_mm=float(initial*1000),final_penetration_mm=float(final*1000),wrist_shift_mm=float(np.linalg.norm(x[:3]-xorig[:3])*1000),contact_gap_mm=float(np.linalg.norm(d.site_xpos[sites][on]-target[on],axis=1).mean()*1000) if on.any() else None,iterations=it+1))
+ if f%20==0 or args.pilot:print('frame',f,report[-2:],flush=True)
+ (OUT/('pilot_fit.json' if args.pilot else 'fit_progress.json')).write_text(json.dumps(report,indent=2))
+if args.pilot:
+ print('PILOT_FINISHED',max(x['final_penetration_mm'] for x in report),flush=True);raise SystemExit
+np.savez_compressed(OUT/'reference_video_rate.npz',qpos=q,contact=contact,contact_pos=cp,time=oldref['time'],camera_world_from_cv=oldref['camera_world_from_cv'])
+# Interpolated states are separately collision-checked before physical optimization.
+times=np.load(OLD/'datasets/processed/current/l20/bimanual/boxes/0/trajectory_kinematic_act.npz')['time'];qi=np.stack([np.interp(times,oldref['time'],q[:,i]) for i in range(m.nq)],1);vel=np.gradient(qi,.0025,axis=0);vel[0]=0;ctrl=qi[:,m.jnt_qposadr[m.actuator_trnid[:,0]]];idx=np.minimum(np.searchsorted(oldref['time'],times),N-1);ci=np.stack([np.interp(times,oldref['time'],cp.reshape(N,-1)[:,j]) for j in range(30)],1).reshape(-1,10,3)
+np.savez_compressed(T/'0/trajectory_kinematic_act.npz',qpos=qi,qvel=vel,ctrl=ctrl,contact=contact[idx],contact_pos=ci,time=times)
+(T/'task_info.json').write_text((OLD/'datasets/processed/current/l20/bimanual/boxes/task_info.json').read_text());config=json.loads((OLD/'config.json').read_text());config.update(dataset_dir=str(OUT/'datasets'),num_samples=64,nconmax_per_env=1024,njmax_per_env=3072);(OUT/'config.json').write_text(json.dumps(config,indent=2));print('COLLISION_FIT_DONE',flush=True)

scripts/foundationpose_env.sh

@@ -0,0 +1,17 @@
+#!/usr/bin/env bash
+# Source this file before building or running FoundationPose.
+FP_ROOT="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)"
+unset PYTHONPATH LD_LIBRARY_PATH
+export CUDA_HOME="$FP_ROOT/.cuda/usr/local/cuda-12.8"
+export PATH="$FP_ROOT/.venv/bin:$CUDA_HOME/bin:$PATH"
+export TORCH_CUDA_ARCH_LIST=8.6
+export MAX_JOBS=2
+export PIP_CACHE_DIR="$FP_ROOT/.cache/pip"
+export TORCH_EXTENSIONS_DIR="$FP_ROOT/.cache/torch_extensions"
+export XDG_CACHE_HOME="$FP_ROOT/.cache"
+export MPLCONFIGDIR="$FP_ROOT/.cache/matplotlib"
+export CPATH="$CUDA_HOME/include"
+for fp_inc in /home/timessage/python_envs/unitree_rl_mjlab/lib/python3.11/site-packages/nvidia/*/include; do
+    export CPATH="$CPATH:$fp_inc"
+done
+unset fp_inc

scripts/interpolate_collision_reference.py

@@ -0,0 +1,18 @@
+"""Interpolate rigid rotations on SO(3), choosing a continuous Euler chart for SPIDER."""
+from pathlib import Path
+import numpy as np,json
+from scipy.spatial.transform import Rotation as R,Slerp
+O=Path(__file__).resolve().parents[1]/'output/collision_fix_20260915';T=O/'datasets/processed/current/l20/bimanual/boxes/0';a=np.load(O/'reference_video_rate.npz');old=np.load(T/'trajectory_kinematic_act.npz');arrays={k:old[k] for k in old.files};times=arrays['time'];t=a['time'];q=a['qpos'];qi=np.stack([np.interp(times,t,q[:,j]) for j in range(q.shape[1])],1)
+def continuous_euler(rot,initial):
+ angles=rot.as_euler('XYZ');prev=initial.copy();rows=[]
+ for v in angles:
+  candidates=np.array([v,[v[0]+np.pi,np.pi-v[1],v[2]+np.pi]])
+  candidates+=2*np.pi*np.round((prev-candidates)/(2*np.pi));v=candidates[np.argmin(np.linalg.norm(candidates-prev,axis=1))];rows.append(v);prev=v
+ return np.array(rows)
+for start in [0,27,54,60]:
+ rotation=Slerp(t,R.from_euler('XYZ',q[:,start+3:start+6]))(np.clip(times,t[0],t[-1]));qi[:,start+3:start+6]=continuous_euler(rotation,q[0,start+3:start+6])
+arrays['qpos']=qi;arrays['qvel']=np.gradient(qi,.0025,axis=0);arrays['qvel'][0]=0
+import mujoco
+m=mujoco.MjModel.from_xml_path(str(T.parent/'scene_act.xml'));arrays['ctrl']=qi[:,m.jnt_qposadr[m.actuator_trnid[:,0]]]
+np.savez_compressed(T/'trajectory_kinematic_act.npz',**arrays)
+print('SO3 interpolation done',qi.shape)

scripts/l20_calibrated.py

@@ -79,17 +79,18 @@ class CalibratedAdaptor(MimicJointKinematicAdaptor):
             self.multipliers[i]=slope
         return pin_qpos
 
-def build_assets(out):
+def build_assets(out, side='right'):
     out.mkdir(parents=True,exist_ok=True)
-    fixed=source.build_model('right',out)
+    asset=source.REPO_ROOT/'L20'/side.upper()
+    fixed=source.build_model(side,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')
+    tree=ET.parse(asset/f'linkerhand_g20_{side}.urdf')
     root=tree.getroot()
-    for mesh in root.iter('mesh'): mesh.set('filename',str(ASSET/mesh.get('filename')))
+    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)

scripts/large_files.py

@@ -0,0 +1,86 @@
+"""Split / reassemble files larger than the hosting platform's 50 MB per-request limit.
+
+The Gitea server rejects any single upload above 50 MB (HTTP 413), so weights above that size are stored in
+the repository as 48 MB chunks  <file>.part-000, .part-001, ...  (each chunk is a Git LFS object) together with
+configs/large_files.json (original path, size, sha256, chunk count).
+
+    python3 scripts/large_files.py assemble          # after clone: rebuild every original file, verify sha256
+    python3 scripts/large_files.py split <path>...   # maintainer: chunk new large files and update the manifest
+    python3 scripts/large_files.py check             # verify assembled files against the manifest
+
+Chunks are left in place after assembly; delete them with `assemble --clean` if you need the space.
+"""
+import argparse, hashlib, json, sys
+from pathlib import Path
+
+ROOT = Path(__file__).resolve().parents[1]
+MANIFEST = ROOT / 'configs/large_files.json'
+CHUNK = 48 * 1024 * 1024
+
+
+def sha256(path):
+    h = hashlib.sha256()
+    with path.open('rb') as f:
+        for block in iter(lambda: f.read(1 << 24), b''):
+            h.update(block)
+    return h.hexdigest()
+
+
+def load():
+    return json.loads(MANIFEST.read_text()) if MANIFEST.exists() else {'chunk_bytes': CHUNK, 'files': []}
+
+
+def split(paths):
+    m = load(); known = {e['path'] for e in m['files']}
+    for p in paths:
+        src = (ROOT / p).resolve(); rel = src.relative_to(ROOT).as_posix()
+        if rel in known:
+            print('already in manifest:', rel); continue
+        n = 0
+        with src.open('rb') as f:
+            while True:
+                block = f.read(CHUNK)
+                if not block: break
+                (src.parent / f'{src.name}.part-{n:03d}').write_bytes(block); n += 1
+        m['files'].append({'path': rel, 'bytes': src.stat().st_size, 'sha256': sha256(src), 'parts': n})
+        print(f'split {rel}: {n} parts'); src.unlink()
+    MANIFEST.write_text(json.dumps(m, indent=2) + '\n')
+
+
+def assemble(clean=False):
+    m = load(); bad = 0
+    for e in m['files']:
+        dst = ROOT / e['path']; parts = [dst.parent / f'{dst.name}.part-{i:03d}' for i in range(e['parts'])]
+        if dst.exists() and dst.stat().st_size == e['bytes'] and sha256(dst) == e['sha256']:
+            print('ok      ', e['path'])
+        else:
+            missing = [p.name for p in parts if not p.exists() or p.stat().st_size < 200]
+            if missing:
+                print('MISSING chunks (run `git lfs pull` first):', e['path'], missing[:3], '...'); bad += 1; continue
+            with dst.open('wb') as out:
+                for p in parts: out.write(p.read_bytes())
+            if sha256(dst) != e['sha256']:
+                print('SHA256 MISMATCH', e['path']); bad += 1; continue
+            print('assembled', e['path'], f"({e['bytes'] / 1e6:.0f} MB, {e['parts']} parts)")
+        if clean:
+            for p in parts: p.unlink(missing_ok=True)
+    if bad: sys.exit(f'{bad} file(s) could not be assembled')
+
+
+def check():
+    for e in load()['files']:
+        dst = ROOT / e['path']
+        ok = dst.exists() and dst.stat().st_size == e['bytes'] and sha256(dst) == e['sha256']
+        print('ok  ' if ok else 'BAD ', e['path'])
+
+
+if __name__ == '__main__':
+    ap = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
+    sub = ap.add_subparsers(dest='cmd', required=True)
+    s = sub.add_parser('split'); s.add_argument('paths', nargs='+')
+    a = sub.add_parser('assemble'); a.add_argument('--clean', action='store_true')
+    sub.add_parser('check')
+    args = ap.parse_args()
+    if args.cmd == 'split': split(args.paths)
+    elif args.cmd == 'assemble': assemble(args.clean)
+    else: check()

scripts/package_source_release.py

@@ -39,9 +39,12 @@ def main():
             reason = 'symlink: restore dependency explicitly'
         elif any(p in EXCLUDE_PARTS for p in relative.parts):
             reason = 'runtime/data/cache directory'
+        elif relative.parts[0] == 'third_party' and ('build' in relative.parts or '.cache' in relative.parts or src.suffix == '.part' or any(x.endswith('.egg-info') for x in relative.parts)):
+            reason = 'build artifact or partial download'
         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')):
+        elif src.suffix not in EXTENSIONS and src.name not in SPECIAL and not src.name.startswith(('LICENSE','COPYING','NOTICE')) \
+                and not (relative.parts[0] == 'docs' and src.suffix.lower() == '.stl' and src.stat().st_size < 1024*1024):
             reason = 'non-source file; restore separately when needed'
         elif src.stat().st_size > 5*1024*1024:
             reason = 'file exceeds 5 MiB source limit'
@@ -49,7 +52,7 @@ def main():
             excluded.append({'path':relative.as_posix(),'reason':reason})
             return
         raw = src.read_bytes()
-        if b'\0' in raw:
+        if b'\0' in raw and src.suffix.lower() != '.stl':
             excluded.append({'path':relative.as_posix(),'reason':'binary contents'})
             return
         target = dest/relative
@@ -57,7 +60,7 @@ def main():
         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):
+                if re.search(r'/home/|/tmp/|2047635068|15886123|20260915_171525|20260916_104026',line):
                     portability.append({'path':relative.as_posix(),'line':i,
                                         'kind':'machine path or fixed example identifier'})
 
@@ -97,7 +100,7 @@ def main():
                 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']:
+    for name in ['hamer','vipe','Dyn-HaMR','spider','FoundationPose','nvdiffrast']:
         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'),
@@ -107,10 +110,13 @@ def main():
         '# 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')
+        '/.uv_cache_spider/\n/.env\n__pycache__/\n*.pyc\n'
+        '/third_party/FoundationPose/weights/\n/third_party/*/build/\n/.cache/\n*.part\n'
+        '/docs/20260915_171525/\n/docs/20260916_104026/\n')
     assets=[]
-    for name in ['l20_assets','bottle_model_parametric','dex-assets']:
-        folder=ROOT/'third_party'/name
+    for rel in ['third_party/l20_assets','third_party/bottle_model_parametric','third_party/dex-assets',
+                'third_party/FoundationPose/weights','docs/20260915_171525']:
+        folder=ROOT/rel
         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()]

scripts/play_compare.py

@@ -0,0 +1,84 @@
+"""Side-by-side interactive playback: the same scene duplicated in one MuJoCo world.
+Left copy = trajectory A (e.g. kinematic reference), right copy = trajectory B (e.g. physics rollout), time-synchronised."""
+import argparse, os, time, json, copy
+from pathlib import Path
+import xml.etree.ElementTree as E
+p = argparse.ArgumentParser(description=__doc__)
+p.add_argument('--scene', type=Path, required=True); p.add_argument('--a', type=Path, required=True); p.add_argument('--b', type=Path, required=True)
+p.add_argument('--label-a', default='A'); p.add_argument('--label-b', default='B'); p.add_argument('--offset', type=float, default=.7); p.add_argument('--check', action='store_true'); p.add_argument('--out', type=Path)
+a = p.parse_args(); os.environ['MUJOCO_GL'] = 'osmesa' if a.check else 'glfw'
+import numpy as np, mujoco
+root = E.parse(a.scene).getroot(); world = root.find('worldbody')
+NAMED = ['name', 'joint', 'joint1', 'joint2', 'site', 'body', 'target', 'geom1', 'geom2', 'body1', 'body2']
+def prefix_tree(el, pre):
+    for e in el.iter():
+        for k in NAMED:
+            v = e.get(k)
+            if v and k != 'mesh' and k != 'material' and k != 'class': e.set(k, pre + v)
+originals = list(world)
+copyB = E.Element('body', name='B_root', pos=f'0 {a.offset} 0')
+for e in originals:
+    if e.tag == 'camera' or e.tag == 'light': continue
+    c = copy.deepcopy(e); prefix_tree(c, 'B_'); copyB.append(c)
+for e in originals:
+    if e.tag not in ('camera', 'light'): prefix_tree(e, 'A_')
+world.append(copyB)
+for sec in ['actuator', 'equality', 'contact', 'sensor', 'tendon']:
+    s = root.find(sec)
+    if s is None: continue
+    items = list(s)
+    for e in items:
+        c = copy.deepcopy(e); prefix_tree(c, 'B_'); s.append(c); prefix_tree(e, 'A_')
+out = a.out or (a.a.parent / 'compare_scene.xml'); E.ElementTree(root).write(out)
+m = mujoco.MjModel.from_xml_path(str(out)); d = mujoco.MjData(m)
+def load(path):
+    z = np.load(path); return z['qpos'], z['time']
+qa, ta = load(a.a); qb, tb = load(a.b); n = qa.shape[1]; assert qb.shape[1] == n and m.nq == 2 * n, (m.nq, n)
+# qpos layout: joints of A come first (original order), then B (same order) -> verify via joint names
+ja = [i for i in range(m.njnt) if m.joint(i).name.startswith('A_')]; jb = [i for i in range(m.njnt) if m.joint(i).name.startswith('B_')]
+adrA = np.concatenate([np.arange(m.jnt_qposadr[i], m.jnt_qposadr[i] + (7 if m.jnt_type[i] == 0 else 4 if m.jnt_type[i] == 1 else 1)) for i in ja]); adrB = np.concatenate([np.arange(m.jnt_qposadr[i], m.jnt_qposadr[i] + (7 if m.jnt_type[i] == 0 else 4 if m.jnt_type[i] == 1 else 1)) for i in jb])
+assert len(adrA) == n and len(adrB) == n
+tmax = float(min(ta[-1], tb[-1]))
+def frame(tq, q, t): i = int(np.clip(np.searchsorted(tq, t, side='right') - 1, 0, len(tq) - 1)); return q[i]
+def set_time(t):
+    d.qpos[adrA] = frame(ta, qa, t); d.qpos[adrB] = frame(tb, qb, t); d.time = t; mujoco.mj_forward(m, d)
+if a.check:
+    for t in [0, tmax / 2, tmax]: set_time(t)
+    print(json.dumps({'nq': m.nq, 'duration_s': tmax, 'finite': bool(np.isfinite(d.qpos).all()), 'scene': str(out)})); raise SystemExit
+import mujoco.viewer
+state = {'paused': False, 'time': 0., 'speed': 1., 'reset_camera': False}
+def key(k):
+    if k == 32: state['paused'] = not state['paused']
+    elif k in [82, 114]: state['time'] = 0.
+    elif k in [262, 263]: state['time'] = float(np.clip(state['time'] + (1 if k == 262 else -1) / 30, 0, tmax)); state['paused'] = True
+    elif k == 265: state['speed'] = min(4., state['speed'] * 2)
+    elif k == 264: state['speed'] = max(.125, state['speed'] / 2)
+    elif k in [67, 99]: state['reset_camera'] = True
+m.vis.headlight.ambient[:] = .7; set_time(0)
+centers = [np.mean([d.site_xpos[m.site(pre + s + '_object_track').id] for s in ['right', 'left']], axis=0) for pre in ['A_', 'B_']]; center = np.mean(centers, axis=0)
+cid = m.camera('A_source_camera').id if any(m.camera(i).name == 'A_source_camera' for i in range(m.ncam)) else 0; offset = d.cam_xpos[cid] - centers[0]
+camera = {'distance': float(np.linalg.norm(offset)) * 1.6, 'azimuth': float(np.degrees(np.arctan2(offset[1], offset[0]))), 'elevation': float(-np.degrees(np.arctan2(offset[2], np.linalg.norm(offset[:2]))))}
+status = out.parent / 'compare_playback_status.json'
+print(f'LEFT (y=0): {a.label_a} | RIGHT (y=+{a.offset}): {a.label_b} | SPACE pause | R restart | LEFT/RIGHT frame step | UP/DOWN speed | C camera', flush=True)
+with mujoco.viewer.launch_passive(m, d, key_callback=key) as viewer:
+    def reset_camera():
+        viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FREE; viewer.cam.lookat[:] = center
+        for k, x in camera.items(): setattr(viewer.cam, k, x)
+    with viewer.lock():
+        reset_camera(); viewer.opt.sitegroup[:] = 0; viewer.opt.geomgroup[3:] = 0
+        # colour markers: green sphere above A, red above B
+        for i, (c, rgba) in enumerate([(centers[0], (0, .8, 0, 1)), (centers[1], (.9, .1, .1, 1))]):
+            g = viewer.user_scn.geoms[i]; mujoco.mjv_initGeom(g, mujoco.mjtGeom.mjGEOM_SPHERE, np.array([.02, 0, 0]), np.array(c) + np.array([0, 0, .35]), np.eye(3).ravel(), np.array(rgba, dtype=np.float32))
+        viewer.user_scn.ngeom = 2
+    last = time.perf_counter(); last_status = 0
+    while viewer.is_running():
+        now = time.perf_counter(); el = now - last; last = now
+        if not state['paused']: state['time'] = (state['time'] + el * state['speed']) % tmax
+        with viewer.lock():
+            if state['reset_camera']: reset_camera(); state['reset_camera'] = False
+            set_time(state['time'])
+        viewer.sync()
+        if now - last_status > 1:
+            status.write_text(json.dumps({'running': True, 'pid': os.getpid(), 'left': a.label_a, 'right': a.label_b, 'time_s': state['time'], 'speed': state['speed'], 'paused': state['paused'], 'updated_unix': time.time()}, indent=2)); last_status = now
+        time.sleep(.008)
+status.write_text(json.dumps({'running': False, 'pid': os.getpid(), 'updated_unix': time.time()}))

scripts/play_spider_result.py

@@ -0,0 +1,60 @@
+"""Interactive wall-clock playback of saved SPIDER states (no re-simulation)."""
+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/spider_dynamics_fix_20260915')
+p.add_argument('--mode',choices=['physics','reference'],default='physics')
+p.add_argument('--check',action='store_true');a=p.parse_args()
+os.environ['MUJOCO_GL']='osmesa' if a.check else 'glfw'
+import numpy as np,mujoco
+root=a.directory.resolve();task=root/'datasets/processed/current/l20/bimanual/boxes'
+m=mujoco.MjModel.from_xml_path(str(task/'scene_act.xml'));d=mujoco.MjData(m)
+source=root/('reference_video_rate.npz' if a.mode=='reference' else 'physics_motion.npz')
+mode='collision-corrected kinematic reference (middle video panel)' if a.mode=='reference' else 'saved physical state playback, no re-simulation'
+with np.load(source) as z:
+ q=z['qpos'];t=z['time']
+ if a.mode=='physics':u=z['ctrl']
+if a.mode=='reference':
+ v=np.zeros((len(q),m.nv));u=q[:,m.jnt_qposadr[m.actuator_trnid[:,0]]]
+else:
+ with np.load(task/'0/trajectory_mjwp_act.npz') as z:v=z['qvel'].reshape(len(q),-1)
+assert q.shape==(len(t),m.nq) and np.isfinite(q).all() and np.all(np.diff(t)>0)
+if a.check:
+ for i in [0,len(q)//2,len(q)-1]:
+  d.qpos[:]=q[i];d.qvel[:]=v[i];d.ctrl[:]=u[i];mujoco.mj_forward(m,d)
+ print(json.dumps({'steps':len(q),'duration_s':float(t[-1]),'nq':m.nq,'finite':True,'mode':mode}));raise SystemExit
+import mujoco.viewer
+state={'paused':False,'time':0.,'speed':1.,'reset_camera':False}
+def key(k):
+ if k==32:state['paused']=not state['paused']
+ elif k in [82,114]:state['time']=0.
+ elif k in [262,263]:state['time']=float(np.clip(state['time']+(1 if k==262 else -1)/30,0,t[-1]));state['paused']=True
+ elif k==265:state['speed']=min(4.,state['speed']*2)
+ elif k==264:state['speed']=max(.125,state['speed']/2)
+ elif k in [67,99]:state['reset_camera']=True
+m.vis.headlight.ambient[:]=.7
+d.qpos[:]=q[0];mujoco.mj_forward(m,d)
+center=np.mean([d.site_xpos[m.site(s+'_object_track').id] for s in ['right','left']],axis=0)
+cid=m.camera('source_camera').id;offset=d.cam_xpos[cid]-center
+camera={'distance':float(np.linalg.norm(offset)),'azimuth':float(np.degrees(np.arctan2(offset[1],offset[0]))),'elevation':float(-np.degrees(np.arctan2(offset[2],np.linalg.norm(offset[:2]))))}
+status=root/'interactive_playback_status.json'
+print(mode+' | SPACE pause | R restart | LEFT/RIGHT video-frame step | UP/DOWN speed | C camera reset | mouse orbit/zoom',flush=True)
+with mujoco.viewer.launch_passive(m,d,key_callback=key) as viewer:
+ def reset_camera():
+  viewer.cam.type=mujoco.mjtCamera.mjCAMERA_FREE;viewer.cam.lookat[:]=center
+  for k,x in camera.items():setattr(viewer.cam,k,x)
+ with viewer.lock():
+  reset_camera();viewer.opt.sitegroup[:]=0;viewer.opt.geomgroup[3:]=0
+ last=time.perf_counter();last_status=0
+ while viewer.is_running():
+  now=time.perf_counter();elapsed=now-last;last=now
+  if not state['paused']:state['time']=(state['time']+elapsed*state['speed'])%float(t[-1])
+  i=int(np.clip(np.searchsorted(t,state['time'],side='right')-1,0,len(t)-1))
+  with viewer.lock():
+   if state['reset_camera']:reset_camera();state['reset_camera']=False
+   d.qpos[:]=q[i];d.qvel[:]=v[i];d.ctrl[:]=u[i];d.time=float(t[i]);mujoco.mj_forward(m,d)
+  viewer.sync()
+  if now-last_status>1:
+   status.write_text(json.dumps({'running':True,'pid':os.getpid(),'mode':mode,'source':str(source),'step':i,'time_s':float(t[i]),'speed':state['speed'],'paused':state['paused'],'updated_unix':time.time()},indent=2));last_status=now
+  time.sleep(.008)
+status.write_text(json.dumps({'running':False,'pid':os.getpid(),'updated_unix':time.time()}))

scripts/prepare_rgbd_20260915.py

@@ -0,0 +1,53 @@
+"""Audit RGB-D input, preserve object meshes and estimate static-background camera motion."""
+import os
+os.environ.setdefault('OMP_NUM_THREADS','4')
+from pathlib import Path
+import json
+import shutil
+import cv2
+import numpy as np
+import open3d as o3d
+
+ROOT=Path(__file__).resolve().parents[1]
+SRC=ROOT/'docs/20260915_171525';OUT=ROOT/'output/20260915_171525';OUT.mkdir(exist_ok=True)
+meta=json.loads((SRC/'intrinsics.json').read_text())
+times=np.asarray(meta['timestamps_ms'])/1000;times-=times[0]
+assert len(times)==meta['frames'] and np.all(np.diff(times)>0)
+assert len(list((SRC/'depth').glob('*.png')))==len(times)
+assets=OUT/'object_assets';assets.mkdir(exist_ok=True)
+meshes=[]
+for source,name in [('上半.stl','upper.stl'),('下半.stl','lower.stl')]:
+    shutil.copy2(ROOT/'docs'/source,assets/name)
+    mesh=o3d.io.read_triangle_mesh(str(assets/name));v=np.asarray(mesh.vertices)
+    meshes.append(dict(file=name,vertices=len(v),triangles=len(mesh.triangles),bounds=[v.min(0).tolist(),v.max(0).tolist()],extent_m=np.ptp(v,axis=0).tolist()))
+(assets/'manifest.json').write_text(json.dumps(dict(units='assumed meters from numeric dimensions; compare observed depth before claiming registration',parts=meshes,preserve_shared_coordinates=True),indent=2))
+cap=cv2.VideoCapture(str(SRC/'color.mp4'));K=o3d.camera.PinholeCameraIntrinsic(424,240,meta['fx']/2,meta['fy']/2,(meta['cx']+.5)/2-.5,(meta['cy']+.5)/2-.5)
+option=o3d.pipelines.odometry.OdometryOption();option.depth_max=1.5;option.depth_min=.10;option.depth_diff_max=.025
+option.iteration_number_per_pyramid_level=o3d.utility.IntVector([15,8,5])
+c2w=[np.eye(4)];valid=[True];rows=[];previous=None
+for i in range(len(times)):
+    ok,frame=cap.read();assert ok
+    depth=cv2.imread(str(SRC/'depth'/f'{i:06d}.png'),-1);assert depth.dtype==np.uint16 and depth.shape==frame.shape[:2]
+    color=cv2.resize(frame,(424,240),interpolation=cv2.INTER_AREA)
+    z=cv2.resize(depth,(424,240),interpolation=cv2.INTER_NEAREST).astype(np.float32)*meta['depth_scale_m']
+    hsv=cv2.cvtColor(color,cv2.COLOR_BGR2HSV);ycc=cv2.cvtColor(color,cv2.COLOR_BGR2YCrCb)
+    skin=cv2.inRange(ycc,np.array([0,133,77]),np.array([255,173,127]))>0
+    colored=(hsv[:,:,1]>85)&(((hsv[:,:,0]<14)|(hsv[:,:,0]>165))|((hsv[:,:,0]>90)&(hsv[:,:,0]<135)))
+    dynamic=skin|colored;dynamic[160:]=True
+    dynamic=cv2.dilate(dynamic.astype(np.uint8),np.ones((9,9),np.uint8))>0
+    z[dynamic]=0
+    rgbd=o3d.geometry.RGBDImage.create_from_color_and_depth(o3d.geometry.Image(cv2.cvtColor(color,cv2.COLOR_BGR2RGB)),o3d.geometry.Image(z),depth_scale=1.,depth_trunc=1.5,convert_rgb_to_intensity=True)
+    if previous is not None:
+        success,T,info=o3d.pipelines.odometry.compute_rgbd_odometry(previous,rgbd,K,np.eye(4),o3d.pipelines.odometry.RGBDOdometryJacobianFromHybridTerm(),option)
+        angle=np.arccos(np.clip((np.trace(T[:3,:3])-1)/2,-1,1))
+        accepted=bool(success and np.isfinite(T).all() and np.linalg.norm(T[:3,3])<.06 and angle<.12)
+        valid.append(accepted)
+        c2w.append(c2w[-1]@np.linalg.inv(T) if accepted else c2w[-1].copy())
+        rows.append(dict(frame=i,accepted=accepted,translation_step_m=float(np.linalg.norm(T[:3,3])),rotation_step_rad=float(angle)))
+    previous=rgbd
+    if i%50==0:print('RGBD camera',i,len(times),flush=True)
+cap.release()
+np.savez_compressed(OUT/'rgbd_camera.npz',c2w=np.asarray(c2w),intrinsics=np.array([meta[k] for k in ['fx','fy','cx','cy']]),time=times,valid=np.asarray(valid),method='Open3D adjacent RGBD odometry; mask skin/red/blue/bottom third; first camera world; no loop closure')
+report=dict(frames=len(times),time_span_s=float(times[-1]),color_depth_timestamp_max_difference_ms=float(np.max(np.abs(np.array(meta['timestamps_ms'])-meta['depth_timestamps_ms']))),camera_valid_steps=int(np.sum(valid)),camera_rejected_frames=np.flatnonzero(~np.asarray(valid)).tolist(),steps=rows,boundary='Odometry estimates, not external tracking truth; drift possible; failed steps held and explicitly invalid')
+(OUT/'rgbd_preflight.json').write_text(json.dumps(report,indent=2))
+print(json.dumps({k:v for k,v in report.items() if k!='steps'},indent=2))

scripts/prepare_yesterday_spider.py

@@ -0,0 +1,129 @@
+"""Build the two-hand/two-object SPIDER task from recorded, calibrated tracks."""
+import os
+os.environ.setdefault('MUJOCO_GL','osmesa')
+from pathlib import Path
+import copy,json,xml.etree.ElementTree as E
+import numpy as np,cv2,mujoco,trimesh,open3d as o3d
+from scipy.spatial.transform import Rotation as R,Slerp
+from scipy.optimize import least_squares
+import l20_model_source as source
+ROOT=Path(__file__).resolve().parents[1];OUT=Path(os.environ.get('HF_SPIDER_OUT',ROOT/'output/foundationpose_spider_20260915'));BASE=Path(os.environ.get('HF_HAND_BASE',ROOT/'output/20260915_171525_dynhamr'))
+TASK=OUT/'datasets/processed/current/l20/bimanual/boxes';TRIAL=TASK/'0';TRIAL.mkdir(parents=True,exist_ok=True)
+source.REPO_ROOT=ROOT/'third_party/l20_assets';fingers=source.FINGERS;fp=np.load(OUT/'foundationpose_objects.npz');N=len(fp['time']);t=np.arange(N)/30
+# Fit the visible table, excluding saturated foreground; z-up is table normal, not IMU gravity.
+cap=cv2.VideoCapture(str(ROOT/'docs/20260915_171525/color.mp4'));ok,img=cap.read();cap.release();hsv=cv2.cvtColor(img,cv2.COLOR_BGR2HSV)
+dep=cv2.imread(str(ROOT/'docs/20260915_171525/depth/000000.png'),-1)*.0001;yy,xx=np.indices(dep.shape);K=fp['K']
+mask=(xx>100)&(xx<780)&(yy>200)&(yy<460)&(hsv[:,:,1]<50)&(dep>.2)&(dep<1)
+pts=np.stack([(xx-K[0,2])*dep/K[0,0],(yy-K[1,2])*dep/K[1,1],dep],-1)[mask][::3]
+pc=o3d.geometry.PointCloud(o3d.utility.Vector3dVector(pts));plane,ii=pc.segment_plane(.004,3,1000);normal=np.array(plane[:3]);offset=plane[3]
+if normal[2]>0:normal=-normal;offset=-offset
+S=R.align_vectors([[0,0,1]],[normal])[0].as_matrix();shift=np.array([0.,0.,offset]);world_from_source=np.eye(4);world_from_source[:3,:3]=S;world_from_source[:3,3]=shift
+# Interpolate only upper rejected intervals. Lower occlusion gets an explicit, separate hypothesis.
+poses={};provenance={}
+for name in ['upper','lower']:
+ raw=fp[name+'_T_world'].copy();valid=fp[name+'_valid'].astype(bool);idx=np.flatnonzero(valid)
+ out=raw.copy();clip=np.clip(t,t[idx[0]],t[idx[-1]])
+ out[:,:3,3]=np.stack([np.interp(t,t[idx],raw[idx,k,3]) for k in range(3)],1)
+ out[:,:3,:3]=Slerp(t[idx],R.from_matrix(raw[idx,:3,:3]))(clip).as_matrix()
+ if name=='lower':
+  # Hold last visible world pose after frame176. No fabricated assembled pose.
+  provenance[name]='Observed through frame176; held last valid world pose during occlusion, not a measurement.'
+ else:provenance[name]='Rejected frames interpolated between accepted poses; endpoints held.'
+ poses[name]=world_from_source[None]@out
+np.savez_compressed(OUT/'object_reference.npz',**{k:v for k,v in poses.items()},source_world_to_sim=world_from_source,time=t,upper_observed=fp['upper_observed_raw'] if 'upper_observed_raw' in fp else fp['upper_valid'],lower_observed=fp['lower_observed_raw'] if 'lower_observed_raw' in fp else fp['lower_valid'])
+root=None;handmeta={}
+for side in ['right','left']:
+ build=OUT/('model_'+side);path=source.build_model(side,build);rr=E.parse(path).getroot()
+ for mesh in rr.findall('asset/mesh'):mesh.set('file',str((build/mesh.get('file')).resolve()));mesh.set('maxhullvert','32')
+ # Prefix all named entities and their references before combining.
+ for e in rr.iter():
+  for a in ['name','mesh','material','joint1','joint2']:
+   if e.get(a):e.set(a,side+'_'+e.get(a))
+ hand=rr.find('worldbody/body');wn=[]
+ for i in range(6):
+  n=f'{side}_hand_'+('pos_'+'xyz'[i] if i<3 else 'rot_'+'xyz'[i-3]);wn.append(n)
+  hand.insert(i,E.Element('joint',name=n,type='slide' if i<3 else 'hinge',axis=['1 0 0','0 1 0','0 0 1'][i%3],limited='false',damping='0',armature='.02'))
+ for g in hand.iter('geom'):g.attrib.update(contype='1',conaffinity='2',friction='.8 .005 .001',condim='3',solref='.015 1')
+ for j,f in enumerate(fingers):hand.find(f".//site[@name='{side}_landmark_{4+4*j:02d}']").set('name',f'{side}_hand_{f}_track')
+ kin=source.HandKinematics(path,side);handmeta[side]=(kin,wn)
+ if root is None:
+  root=rr;world=root.find('worldbody');assets=root.find('asset');act=E.SubElement(root,'actuator')
+  floor=world.find("geom[@name='right_floor']");floor.attrib.update(pos='0 0 0',size='2 2 .01',contype='4',conaffinity='2',rgba='.6 .62 .65 1')
+ else:
+  assets.extend(list(rr.find('asset')));world.append(hand);root.find('equality').extend(list(rr.find('equality')))
+ for i,n in enumerate(wn):E.SubElement(act,'position',name=n+'_act',joint=n,kp='2000' if i<3 else '40',kv='60' if i<3 else '2',forcerange='-200 200' if i<3 else '-20 20')
+ for n,lo,hi in zip(kin.independent_joint_names,kin.lower,kin.upper):E.SubElement(act,'position',name=side+'_'+n+'_act',joint=side+'_'+n,kp='8',kv='.15',forcerange='-1 1',ctrlrange=source._fmt([lo,hi]))
+root.find('option').attrib.update(timestep='.0025',gravity='0 0 -9.81',integrator='implicitfast',cone='elliptic')
+root.find('default/joint').attrib.update(damping='.05',armature='.002')
+for eq in root.findall('equality/joint'):eq.set('solref','.01 1')
+meshes={};scenes={}
+for side,name,stl,color in [('right','upper','上半.stl','.85 .08 .06 1'),('left','lower','下半.stl','.06 .18 .8 1')]:
+ mesh=trimesh.load(ROOT/'docs'/stl);meshes[name]=mesh;assets.append(E.Element('mesh',name=name+'_visual',file=str(ROOT/'docs'/stl)))
+ obj=E.SubElement(world,'body',name=side+'_object')
+ # Mass is an explicit simulation assumption, not a sensor measurement.
+ E.SubElement(obj,'inertial',pos=source._fmt(mesh.center_mass),mass='.12',diaginertia='.0004 .0006 .0004')
+ for i in range(6):
+  n=side+'_object_'+('pos_'+'xyz'[i] if i<3 else 'rot_'+'xyz'[i-3]);E.SubElement(obj,'joint',name=n,type='slide' if i<3 else 'hinge',axis=['1 0 0','0 1 0','0 0 1'][i%3],limited='false',damping='0.001',armature='.001');E.SubElement(act,'position',name=n,joint=n,kp='0',kv='0')
+ E.SubElement(obj,'geom',name=name+'_visual',type='mesh',mesh=name+'_visual',rgba=color,contype='0',conaffinity='0',mass='0',group='1')
+ for i,p in enumerate(sorted((OUT/'collision').glob(name+'_*.obj'))):
+  mn=f'{name}_collision_{i}';E.SubElement(assets,'mesh',name=mn,file=str(p),maxhullvert='32');E.SubElement(obj,'geom',name=mn,type='mesh',mesh=mn,contype='2',conaffinity='7',friction='.8 .005 .001',condim='3',solref='.015 1',mass='0',group='3')
+ E.SubElement(obj,'site',name=side+'_object_track',pos=source._fmt(mesh.centroid))
+ sc=o3d.t.geometry.RaycastingScene();sc.add_triangles(o3d.t.geometry.TriangleMesh.from_legacy(o3d.geometry.TriangleMesh(o3d.utility.Vector3dVector(mesh.vertices),o3d.utility.Vector3iVector(mesh.faces))));scenes[name]=sc
+E.SubElement(world,'camera',name='source_camera',fovy=str(np.degrees(2*np.arctan(480/(2*K[1,1])))))
+scene=TASK/'scene_act.xml';E.ElementTree(root).write(scene)
+m=mujoco.MjModel.from_xml_path(str(scene));d=mujoco.MjData(m);q=np.zeros((N,m.nq));contact=np.zeros((N,10));cp=np.zeros((N,10,3));fit_report={};initialq=None
+for si,(side,name) in enumerate([('right','upper'),('left','lower')]):
+ kin,wn=handmeta[side];motion=np.load(BASE/f'l20_{side}_stable/motion.npz');human=np.load(BASE/f'human_joints_{side}.npz')
+ wp=(motion['wrist_pos']-motion['scene_translation'])@motion['scene_rotation'];wr=np.einsum('ij,njk->nik',motion['scene_rotation'].T,R.from_quat(motion['wrist_quat_wxyz'][:,[1,2,3,0]]).as_matrix())
+ wp=wp@S.T+shift;wr=np.einsum('ij,njk->nik',S,wr);angles=np.unwrap(R.from_matrix(wr).as_euler('XYZ'),axis=0)
+ wa=[m.jnt_qposadr[m.joint(n).id] for n in wn];ja=[m.jnt_qposadr[m.joint(side+'_'+n).id] for n in kin.joint_names]
+ full=motion['qpos'][:,[list(motion['joint_names']).index(n) for n in kin.joint_names]];a=full[:,kin.independent_indices];a=np.clip(a,kin.lower,kin.upper)
+ q[:,wa]=np.c_[wp,angles];q[:,ja]=kin.expand(a)
+ oa=[m.jnt_qposadr[m.joint(side+'_object_'+s).id] for s in ['pos_x','pos_y','pos_z','rot_x','rot_y','rot_z']];q[:,oa]=np.c_[poses[name][:,:3,3],np.unwrap(R.from_matrix(poses[name][:,:3,:3]).as_euler('XYZ'),axis=0)]
+ # Human fingertips yield geometric contact hypotheses, independently of retargeted hand error.
+ hp=(human['joints']+human['wrist_world'][:,None,:])@S.T+shift
+ candidates=[];surface=[]
+ for candidate in ['upper','lower']:
+  local=np.einsum('nji,nkj->nki',poses[candidate][:,:3,:3],hp[:,[4,8,12,16,20]]-poses[candidate][:,None,:3,3])
+  nearest=scenes[candidate].compute_closest_points(o3d.core.Tensor(local.astype(np.float32).reshape(-1,3)))['points'].numpy().reshape(N,5,3)
+  distance=np.linalg.norm(nearest-local,axis=-1)
+  if candidate=='lower':distance[177:]=100.
+  candidates.append(distance);surface.append(np.einsum('nij,nkj->nki',poses[candidate][:,:3,:3],nearest)+poses[candidate][:,None,:3,3])
+ choose=np.argmin(candidates,axis=0);dist=np.min(candidates,axis=0);on=np.zeros(5,dtype=bool)
+ for f in range(N):
+  on=np.where(on,dist[f]<.035,dist[f]<.025);contact[f,si*5:si*5+5]=on
+  for tip in range(5):cp[f,si*5+tip]=surface[choose[f,tip]][f,tip]
+ fit_report[side]={'contact_frames':int(np.any(contact[:,si*5:si*5+5],axis=1).sum()),'human_tip_distance_median_mm':float(np.median(dist)*1000)}
+np.savez_compressed(OUT/'reference_before_contact_fit.npz',qpos=q,time=t)
+# Bounded kinematic warm start; subsequent optimization is actual upstream SPIDER.
+for si,(side,name) in enumerate([('right','upper'),('left','lower')]):
+ kin,wn=handmeta[side];wa=[m.jnt_qposadr[m.joint(n).id] for n in wn];ja=[m.jnt_qposadr[m.joint(side+'_'+n).id] for n in kin.joint_names];sid=[m.site(f'{side}_hand_{f}_track').id for f in fingers];errors=[]
+ handgeom=[g for g in range(m.ngeom) if m.geom_contype[g]==1 and m.geom(g).name.startswith(side+'_')]
+ for f in range(N):
+  active=contact[f,si*5:si*5+5].astype(bool)
+  old=q[f].copy();base=old[ja][kin.independent_indices];target=cp[f,si*5:si*5+5]
+  def residual(x):
+   d.qpos[:]=old;d.qpos[wa[:3]]=old[wa[:3]]+x[16:];d.qpos[ja]=kin.expand(x[:16]);mujoco.mj_fwdPosition(m,d)
+   penetrations=np.zeros(len(handgeom));lookup={g:k for k,g in enumerate(handgeom)}
+   for cc in d.contact:
+    g0,g1=map(int,cc.geom)
+    if g0 in lookup and m.geom_contype[g1]==2:penetrations[lookup[g0]]=max(penetrations[lookup[g0]],-cc.dist-.001)
+    if g1 in lookup and m.geom_contype[g0]==2:penetrations[lookup[g1]]=max(penetrations[lookup[g1]],-cc.dist-.001)
+   return np.r_[penetrations/.002,(d.site_xpos[sid][active]-target[active]).ravel()/.008,.15*(x[:16]-base),.3*x[16:]/.03]
+  fit=least_squares(residual,np.r_[base,np.zeros(3)],bounds=(np.r_[kin.lower,[-.05]*3],np.r_[kin.upper,[.05]*3]),max_nfev=30,diff_step=1e-4)
+  residual(fit.x);q[f]=d.qpos;
+  if active.any():errors.append(np.linalg.norm(d.site_xpos[sid][active]-target[active],axis=-1).mean()*1000)
+  if f%80==0:print('warmstart',side,f,flush=True)
+ fit_report[side]['warmstart_contact_gap_mean_mm']=float(np.mean(errors)) if errors else None
+ print('fit',side,fit_report[side],flush=True)
+# Interpolate unwrapped scalar coordinates, with exact mimic preserved by linear interpolation.
+dt=.0025;times=np.arange(int(np.ceil(t[-1]/.1)*.1/dt)+1)*dt
+qi=np.stack([np.interp(times,t,q[:,j]) for j in range(m.nq)],1);ci=np.stack([np.interp(times,t,cp.reshape(N,-1)[:,j]) for j in range(30)],1).reshape(-1,10,3)
+nearestidx=np.minimum(np.searchsorted(t,times),N-1);ct=contact[nearestidx];vel=np.gradient(qi,dt,axis=0);vel[0]=0
+ctrl=qi[:,m.jnt_qposadr[m.actuator_trnid[:,0]]];siteids=[m.site(f'{s}_hand_{f}_track').id for s in ['right','left'] for f in fingers]
+np.savez_compressed(TRIAL/'trajectory_kinematic_act.npz',qpos=qi,qvel=vel,ctrl=ctrl,contact=ct,contact_pos=ci,time=times)
+np.savez_compressed(OUT/'reference_video_rate.npz',qpos=q,contact=contact,contact_pos=cp,time=t,camera_world_from_cv=world_from_source[None]@fp['c2w'])
+(TASK/'task_info.json').write_text(json.dumps({'ref_dt':dt,'contact_site_ids':siteids},indent=2))
+c=json.loads((ROOT/'output/spider_l20_contact/config.json').read_text());c.update(dataset_dir=str(OUT/'datasets'),task='boxes',embodiment_type='bimanual',object_action_dims=12,num_samples=96,max_num_iterations=3,horizon=.4,max_sim_steps=len(times)-1,nconmax_per_env=512,njmax_per_env=1536,contact_rew_scale=2.,pos_rew_scale=10.)
+(OUT/'config.json').write_text(json.dumps(c,indent=2));report=dict(nq=m.nq,nv=m.nv,nu=m.nu,frames=N,duration_s=float(times[-1]),table_plane=np.asarray(plane).tolist(),table_inliers=len(ii),source_world_to_sim=world_from_source.tolist(),object_provenance=provenance,contact=fit_report,contact_rule='Human fingertip to CAD distance: engage25mm release35mm; nearest observed object per fingertip; lower excluded after176. Geometric hypothesis, no tactile labels.',mass_assumption_kg=.12,friction_assumption=.8,collision='20 CoACD convex parts per object; approximate concavity',warmstart='Bounded finger+translation least-squares with collision penalty; not physics and not SPIDER',object_joint_armature=.001,object_real_actuator_gains_zero=bool(not m.actuator_gainprm[-12:].any()))
+(OUT/'preparation.json').write_text(json.dumps(report,indent=2));print(json.dumps(report,indent=2),flush=True)

scripts/probe_collision_dynamics.py

@@ -0,0 +1,22 @@
+"""Small full-clip CPU replay to choose bounded drive and contact settings before GPU rerun."""
+import os
+os.environ['OPENBLAS_NUM_THREADS']='1';os.environ['OMP_NUM_THREADS']='1'
+from pathlib import Path
+import json,time,numpy as np,mujoco
+O=Path(__file__).resolve().parents[1]/'output/collision_fix_20260915';T=O/'datasets/processed/current/l20/bimanual/boxes';r=np.load(T/'0/trajectory_kinematic_act.npz');rq=r['qpos'];rv=r['qvel'];rc=r['ctrl'];a=np.load(O/'physics_attempt1/trajectory_mjwp_act.npz');ctrl=a['ctrl'].reshape(-1,56);allreports=[]
+for name,drive,hard,sub in [('hard_only',1.,True,1),('bounded_drive',.1,True,1),('bounded_half_dt',.1,True,2),('softer_drive',.03,True,2)]:
+ m=mujoco.MjModel.from_xml_path(str(T/'scene_act.xml'));m.opt.iterations=80;m.opt.ls_iterations=50;m.opt.timestep=.0025/sub
+ if hard:
+  ids=np.flatnonzero(np.isin(m.geom_contype,[1,2]));m.geom_solimp[ids,:]=[.999,.9999,.0001,.5,2];m.geom_solref[ids,:]=[.005,1]
+ for i in range(m.nu-12):
+  n=m.actuator(i).name
+  if '_hand_pos_' in n:kp,kv,limit=500,30,200*drive
+  elif '_hand_rot_' in n:kp,kv,limit=15,2,20*drive
+  else:kp,kv,limit=4,.15,drive
+  m.actuator_gainprm[i,0]=kp;m.actuator_biasprm[i,1]=-kp;m.actuator_biasprm[i,2]=-kv;m.actuator_forcerange[i]=[-limit,limit]
+ d=mujoco.MjData(m);d.qpos[:]=rq[0];d.qvel[:]=rv[0];d.ctrl[:]=rc[0];mujoco.mj_step(m,d);pen=[];rows=[];start=time.monotonic()
+ for f,u in enumerate(ctrl):
+  d.ctrl[:]=u
+  for _ in range(sub):mujoco.mj_step(m,d)
+  mujoco.mj_kinematics(m,d);mujoco.mj_collision(m,d);cs=[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 cs],default=0)*1000);rows.append(d.qpos.copy())
+ report=dict(name=name,drive_scale=drive,hard=hard,substeps=sub,max_penetration_mm=float(max(pen)),steps_over_1mm=int(np.sum(np.array(pen)>1)),finite=bool(np.isfinite(rows).all()),wall_s=time.monotonic()-start);allreports.append(report);np.savez_compressed(O/(name+'_cpu_probe.npz'),qpos=rows,max_penetration_mm=pen);(O/'dynamics_probe.json').write_text(json.dumps(allreports,indent=2));print(report,flush=True)

scripts/probe_spider_gpu_contact.py

@@ -0,0 +1,21 @@
+"""Isolate early CPU/GPU contact instability with identical saved controls."""
+import os,json
+from pathlib import Path
+ROOT=Path(__file__).resolve().parents[1];O=ROOT/'output/spider_dynamics_fix_20260915';T=O/'datasets/processed/current/l20/bimanual/boxes'
+os.environ['WARP_CACHE_PATH']=str(ROOT/'.spider_cache/warp');os.environ['OMP_NUM_THREADS']='2'
+import numpy as np,mujoco,mujoco_warp as mw,warp as wp
+r=np.load(T/'0/trajectory_kinematic_act.npz');rq,rv,rc=(r[k] for k in ['qpos','qvel','ctrl']);u=np.load(O/'hard_contact_attempt/control_replay.npz')['ctrl'][:80];wp.init();reports=[]
+variants=[('hard',.999,.9999,.0001,.005,2),('medium',.95,.99,.001,.005,2),('default',.9,.95,.001,.005,2),('softtime',.9,.95,.001,.01,2),('cg',.95,.99,.001,.005,1),('tau20',.9,.95,.001,.02,2),('tau40',.9,.95,.001,.04,2),('imp90',.8,.9,.001,.01,2),('imp80',.7,.8,.001,.01,2),('buffer_tau10',.9,.95,.001,.01,2),('buffer_tau20',.9,.95,.001,.02,2)]
+for name,dmin,dmax,width,tau,solver in variants:
+ m=mujoco.MjModel.from_xml_path(str(O/'hard_contact_attempt/scene_act.xml'));m.opt.iterations=80;m.opt.ls_iterations=50;m.opt.solver=solver;m.opt.integrator=mujoco.mjtIntegrator.mjINT_IMPLICITFAST;m.geom_solimp[:]=[dmin,dmax,width,.5,2];m.geom_solref[:]=[tau,1]
+ if name.startswith('buffer'):m.geom_gap[m.geom_contype>0]=.002
+ d=mujoco.MjData(m);d.qpos[:]=rq[0];d.qvel[:]=rv[0];d.ctrl[:]=rc[0];mujoco.mj_step(m,d);cpu=[];gpu=[]
+ with wp.ScopedDevice('cuda:0'):
+  wm=mw.put_model(m);wd=mw.put_data(m,d,nworld=2,nconmax=1024,njmax=3072)
+  with wp.ScopedCapture() as cap:mw.step(wm,wd)
+  for ctrl in u:
+   wd.ctrl.assign(np.tile(ctrl.astype(np.float32),(2,1)));wp.capture_launch(cap.graph);wp.synchronize();gpu.append(wd.qpos.numpy()[0].copy());d.ctrl[:]=ctrl;mujoco.mj_step(m,d);cpu.append(d.qpos.copy())
+ gpu=np.array(gpu);cpu=np.array(cpu);pen=[];shift=[]
+ for q in gpu:
+  d.qpos[:]=q;mujoco.mj_fwdPosition(m,d);pen.append(max([max(0,-c.dist) for c in d.contact if {int(m.geom_contype[c.geom[0]]),int(m.geom_contype[c.geom[1]])}=={1,2}],default=0)*1000);shift.append([d.xipos[m.body(n).id].copy() for n in ['right_object','left_object']])
+ shift=np.array(shift);report={'name':name,'max_cpu_gpu_qpos_difference':float(np.max(abs(gpu-cpu))),'max_penetration_mm':float(max(pen)),'max_object_com_displacement_mm':float(np.max(np.linalg.norm(shift-shift[0],axis=2))*1000),'finite':bool(np.isfinite(gpu).all())};reports.append(report);np.savez_compressed(O/('contact_probe_'+name+'.npz'),gpu=gpu,cpu=cpu);(O/'gpu_contact_probe.json').write_text(json.dumps(reports,indent=2));print(report,flush=True)

scripts/red_box_distance.py

@@ -0,0 +1,18 @@
+"""Unsigned closest distance with solid-angle winding sign for a closed mesh.
+Avoid ray-parity misclassification at collinear CAD tessellation edges.
+"""
+import numpy as np
+import open3d as o3d
+
+def signed_distance(scene,mesh,points):
+ p=np.asarray(points,dtype=np.float64)
+ d=scene.compute_distance(o3d.core.Tensor(p.astype('float32'))).numpy()
+ ids=np.flatnonzero(((p>=mesh.bounds[0])&(p<=mesh.bounds[1])).all(1))
+ triangles=np.asarray(mesh.triangles,dtype=np.float64)
+ for start in range(0,len(ids),32):
+  ix=ids[start:start+32];v=triangles[None]-p[ix,None,None,:];a,b,c=v[:,:,0],v[:,:,1],v[:,:,2];length=np.linalg.norm(v,axis=-1)
+  numerator=np.einsum('bij,bij->bi',a,np.cross(b,c))
+  denominator=length.prod(-1)+(a*b).sum(-1)*length[:,:,2]+(b*c).sum(-1)*length[:,:,0]+(c*a).sum(-1)*length[:,:,1]
+  winding=np.arctan2(numerator,denominator).sum(-1)/(2*np.pi)
+  d[ix[np.abs(winding)>.5]]*=-1
+ return d

scripts/refit_rgbd_objects_outline.py

@@ -0,0 +1,60 @@
+"""Refit CAD poses from observed outlines and robust depth planes; fixed CAD scale."""
+import os
+os.environ.setdefault('OMP_NUM_THREADS','4')
+from pathlib import Path
+import json,cv2,numpy as np,open3d as o3d
+from scipy.spatial.transform import Rotation,Slerp
+ROOT=Path(__file__).resolve().parents[1];BASE=ROOT/'output/20260915_171525_dynhamr';OUT=BASE/'object_refit';OUT.mkdir(exist_ok=True)
+meta=json.loads((ROOT/'docs/20260915_171525/intrinsics.json').read_text());cam=np.load(BASE/'rgbd_camera.npz');old=np.load(BASE/'object_poses.npz');N=352
+K=np.array([[meta['fx'],0,meta['cx']],[0,meta['fy'],meta['cy']],[0,0,1.]])
+meshes={n:np.asarray(o3d.io.read_triangle_mesh(str(BASE/'object_assets'/f'{n}.stl')).vertices) for n in ['upper','lower']}
+poses={n:[] for n in meshes};records={n:[] for n in meshes};cap=cv2.VideoCapture(str(ROOT/'docs/20260915_171525/color.mp4'))
+def project(v,T):
+ p=v@T[:3,:3].T+T[:3,3];return (p@K.T)[:,:2]/p[:,2,None]
+def score(v,T,mask):
+ uv=project(v,T); hull=cv2.convexHull(np.rint(uv).astype(np.int32)); pred=np.zeros_like(mask);cv2.fillConvexPoly(pred,hull,1)
+ obs=np.zeros_like(mask);cs,_=cv2.findContours(mask,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE);cv2.fillConvexPoly(obs,cv2.convexHull(max(cs,key=cv2.contourArea)),1)
+ return float(np.sum((pred>0)&(obs>0))/max(1,np.sum((pred>0)|(obs>0))))
+for t in range(N):
+ ok,im=cap.read();assert ok
+ hsv=cv2.cvtColor(im,cv2.COLOR_BGR2HSV);dep=cv2.imread(str(ROOT/'docs/20260915_171525/depth'/f'{t:06d}.png'),-1)*meta['depth_scale_m']
+ for n,v in meshes.items():
+  hue=hsv[:,:,0]; mask=(((hue<12)|(hue>170)) if n=='upper' else ((hue>90)&(hue<135)))&(hsv[:,:,1]>90)&(hsv[:,:,2]>35)&(dep>.12)&(dep<.85);mask[:80]=False
+  num,labels,stats,_=cv2.connectedComponentsWithStats(mask.astype('uint8'),8);label=1+np.argmax(stats[1:,4]);mask=(labels==label).astype('uint8')
+  cs,_=cv2.findContours(mask,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE);hull=cv2.convexHull(max(cs,key=cv2.contourArea));poly=None
+  for eps in [.015,.025,.04,.06]:
+   q=cv2.approxPolyDP(hull,eps*cv2.arcLength(hull,True),True)
+   if len(q)==4:poly=q[:,0].astype(float);break
+  if poly is None:poly=cv2.boxPoints(cv2.minAreaRect(hull)).astype(float)
+  center=poly.mean(0);poly=poly[np.argsort(np.arctan2(poly[:,1]-center[1],poly[:,0]-center[0]))];poly=np.roll(poly,-np.argmin(poly.sum(1)),axis=0)
+  # Robust plane uses only colored measured pixels, never white insert/background hull fill.
+  yy,xx=np.nonzero(cv2.erode(mask,np.ones((3,3),np.uint8)));z=dep[yy,xx];pts=np.c_[(xx-meta['cx'])/meta['fx']*z,(yy-meta['cy'])/meta['fy']*z,z]
+  pc=o3d.geometry.PointCloud(o3d.utility.Vector3dVector(pts[::3]));plane,inliers=pc.segment_plane(.004,3,100);normal=np.array(plane[:3]);rays=np.c_[poly,np.ones(4)]@np.linalg.inv(K).T;corners=rays*(-plane[3]/(rays@normal))[:,None]
+  lo=v.min(0);hi=v.max(0)
+  if n=='upper':local=np.array([[lo[0],lo[1],hi[2]],[hi[0],lo[1],hi[2]],[hi[0],lo[1],lo[2]],[lo[0],lo[1],lo[2]]])
+  else:local=np.array([[lo[0],-.0172,lo[2]],[hi[0],-.0172,lo[2]],[hi[0],-.0172,hi[2]],[lo[0],-.0172,hi[2]]])
+  A=local-local.mean(0);B=corners-corners.mean(0);u,s,vt=np.linalg.svd(A.T@B);rr=vt.T@np.diag([1,1,np.linalg.det(vt.T@u.T)])@u.T
+  T=np.eye(4);T[:3,:3]=rr;T[:3,3]=corners.mean(0)-rr@local.mean(0)
+  before=np.eye(4);before[:3,:3]=Rotation.from_quat(old[n+'_quaternion_xyzw'][t]).as_matrix();before[:3,3]=old[n+'_position_world'][t];before=np.linalg.inv(cam['c2w'][t])@before
+  candidates=[T]
+  for shift in [0,2]:
+   success,rv,tv=cv2.solvePnP(local.astype(float),np.roll(poly,shift,axis=0).astype(float),K,None,flags=cv2.SOLVEPNP_ITERATIVE)
+   if success:
+    candidate=np.eye(4);candidate[:3,:3]=Rotation.from_rotvec(rv.ravel()).as_matrix();candidate[:3,3]=tv.ravel();candidates.append(candidate)
+  T=max(candidates,key=lambda candidate:score(v,candidate,mask)-2*abs((local@candidate[:3,:3].T+candidate[:3,3])[:,2].mean()-corners[:,2].mean()))
+  rr=T[:3,:3]
+  iou0=score(v,before,mask);iou1=score(v,T,mask);rmse=np.sqrt(np.mean(np.sum((local@rr.T+T[:3,3]-corners)**2,axis=1)))
+  accepted=bool(iou1>.65 and rmse<.06 and len(inliers)>len(pts[::3])*.5)
+  poses[n].append(cam['c2w'][t]@T);records[n].append(dict(frame=t,accepted=accepted,old_outline_iou=iou0,new_outline_iou=iou1,corner_fit_rmse_m=float(rmse)))
+  if t in [0,176,351]:
+   pic=im.copy()
+   for transform,color in [(before,(0,200,255)),(T,(0,255,0))]:cv2.polylines(pic,[cv2.convexHull(np.rint(project(v,transform)).astype('int32'))],True,color,2)
+   cv2.imwrite(str(OUT/f'{n}_{t:04d}_overlay.jpg'),pic)
+ if t%80==0:print('refit',t,flush=True)
+cap.release();(OUT/'all_candidates.json').write_text(json.dumps(records));arrays={'time':cam['time']};summary={}
+for n in meshes:
+ P=np.asarray(poses[n]);good=np.array([r['accepted'] for r in records[n]]);idx=np.flatnonzero(good);assert len(idx)>N*.5,(n,len(idx))
+ ts=np.clip(np.arange(N),idx[0],idx[-1]);p=np.stack([np.interp(ts,idx,P[idx,k,3]) for k in range(3)],axis=1);rot=Slerp(idx,Rotation.from_matrix(P[idx,:3,:3]))(ts)
+ arrays.update({n+'_position_world':p,n+'_quaternion_xyzw':rot.as_quat(),n+'_keyframes':np.arange(N),n+'_keyframe_valid':good})
+ summary[n]=dict(accepted=int(good.sum()),old_iou_median=float(np.median([r['old_outline_iou'] for r in records[n]])),new_iou_median=float(np.median([r['new_outline_iou'] for r in records[n]])),frames=records[n])
+np.savez_compressed(OUT/'object_poses.npz',**arrays);(OUT/'validation.json').write_text(json.dumps(summary,indent=2));print({n:{k:v for k,v in s.items() if k!='frames'} for n,s in summary.items()})

scripts/refit_rgbd_objects_tracked.py

@@ -0,0 +1,94 @@
+"""Refit CAD poses from observed outlines and robust depth planes; fixed CAD scale."""
+import os
+os.environ.setdefault('OMP_NUM_THREADS','4')
+from pathlib import Path
+import json,cv2,numpy as np,open3d as o3d
+from scipy.spatial.transform import Rotation,Slerp
+ROOT=Path(__file__).resolve().parents[1];BASE=ROOT/'output/20260915_171525_dynhamr';OUT=BASE/'object_refit_tracked';OUT.mkdir(exist_ok=True)
+meta=json.loads((ROOT/'docs/20260915_171525/intrinsics.json').read_text());cam=np.load(BASE/'rgbd_camera.npz');old=np.load(BASE/'object_poses.npz');N=352
+K=np.array([[meta['fx'],0,meta['cx']],[0,meta['fy'],meta['cy']],[0,0,1.]])
+meshes={n:np.asarray(o3d.io.read_triangle_mesh(str(BASE/'object_assets'/f'{n}.stl')).vertices) for n in ['upper','lower']}
+poses={n:[] for n in meshes};records={n:[] for n in meshes};cap=cv2.VideoCapture(str(ROOT/'docs/20260915_171525/color.mp4'))
+def project(v,T):
+ p=v@T[:3,:3].T+T[:3,3];return (p@K.T)[:,:2]/p[:,2,None]
+def score(v,T,mask):
+ uv=project(v,T); hull=cv2.convexHull(np.rint(uv).astype(np.int32)); pred=np.zeros_like(mask);cv2.fillConvexPoly(pred,hull,1)
+ obs=np.zeros_like(mask);cs,_=cv2.findContours(mask,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE);cv2.fillConvexPoly(obs,cv2.convexHull(max(cs,key=cv2.contourArea)),1)
+ return float(np.sum((pred>0)&(obs>0))/max(1,np.sum((pred>0)|(obs>0))))
+polygons={'upper':np.array([[491,268],[601,275],[631,399],[488,379]],dtype=np.float32),'lower':np.array([[309,254],[436,264],[442,382],[291,375]],dtype=np.float32)}
+previous_gray=None
+face_choice={}
+for t in range(N):
+ ok,im=cap.read();assert ok
+ gray=cv2.cvtColor(im,cv2.COLOR_BGR2GRAY)
+ if previous_gray is not None:
+  for n,poly in polygons.items():
+   featuremask=np.zeros_like(gray);cv2.fillConvexPoly(featuremask,poly.astype('int32'),255)
+   points=cv2.goodFeaturesToTrack(previous_gray,300,.01,5,mask=featuremask)
+   if points is not None and len(points)>=8:
+    nxt,st,_=cv2.calcOpticalFlowPyrLK(previous_gray,gray,points,None,winSize=(21,21),maxLevel=3)
+    back,st2,_=cv2.calcOpticalFlowPyrLK(gray,previous_gray,nxt,None,winSize=(21,21),maxLevel=3)
+    good=(st.ravel()>0)&(st2.ravel()>0)&(np.linalg.norm(back-points,axis=(1,2))<1.)
+    if good.sum()>=8:
+     aff,inl=cv2.estimateAffinePartial2D(points[good],nxt[good],method=cv2.RANSAC,ransacReprojThreshold=2.)
+     H=None if aff is None else np.vstack([aff,[0,0,1]])
+     if H is not None and inl.sum()>=8:
+      proposed=cv2.perspectiveTransform(poly[None],H)[0]
+      if np.max(np.linalg.norm(proposed-poly,axis=1))<30:polygons[n]=proposed
+ previous_gray=gray
+ hsv=cv2.cvtColor(im,cv2.COLOR_BGR2HSV);dep=cv2.imread(str(ROOT/'docs/20260915_171525/depth'/f'{t:06d}.png'),-1)*meta['depth_scale_m']
+ for n,v in meshes.items():
+  hue=hsv[:,:,0]; mask=(((hue<12)|(hue>170)) if n=='upper' else ((hue>90)&(hue<135)))&(hsv[:,:,1]>90)&(hsv[:,:,2]>35)&(dep>.12)&(dep<.85);mask[:80]=False
+  mask &= ~(cv2.inRange(cv2.cvtColor(im,cv2.COLOR_BGR2YCrCb),np.array([0,133,77]),np.array([255,173,127]))>0)
+  num,labels,stats,_=cv2.connectedComponentsWithStats(mask.astype('uint8'),8);label=1+np.argmax(stats[1:,4]);mask=(labels==label).astype('uint8')
+  cs,_=cv2.findContours(mask,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE);hull=cv2.convexHull(max(cs,key=cv2.contourArea))
+  if cv2.contourArea(hull)>6500 and mask.sum()/cv2.contourArea(hull)>.55:
+   for eps in [.015,.025,.04,.06]:
+    quad=cv2.approxPolyDP(hull,eps*cv2.arcLength(hull,True),True)
+    if len(quad)==4:
+     poly=quad[:,0].astype('float32');center=poly.mean(0);poly=poly[np.argsort(np.arctan2(poly[:,1]-center[1],poly[:,0]-center[0]))];poly=np.roll(poly,-np.argmin(poly.sum(1)),axis=0)
+     poly=min([np.roll(poly,k,axis=0) for k in range(4)],key=lambda q:np.sum((q-polygons[n])**2))
+     polygons[n]=poly;break
+  poly=polygons[n].astype(float)
+  observed=np.zeros_like(mask);cv2.fillConvexPoly(observed,poly.astype('int32'),1)
+  mask=mask*observed
+  # Robust plane uses only colored measured pixels, never white insert/background hull fill.
+  yy,xx=np.nonzero(cv2.erode(mask,np.ones((3,3),np.uint8)));z=dep[yy,xx];pts=np.c_[(xx-meta['cx'])/meta['fx']*z,(yy-meta['cy'])/meta['fy']*z,z]
+  if len(pts)<30:pts=np.array([[0,0,.5],[.1,0,.5],[0,.1,.5]])
+  pc=o3d.geometry.PointCloud(o3d.utility.Vector3dVector(pts[::max(1,len(pts)//1500)]));plane,inliers=pc.segment_plane(.004,3,100);normal=np.array(plane[:3]);rays=np.c_[poly,np.ones(4)]@np.linalg.inv(K).T;corners=rays*(-plane[3]/(rays@normal))[:,None]
+  lo=v.min(0);hi=v.max(0)
+  if n=='upper':local=np.array([[lo[0],lo[1],hi[2]],[hi[0],lo[1],hi[2]],[hi[0],lo[1],lo[2]],[lo[0],lo[1],lo[2]]])
+  else:local=np.array([[lo[0],-.0172,lo[2]],[hi[0],-.0172,lo[2]],[hi[0],-.0172,hi[2]],[lo[0],-.0172,hi[2]]])
+  A=local-local.mean(0);B=corners-corners.mean(0);u,s,vt=np.linalg.svd(A.T@B);rr=vt.T@np.diag([1,1,np.linalg.det(vt.T@u.T)])@u.T
+  T=np.eye(4);T[:3,:3]=rr;T[:3,3]=corners.mean(0)-rr@local.mean(0)
+  before=np.eye(4);before[:3,:3]=Rotation.from_quat(old[n+'_quaternion_xyzw'][t]).as_matrix();before[:3,3]=old[n+'_position_world'][t];before=np.linalg.inv(cam['c2w'][t])@before
+  candidates=[]
+  for face in [lo[1],hi[1]]:
+   for flip in [False,True]:
+    loc=local.copy();loc[:,1]=face
+    if flip:loc=loc[[3,2,1,0]]
+    success,rv,tv=cv2.solvePnP(loc.astype(float),poly.astype(float),K,None,flags=cv2.SOLVEPNP_ITERATIVE)
+    if success:
+     candidate=np.eye(4);candidate[:3,:3]=Rotation.from_rotvec(rv.ravel()).as_matrix();candidate[:3,3]=tv.ravel();candidates.append(candidate)
+  previous=None if not poses[n] else np.linalg.inv(cam['c2w'][t])@poses[n][-1]
+  if n not in face_choice:face_choice[n]=int(np.argmax([score(v,candidate,observed) for candidate in candidates]))
+  T=candidates[face_choice[n]]
+  rr=T[:3,:3]
+  iou0=score(v,before,observed);iou1=score(v,T,observed);rmse=np.sqrt(np.mean(np.sum((local@rr.T+T[:3,3]-corners)**2,axis=1)))
+  center_step=0 if previous is None else np.linalg.norm((T[:3,:3]@v.mean(0)+T[:3,3])-(previous[:3,:3]@v.mean(0)+previous[:3,3]))
+  angle_step=0 if previous is None else Rotation.from_matrix(previous[:3,:3].T@T[:3,:3]).magnitude()
+  accepted=bool(iou1>.60 and np.isfinite(T).all() and center_step<.04 and angle_step<.3)
+  # Keep candidate for continuity diagnostics; rejected poses are excluded at interpolation.
+  poses[n].append(cam['c2w'][t]@T);records[n].append(dict(frame=t,accepted=accepted,old_outline_iou=iou0,new_outline_iou=iou1,corner_fit_rmse_m=float(rmse)))
+  if t in [0,176,351]:
+   pic=im.copy()
+   for transform,color in [(before,(0,200,255)),(T,(0,255,0))]:cv2.polylines(pic,[cv2.convexHull(np.rint(project(v,transform)).astype('int32'))],True,color,2)
+   cv2.imwrite(str(OUT/f'{n}_{t:04d}_overlay.jpg'),pic)
+ if t%80==0:print('refit',t,flush=True)
+cap.release();(OUT/'all_candidates.json').write_text(json.dumps(records));arrays={'time':cam['time']};summary={}
+for n in meshes:
+ P=np.asarray(poses[n]);good=np.array([r['accepted'] for r in records[n]]);idx=np.flatnonzero(good);assert len(idx)>N*.5,(n,len(idx))
+ ts=np.clip(np.arange(N),idx[0],idx[-1]);p=np.stack([np.interp(ts,idx,P[idx,k,3]) for k in range(3)],axis=1);rot=Slerp(idx,Rotation.from_matrix(P[idx,:3,:3]))(ts)
+ arrays.update({n+'_position_world':p,n+'_quaternion_xyzw':rot.as_quat(),n+'_keyframes':np.arange(N),n+'_keyframe_valid':good})
+ summary[n]=dict(accepted=int(good.sum()),old_iou_median=float(np.median([r['old_outline_iou'] for r in records[n]])),new_iou_median=float(np.median([r['new_outline_iou'] for r in records[n]])),frames=records[n])
+np.savez_compressed(OUT/'object_poses.npz',**arrays);(OUT/'validation.json').write_text(json.dumps(summary,indent=2));print({n:{k:v for k,v in s.items() if k!='frames'} for n,s in summary.items()})

scripts/register_hands_rgbd.py

@@ -0,0 +1,122 @@
+"""Register yesterday's Dyn-HaMR hands to the RGB-D odometry world used by the object tracks.
+
+Three corrections, all rigid/similarity, no articulation change:
+1. World frame: Dyn-HaMR world differs from the RGB-D odometry world (object tracks, table plane)
+   by a constant translation (~12.6 cm). Hands are re-expressed in the odometry world.
+2. Scale: MANO silhouettes match the image at the wrong depth (right hand ~10% too close),
+   so a per-hand constant scale about the camera centre fixes depth while keeping the silhouette.
+3. Residual per-frame depth shift along the wrist ray (smoothed), from the held-out depth fit.
+Outputs go to output/20260915_171525_dynhamr/registered/ ; nothing upstream is overwritten.
+"""
+import os, sys, json, shutil
+os.environ.setdefault('OMP_NUM_THREADS', '4')
+from pathlib import Path
+ROOT = Path(__file__).resolve().parents[1]
+sys.path.insert(0, str(ROOT / 'third_party/Dyn-HaMR/dyn-hamr')); sys.path.insert(0, str(ROOT / 'third_party/FoundationPose')); sys.path.insert(0, str(ROOT / 'scripts'))
+import numpy as np, cv2, torch, trimesh, open3d as o3d, nvdiffrast.torch as dr
+from scipy.ndimage import gaussian_filter1d
+from body_model import MANO, run_mano
+from Utils import nvdiffrast_render
+from red_box_distance import signed_distance
+
+SRC = ROOT / 'docs/20260915_171525'; BASE = ROOT / 'output/20260915_171525_dynhamr'
+PRIOR = BASE / 'corrected/prior/20260915_171525_000000_world_results.npz'
+DIAG = ROOT / 'output/hand_depth_diagnosis_20260915/frame_metrics.json'
+FP = ROOT / 'output/foundationpose_spider_20260915/foundationpose_objects.npz'
+OUT = Path(os.environ.get('HF_REG_OUT', BASE / 'registered')); OUT.mkdir(exist_ok=True)
+CAMERA = Path(os.environ.get('HF_CAMERA_FILE', BASE / 'rgbd_camera.npz')); ALIGN = os.environ.get('HF_ALIGN', 'wrist')   # 'wrist' | 'mcp'
+N = 352; torch.set_num_threads(4)
+meta = json.loads((SRC / 'intrinsics.json').read_text()); K = np.array([[meta['fx'], 0, meta['cx']], [0, meta['fy'], meta['cy']], [0, 0, 1.]])
+
+p = dict(np.load(PRIOR)); cam = np.load(CAMERA); fp = np.load(FP); fm = json.load(open(DIAG))
+model = MANO(model_path=str(ROOT / 'third_party/Dyn-HaMR/_DATA/data/mano'), batch_size=2 * N, pose2rot=True)
+with torch.no_grad():
+    o = run_mano(model, *[torch.tensor(p[k]).float() for k in ['trans', 'root_orient', 'pose_body', 'is_right', 'betas']])
+J = o['joints'].numpy(); V = o['vertices'].numpy(); faces = np.asarray(model.faces if hasattr(model, 'faces') else model.bm.faces).astype(np.int32)
+sides = ['right' if p['is_right'][b, 0] > .5 else 'left' for b in range(2)]
+R = p['cam_R'][0]; tD = p['cam_t'][0]; c2w = cam['c2w']
+w2cR = np.transpose(c2w[:, :3, :3], (0, 2, 1)); tO = -np.einsum('tij,tj->ti', w2cR, c2w[:, :3, 3])
+rot_gap = float(np.abs(R - w2cR).max()); assert rot_gap < (1e-5 if ALIGN == 'wrist' and CAMERA.name == 'rgbd_camera.npz' else 0.2), 'camera rotation differs too much'
+offset = np.einsum('tji,tj->ti', R, tD - tO); offset_mean = offset.mean(0)
+if CAMERA.name == 'rgbd_camera.npz': assert offset.std(0).max() < .005
+Jc = np.einsum('tij,btkj->btki', R, J) + tD[None, :, None, :]; Vc = np.einsum('tij,btvj->btvi', R, V) + tD[None, :, None, :]
+
+ctx = dr.RasterizeCudaContext(); eye = torch.eye(4, device='cuda')[None]; ft = torch.as_tensor(faces, device='cuda', dtype=torch.int32)
+def render(v):
+    mt = {'pos': torch.as_tensor(v, device='cuda', dtype=torch.float32), 'faces': ft, 'vnormals': torch.zeros((len(v), 3), device='cuda'), 'vertex_color': torch.ones((len(v), 3), device='cuda')}
+    with torch.inference_mode(): _, d, _ = nvdiffrast_render(K=K, H=480, W=848, ob_in_cams=eye, glctx=ctx, mesh_tensors=mt)
+    return d[0].cpu().numpy()
+scenes = {}
+for name in ['upper', 'lower']:
+    mesh = trimesh.load(ROOT / f'output/collision_fix_20260915/collision_v2/{name}_closed.ply', process=False); assert mesh.is_watertight
+    sc = o3d.t.geometry.RaycastingScene(); sc.add_triangles(o3d.t.geometry.TriangleMesh.from_legacy(o3d.geometry.TriangleMesh(o3d.utility.Vector3dVector(mesh.vertices), o3d.utility.Vector3iVector(mesh.faces)))); scenes[name] = (sc, mesh)
+def tip_gap(pts_world, name, t):
+    T = fp[name + '_T_world'][t]; local = (pts_world - T[:3, 3]) @ T[:3, :3]
+    return float(np.min(np.abs(signed_distance(scenes[name][0], scenes[name][1], local))) * 1000)
+
+yy, xx = np.indices((480, 848)); train = ((xx // 8 + yy // 8) % 2) == 0
+report = {'camera_file': str(CAMERA), 'l20_alignment': ALIGN, 'offset_dynhamr_to_odometry_world_m': offset_mean.tolist(), 'offset_norm_mm': float(np.linalg.norm(offset_mean) * 1000), 'offset_std_mm': (offset.std(0) * 1000).tolist(), 'hands': {}}
+Jw_new = np.zeros_like(J); Vc_new = np.zeros_like(Vc)
+for b, side in enumerate(sides):
+    rows = {r['frame']: r for r in fm[side]}
+    wz = Jc[b, :, 0, 2]; dz = np.array([rows[t]['fit_shift_z_mm'] / 1000 if rows[t]['depth_supported'] else np.nan for t in range(N)])
+    sup = np.isfinite(dz); k = float(np.median(1 + dz[sup] / wz[sup]))
+    resid = dz - (k - 1) * wz; resid = np.interp(np.arange(N), np.flatnonzero(sup), resid[sup]); resid = gaussian_filter1d(resid, 2)
+    ray = Jc[b, :, 0] / wz[:, None]
+    Jc_new = k * Jc[b] + (ray * resid[:, None])[:, None, :]; Vc_new[b] = k * Vc[b] + (ray * resid[:, None])[:, None, :]
+    Jw_new[b] = np.einsum('tji,tkj->tki', w2cR, Jc_new - tO[:, None, :])   # world of the chosen camera file
+    # independent check: odometry camera must reproduce the corrected camera coordinates
+    back = np.einsum('tij,tkj->tki', w2cR, Jw_new[b]) + tO[:, None, :]; assert np.abs(back - Jc_new).max() < 1e-5
+    report['hands'][side] = {'scale_about_camera': k, 'depth_supported_frames': int(sup.sum()), 'residual_shift_mm': {'median_abs': float(np.median(np.abs(resid)) * 1000), 'max_abs': float(np.abs(resid).max() * 1000)},
+                             'wrist_move_mm': {'median': float(np.median(np.linalg.norm(Jw_new[b, :, 0] - J[b, :, 0], axis=1)) * 1000), 'max': float(np.max(np.linalg.norm(Jw_new[b, :, 0] - J[b, :, 0], axis=1)) * 1000)}}
+# held-out depth residual after correction (same masks/method as the diagnosis), plus tip gaps
+cap = cv2.VideoCapture(str(SRC / 'color.mp4')); after = {s: [] for s in sides}; gaps = {s: {'before': [], 'after': []} for s in sides}; keep = {60, 175, 240, 300}
+pair = {'right': 'upper', 'left': 'lower'}
+for t in range(N):
+    ok, bgr = cap.read(); assert ok
+    z = cv2.imread(str(SRC / 'depth' / f'{t:06d}.png'), -1).astype('float32') * meta['depth_scale_m']
+    hsv = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV); skin = cv2.inRange(cv2.cvtColor(bgr, cv2.COLOR_BGR2YCrCb), np.array([0, 133, 77]), np.array([255, 173, 127])) > 0
+    skin &= ~(((hsv[:, :, 0] < 10) | (hsv[:, :, 0] > 170)) & (hsv[:, :, 1] > 115)); skin &= ~((hsv[:, :, 0] > 95) & (hsv[:, :, 0] < 130) & (hsv[:, :, 1] > 115))
+    d1 = [render(Vc_new[b, t]) for b in range(2)]
+    for b, side in enumerate(sides):
+        other = d1[1 - b]; occ = (other > .1) & (other < d1[b]); interior = cv2.erode((d1[b] > .1).astype('uint8'), np.ones((5, 5), np.uint8)) > 0
+        test = skin & interior & (z > .1) & (z < .85) & ~occ & ~train
+        if test.sum() >= 100: after[side].append(float(np.median(np.abs(z[test] - d1[b][test])) * 1000))
+        name = pair[side]; lim = (t >= 150) if side == 'right' else (t < 177)
+        if fp[name + '_valid'][t] and lim:
+            tips = J[b, t, [4, 8, 12, 16, 20]]; gaps[side]['before'].append(tip_gap(tips, name, t)); gaps[side]['after'].append(tip_gap(Jw_new[b, t, [4, 8, 12, 16, 20]], name, t))
+    if t in keep:
+        img = bgr.copy()
+        for b, col in enumerate([(255, 128, 0), (0, 200, 255)]):
+            cnts, _ = cv2.findContours((d1[b] > .1).astype('uint8'), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE); cv2.drawContours(img, cnts, -1, col, 2)
+        cv2.imwrite(str(OUT / f'overlay_registered_{t:04d}.jpg'), img)
+    if t % 100 == 0: print('frame', t, flush=True)
+cap.release()
+def rms2(x): return float(np.sqrt(np.mean(np.sum(np.diff(x, n=2, axis=0) ** 2, axis=-1))) * 1000)
+for b, side in enumerate(sides):
+    before = [r['abs_residual_before_mm'] for r in fm[side] if r['abs_residual_before_mm'] is not None]
+    report['hands'][side].update({'heldout_abs_residual_mm': {'before_median': float(np.median(before)), 'after_median': float(np.median(after[side])), 'after_p95': float(np.percentile(after[side], 95))},
+                                  'nearest_tip_to_box_mm': {'pipeline_before_median': float(np.median(gaps[side]['before'])), 'after_median': float(np.median(gaps[side]['after'])), 'frames': len(gaps[side]['after'])},
+                                  'wrist_second_difference_rms_mm': {'before': rms2(J[b, :, 0]), 'after': rms2(Jw_new[b, :, 0])}})
+# exports: human joints + L20 motion with wrist replaced; finger retargeting is direction-only so joints are unchanged
+for b, side in enumerate(sides):
+    old = dict(np.load(BASE / f'human_joints_{side}.npz', allow_pickle=True))
+    old.update(joints=Jw_new[b] - Jw_new[b, :, :1], wrist_world=Jw_new[b, :, 0], source=str(PRIOR), depth_correction_applied=True,
+               coordinate_note='RGB-D odometry world (first camera frame), same as FoundationPose object tracks. Constant world offset applied; per-hand scale about camera centre; smoothed residual depth shift along wrist ray. Wrist-relative joints scaled accordingly.',
+               scale_about_camera=np.float64(report['hands'][side]['scale_about_camera']), world_offset_m=offset_mean)
+    np.savez_compressed(OUT / f'human_joints_{side}.npz', **old)
+    src = BASE / f'l20_{side}_stable'; dst = OUT / f'l20_{side}_stable'; dst.mkdir(exist_ok=True)
+    for f in src.iterdir():
+        if f.name != 'motion.npz': shutil.copy2(f, dst / f.name)
+    mo = dict(np.load(src / 'motion.npz', allow_pickle=True)); assert np.abs(mo['wrist_world'] - J[b, :, 0]).max() < 1e-4, 'old motion wrist differs from prior joints'
+    ww = Jw_new[b, :, 0].copy()
+    if ALIGN == 'mcp':
+        # place the L20 so its knuckle row (MCP centroid) coincides with the human's; the wrist retreats by the palm-length difference
+        v = mo['actual'][:, [5, 9, 13, 17]].mean(1) - mo['actual'][:, 0]; mcp_h = Jw_new[b][:, [5, 9, 13, 17]].mean(1)
+        ww = mcp_h - np.einsum('tij,tj->ti', mo['wrist_world_R'], v)
+        report['hands'][side]['mcp_alignment_wrist_retreat_mm'] = {'median': float(np.median(np.linalg.norm(ww - Jw_new[b, :, 0], axis=1)) * 1000), 'max': float(np.max(np.linalg.norm(ww - Jw_new[b, :, 0], axis=1)) * 1000)}
+    rot = mo['scene_rotation']; wp = (ww - ww[0]) @ rot.T + np.array([0, 0, .2])
+    mo.update(wrist_world=ww, wrist_pos=wp, wrist_pos_unsmoothed=wp, scene_translation=np.array([0, 0, .2]) - rot @ ww[0], source=str(OUT / f'human_joints_{side}.npz'), l20_alignment=ALIGN)
+    np.savez_compressed(dst / 'motion.npz', **mo)
+    chk = (mo['wrist_pos'] - mo['scene_translation']) @ mo['scene_rotation']; assert np.abs(chk - ww).max() < 1e-6
+(OUT / 'registration.json').write_text(json.dumps(report, indent=2)); print(json.dumps(report, indent=2))

scripts/render_collision_fix.py

@@ -0,0 +1,23 @@
+"""Source video versus old and collision-corrected reference; no physics claim."""
+import os
+os.environ.setdefault('MUJOCO_GL','osmesa');os.environ['LP_NUM_THREADS']='4'
+import sys
+from pathlib import Path
+import numpy as np,mujoco,cv2,imageio.v2 as imageio
+from scipy.spatial.transform import Rotation as R
+ROOT=Path(__file__).resolve().parents[1];O=Path(os.environ.get('SPIDER_TASK_OUT',str(ROOT/'output/collision_fix_20260915')));OLD=Path(os.environ.get('HF_FIX_OLD',str(ROOT/'output/foundationpose_spider_20260915')));ref=np.load(O/'reference_video_rate.npz');old=np.load(OLD/'reference_video_rate.npz');m=mujoco.MjModel.from_xml_path(str(O/'datasets/processed/current/l20/bimanual/boxes/scene_act.xml'));d=mujoco.MjData(m);m.vis.quality.offsamples=1;m.vis.headlight.ambient[:]=.7
+physics='--physics' in sys.argv
+if physics:
+ actual=np.load(O/'physics_motion.npz');ref={k:ref[k][ref['time']<=actual['time'][-1]+1e-6] for k in ref.files};ix=np.argmin(abs(actual['time'][:,None]-ref['time'][None,:]),axis=0);old=ref;ref=dict(ref);ref['qpos']=actual['qpos'][ix]
+renderer=mujoco.Renderer(m,height=270,width=480);opt=mujoco.MjvOption();opt.sitegroup[:]=0;opt.geomgroup[3:]=0;opt.flags[mujoco.mjtVisFlag.mjVIS_TRANSPARENT]=False;cid=m.camera('source_camera').id
+cap=cv2.VideoCapture(str(ROOT/'docs/20260915_171525/color.mp4'));writer=imageio.get_writer(O/('spider_collision_rollout.mp4' if physics else 'hand_collision_before_after.mp4'),fps=30,codec='libx264',quality=8,macro_block_size=1)
+for f in range(len(ref['qpos'])):
+ ok,img=cap.read();assert ok;panels=[cv2.cvtColor(cv2.resize(img,(480,270)),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 rows in [old['qpos'],ref['qpos']]:
+  d.qpos[:]=rows[f];mujoco.mj_kinematics(m,d);mujoco.mj_camlight(m,d);renderer.update_scene(d,camera='source_camera',scene_option=opt);renderer.scene.flags[mujoco.mjtRndFlag.mjRND_SHADOW]=False;panels.append(renderer.render().copy())
+ panel=np.concatenate(panels,axis=1);cv2.rectangle(panel,(0,0),(1440,39),(20,25,30),-1)
+ for j,label in enumerate((['Recorded RGB-D: red + blue','Collision-corrected reference','SPIDER: free-object physical rollout'] if physics else ['Recorded RGB-D: red + blue','Before: old contact reference','After: collision-corrected reference'])):cv2.putText(panel,label,(j*480+8,16),cv2.FONT_HERSHEY_SIMPLEX,.44,(255,255,255),1)
+ cv2.putText(panel,f'frame {f}/{len(ref["qpos"])-1} | '+('SPIDER physical result - zero object assistance' if physics else 'KINEMATIC COMPARISON - not a free-object grasp rollout'),(8,33),cv2.FONT_HERSHEY_SIMPLEX,.42,(255,255,255),1);writer.append_data(panel)
+ if f in [0,46,160,240,351]:cv2.imwrite(str(O/(('physics' if physics else 'collision')+f'_comparison_{f:04d}.jpg')),cv2.cvtColor(panel,cv2.COLOR_RGB2BGR))
+ if f%80==0:print('render',f,flush=True)
+writer.close();cap.release();renderer.close();print('RENDER_COMPLETE',flush=True)

scripts/render_corrected_dynhamr_rgbd.py

@@ -0,0 +1,15 @@
+import os,subprocess,sys,shutil
+from pathlib import Path
+ROOT=Path(__file__).resolve().parents[1]; dyn=ROOT/'third_party/Dyn-HaMR'; base=ROOT/'output/20260915_171525_dynhamr'
+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',PYOPENGL_PLATFORM='egl',LD_LIBRARY_PATH='',MPLCONFIGDIR='/tmp/matplotlib-dynhamr',HYDRA_FULL_ERROR='1')
+env['DYNHAMR_HIDE_CAMERA_MARKERS']='1'
+env['PATH']=str(dyn/'.dynhamr/bin')+':'+env['PATH'];env['PYTHONPATH']=':'.join(str(dyn/p) for p in ['dyn-hamr','third-party/hamer','third-party/hamer/third-party/ViTPose'])
+target=base/('raw_comparison' if '--raw' in sys.argv else 'corrected')
+if '--raw' in sys.argv:
+ (target/'prior').mkdir(parents=True,exist_ok=True)
+ shutil.copy2(base/'optimization/prior/20260915_171525_000000_world_results.npz',target/'prior/20260915_171525_000000_world_results.npz')
+cmd=[str(dyn/'.dynhamr/bin/python'),'-u','run_opt.py','data=video_vipe','data.root='+str(base/'dataset'),'data.seq="20260915_171525"','data.vipe_dir='+str(base/'rgbd_cameras'),'data.frame_opts.fps=30','run_opt=False','run_vis=True','temporal_smooth=False','vis.phases=[prior]','hydra.run.dir='+str(base/'corrected')]
+cmd[-1]='hydra.run.dir='+str(target)
+cmd+=['vis.overwrite=True']
+with (target/'render.log').open('w') as f:r=subprocess.run(cmd,cwd=dyn/'dyn-hamr',env=env,stdout=f,stderr=subprocess.STDOUT)
+(target/'render_exit.txt').write_text(str(r.returncode));raise SystemExit(r.returncode)

scripts/render_foundationpose_red.py

@@ -0,0 +1,39 @@
+"""Render, audit and package the red-box tracking result without rerunning inference."""
+import sys,json,subprocess
+from pathlib import Path
+ROOT=Path(__file__).resolve().parents[1];sys.path.insert(0,str(ROOT/'third_party/FoundationPose'))
+import cv2,numpy as np,torch,trimesh,nvdiffrast.torch as dr
+from scipy.spatial.transform import Rotation
+from Utils import make_mesh_tensors,nvdiffrast_render
+out=ROOT/'output/foundationpose_red_20260916';src=ROOT/'docs/20260916_104026'
+a=np.load(out/'poses.npz');P=a['T_camera_from_object'];metrics=json.loads((out/'frame_metrics.json').read_text());N=len(P)
+mesh=trimesh.load(ROOT/'docs/上半.stl');mt=make_mesh_tensors(mesh);ctx=dr.RasterizeCudaContext();cap=cv2.VideoCapture(str(src/'color.mp4'))
+writer=subprocess.Popen(['ffmpeg','-y','-v','error','-f','rawvideo','-pix_fmt','bgr24','-s','1696x480','-r','30','-i','-','-an','-c:v','libx264','-preset','fast','-crf','20','-pix_fmt','yuv420p','-movflags','+faststart',str(out/'comparison.mp4')],stdin=subprocess.PIPE)
+finite=np.isfinite(P).all((1,2));centers=P[:,:3,:3]@mesh.bounds.mean(0)+P[:,:3,3];steps=[];angles=[]
+for i in range(N):
+ ok,b=cap.read();assert ok;pic=b.copy()
+ if finite[i]:
+  with torch.inference_mode():_,d,_=nvdiffrast_render(K=a['K'],H=480,W=848,ob_in_cams=torch.as_tensor(P[i:i+1],device='cuda',dtype=torch.float32),glctx=ctx,mesh_tensors=mt)
+  m=d[0].cpu().numpy()>0
+  pic[m]=(pic[m]*.65+np.array([0,220,0])*.35).astype('uint8')
+  contours,_=cv2.findContours(m.astype('uint8'),cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE);cv2.drawContours(pic,contours,-1,(0,255,0),1)
+  if i and finite[i-1]:
+   steps.append([i,float(np.linalg.norm(centers[i]-centers[i-1]))]);angles.append([i,float(np.rad2deg(Rotation.from_matrix(P[i-1,:3,:3].T@P[i,:3,:3]).magnitude()))])
+ label=f'{i:04d} {i/30:.2f}s '+metrics[i]['status']
+ cv2.rectangle(pic,(0,0),(848,45),(0,0,0),-1);cv2.putText(pic,label,(12,29),0,.65,(0,255,255),2)
+ cv2.putText(b,'Original RGB',(12,29),0,.7,(0,255,255),2)
+ writer.stdin.write(np.hstack([b,pic]).tobytes())
+writer.stdin.close();assert writer.wait()==0;cap.release()
+# Independent checks: homogeneous matrices, SO(3), time synchronization, video decoding.
+assert np.isfinite(P[finite]).all();assert np.allclose(P[finite,3,:],[0,0,0,1],atol=1e-5)
+r=P[finite,:3,:3];ortho=float(np.max(np.abs(r.transpose(0,2,1)@r-np.eye(3))));det=float(np.max(np.abs(np.linalg.det(r)-1)));assert max(ortho,det)<1e-3
+v=cv2.VideoCapture(str(out/'comparison.mp4'));decoded=0
+while v.read()[0]:decoded+=1
+v.release();assert decoded==N
+meta=json.loads((src/'intrinsics.json').read_text());deptherr=[x['depth_median_error_m'] for x in metrics if x.get('depth_median_error_m') is not None]
+valid=a['accepted'];ranges=[]
+for i in np.flatnonzero(~valid):
+ if ranges and i==ranges[-1][1]+1:ranges[-1][1]=int(i)
+ else:ranges.append([int(i),int(i)])
+report={'video_frames_decoded':decoded,'pose_matrices':int(finite.sum()),'quality_accepted':int(valid.sum()),'invalid_frame_ranges_inclusive':ranges,'rotation_orthogonality_max_error':ortho,'rotation_determinant_max_error':det,'color_depth_timestamp_difference_max_ms':float(np.max(np.abs(np.array(meta['timestamps_ms'])-meta['depth_timestamps_ms']))),'depth_residual_median_mm':float(np.median(deptherr)*1000),'largest_center_steps_m':sorted(steps,key=lambda x:x[1],reverse=True)[:8],'largest_rotation_steps_deg':sorted(angles,key=lambda x:x[1],reverse=True)[:8],'warning':'Residual measures consistency with input depth, not ground-truth pose error. Heuristic quality flags cannot resolve object symmetries or prove contact correctness.'}
+(out/'validation.json').write_text(json.dumps(report,indent=2));print(json.dumps(report,indent=2))

scripts/render_object_alignment_overlay.py

@@ -0,0 +1,15 @@
+from pathlib import Path
+import cv2,json,numpy as np,trimesh
+from scipy.spatial.transform import Rotation
+B=Path(__file__).resolve().parents[1]/'output/20260915_171525_dynhamr';ROOT=B.parents[1];meta=json.loads((ROOT/'docs/20260915_171525/intrinsics.json').read_text());cam=np.load(B/'rgbd_camera.npz');old=np.load(B/'object_poses.npz');new=np.load(B/'object_aligned/object_poses.npz')
+models={n:trimesh.load(B/'object_assets'/f'{n}.stl',force='mesh').vertices for n in ['upper','lower']}
+c=cv2.VideoCapture(str(ROOT/'docs/20260915_171525/color.mp4'));out=B/'object_aligned';w=cv2.VideoWriter(str(out/'alignment_overlay.mp4'),cv2.VideoWriter_fourcc(*'mp4v'),30,(848,480))
+for t in range(352):
+ ok,im=c.read();assert ok
+ for data,color in [(old,(0,190,255)),(new,(0,255,0))]:
+  for n,v in models.items():
+   p=Rotation.from_quat(data[n+'_quaternion_xyzw'][t]).apply(v)+data[n+'_position_world'][t];p=(p-cam['c2w'][t,:3,3])@cam['c2w'][t,:3,:3];uv=p[:,:2]/p[:,2,None]*[meta['fx'],meta['fy']]+[meta['cx'],meta['cy']]
+   cv2.polylines(im,[cv2.convexHull(np.rint(uv).astype('int32'))],True,color,2)
+ cv2.putText(im,'Orange: previous | Green: refitted',(10,25),0,.65,(255,255,255),2);w.write(im)
+ if t in [0,83,176,351]:cv2.imwrite(str(out/f'overlay_{t:04d}.jpg'),im)
+c.release();w.release()

scripts/render_red_depth_comparison.py

@@ -0,0 +1,39 @@
+"""Camera overlay and fixed oblique 3D comparison; identical view for both conditions."""
+import sys,json,subprocess
+from pathlib import Path
+ROOT=Path(__file__).resolve().parents[1];sys.path.insert(0,str(ROOT/'third_party/FoundationPose'))
+import cv2,numpy as np,torch,trimesh,nvdiffrast.torch as dr
+from Utils import nvdiffrast_render,make_mesh_tensors
+from scipy.spatial.transform import Rotation
+out=ROOT/'output/depth_ablation_red_20260916';a=np.load(out/'foreground_left_depth_comparison.npz');N=len(a['verts_before']);K=a['K'];P=a['object_pose'];mesh=trimesh.load(out/'red_box_closed.ply');ctx=dr.RasterizeCudaContext();eye=torch.eye(4,device='cuda')[None]
+center=np.median(np.einsum('tij,j->ti',P[:,:3,:3],mesh.bounds.mean(0))+P[:,:3,3],axis=0);R=Rotation.from_euler('y',55,degrees=True).as_matrix();virtualK=np.array([[700.,0,424],[0,700,240],[0,0,1.]])
+def render(v,f,color,k):
+ m=trimesh.Trimesh(vertices=v,faces=f,process=False);m.visual.vertex_colors=np.tile([*color,255],(len(v),1));mt=make_mesh_tensors(m)
+ with torch.inference_mode():c,d,_=nvdiffrast_render(K=k,H=480,W=848,ob_in_cams=eye,glctx=ctx,mesh_tensors=mt,use_light=True)
+ return (c[0].cpu().numpy()[:,:,::-1]*255).astype('uint8'),d[0].cpu().numpy()
+def scene(b,hand,box,k):
+ colors=[];depth=[]
+ for v,f,c in [(box,mesh.faces,[60,185,95]),(hand,a['faces'],[80,145,235])]:
+  cc,d=render(v,f,c,k);colors.append(cc);depth.append(d)
+ masks=[d>0 for d in depth];front=masks[1]&((~masks[0])|(depth[1]<depth[0]));img=b.copy()
+ for mask,col in [(masks[0]&~front,colors[0]),(front,colors[1])]:img[mask]=(img[mask]*.25+col[mask]*.75).astype('uint8')
+ return img
+cap=cv2.VideoCapture(str(ROOT/'docs/20260916_104026/color.mp4'));video=out/'depth_before_after.mp4';w=subprocess.Popen(['ffmpeg','-y','-v','error','-f','rawvideo','-pix_fmt','bgr24','-s','1696x960','-r','30','-i','-','-an','-c:v','libx264','-preset','fast','-crf','20','-pix_fmt','yuv420p','-movflags','+faststart',str(video)],stdin=subprocess.PIPE)
+for t in range(N):
+ ok,b=cap.read();assert ok;box=mesh.vertices@P[t,:3,:3].T+P[t,:3,3];panels=[]
+ for view in ['camera','oblique']:
+  for variant in ['before','after']:
+   hand=a['verts_'+variant][t];vv=box
+   if view=='oblique':hand=(hand-center)@R.T+[0,0,.65];vv=(box-center)@R.T+[0,0,.65]
+   bg=b if view=='camera' else np.full_like(b,35);pic=scene(bg,hand,vv,K if view=='camera' else virtualK)
+   name='RGB only' if variant=='before' else 'RGB + depth translation'
+   cv2.rectangle(pic,(0,0),(848,52),(0,0,0),-1);cv2.putText(pic,f'{name} | {view} | {t/30:.2f}s',(12,23),0,.62,(240,240,240),1)
+   status='paired evaluation' if a['paired'][t] else 'not in paired metrics'
+   cv2.putText(pic,status,(12,44),0,.5,(0,220,255),1);panels.append(pic)
+ panel=np.vstack([np.hstack(panels[:2]),np.hstack(panels[2:])]);w.stdin.write(panel.tobytes())
+ if t in [30,90,150,210,270]:cv2.imwrite(str(out/f'comparison_{t:04d}.jpg'),panel)
+ if t%60==0:print('render',t,flush=True)
+w.stdin.close();assert w.wait()==0;cap.release();v=cv2.VideoCapture(str(video));n=0
+while v.read()[0]:n+=1
+assert n==N
+(out/'video_validation.json').write_text(json.dumps({'decoded_frames':n,'fps':30,'width':1696,'height':960,'fixed_oblique_yaw_deg':55,'scope':'foreground left hand; depth-only translation, no pose/shape/smoothing changes'},indent=2));print('Video passed',n,flush=True)

scripts/render_rgbd_l20_objects.py

@@ -0,0 +1,94 @@
+"""Render source RGB next to L20 and observed object-pose estimates."""
+import os
+os.environ.setdefault('MUJOCO_GL','osmesa')
+from pathlib import Path
+import json,shutil,copy,xml.etree.ElementTree as E,argparse
+import numpy as np
+import cv2,mujoco,imageio.v2 as imageio
+from scipy.spatial.transform import Rotation
+
+ROOT=Path(__file__).resolve().parents[1]
+parser=argparse.ArgumentParser();parser.add_argument('--base',type=Path,default=ROOT/'output/20260915_171525');args=parser.parse_args()
+BASE=args.base.resolve();OUT=BASE/'replay';OUT.mkdir(exist_ok=True)
+source=ROOT/'docs/20260915_171525';meta=json.loads((source/'intrinsics.json').read_text())
+hand=np.load(BASE/'l20_right_stable/motion.npz');objects=np.load(BASE/'object_poses.npz');camera=np.load(BASE/'rgbd_camera.npz')
+left=np.load(BASE/'l20_left_stable/motion.npz') if (BASE/'l20_left_stable/motion.npz').exists() else None
+tree=E.parse(BASE/'l20_right_stable/l20_moving.xml');root=tree.getroot();assets=root.find('asset');world=root.find('worldbody')
+(OUT/'assets').mkdir(exist_ok=True)
+for mesh in assets.findall('mesh'):
+    original=(BASE/'l20_right_stable'/mesh.get('file')).resolve();name=mesh.get('name')+original.suffix
+    shutil.copy2(original,OUT/'assets'/name);mesh.set('file','assets/'+name)
+if left is not None:
+    left_xml=E.parse(BASE/'l20_left_stable/l20_moving.xml').getroot()
+    for item in left_xml.find('asset'):
+        item=copy.deepcopy(item)
+        item.set('name','left_'+item.get('name'))
+        if item.get('file'):
+            original=(BASE/'l20_left_stable'/item.get('file')).resolve()
+            name=item.get('name')+original.suffix;shutil.copy2(original,OUT/'assets'/name);item.set('file','assets/'+name)
+        assets.append(item)
+    body=copy.deepcopy(left_xml.find("worldbody/body[@name='hand_base_link']"))
+    for element in body.iter():
+        for attr in ['name','mesh','material']:
+            if element.get(attr):element.set(attr,'left_'+element.get(attr))
+    world.append(body)
+for name,color in [('upper','.8 .08 .05 1'),('lower','.05 .15 .65 1')]:
+    shutil.copy2(BASE/'object_assets'/f'{name}.stl',OUT/'assets'/f'{name}.stl')
+    E.SubElement(assets,'mesh',name=name,file=f'assets/{name}.stl')
+    body=E.SubElement(world,'body',name=name)
+    E.SubElement(body,'freejoint',name=name+'_free')
+    E.SubElement(body,'geom',type='mesh',mesh=name,rgba=color,contype='0',conaffinity='0',mass='.1')
+floor=world.find("geom[@name='floor']")
+if floor is not None:world.remove(floor)
+fovy = float(np.rad2deg(2*np.arctan(meta['height']/(2*meta['fy']))))
+E.SubElement(world,'camera',name='source_camera',fovy=str(fovy))
+scene=OUT/'scene.xml';tree.write(scene)
+model=mujoco.MjModel.from_xml_path(str(scene));data=mujoco.MjData(model)
+names=hand['joint_names'].tolist();addr=[model.jnt_qposadr[model.joint(n).id] for n in names];wrist=model.jnt_qposadr[model.joint('wrist_free').id]
+if left is not None:
+    left_addr=[model.jnt_qposadr[model.joint('left_'+n).id] for n in left['joint_names']]
+    left_wrist=model.jnt_qposadr[model.joint('left_wrist_free').id]
+R=hand['scene_rotation'];shift=hand['scene_translation'];rows=[];cam_poses=[]
+for t in range(len(hand['qpos'])):
+    data.qpos[addr]=hand['qpos'][t];data.qpos[wrist:wrist+3]=hand['wrist_pos'][t];data.qpos[wrist+3:wrist+7]=hand['wrist_quat_wxyz'][t]
+    if left is not None:
+        data.qpos[left_addr]=left['qpos'][t]
+        original_world=left['scene_rotation'].T@(left['wrist_pos'][t]-left['scene_translation'])
+        data.qpos[left_wrist:left_wrist+3]=R@original_world+shift
+        rotation=R@left['scene_rotation'].T@Rotation.from_quat(left['wrist_quat_wxyz'][t,[1,2,3,0]]).as_matrix()
+        quat=Rotation.from_matrix(rotation).as_quat();data.qpos[left_wrist+3:left_wrist+7]=quat[[3,0,1,2]]
+    for name in ['upper','lower']:
+        address=model.jnt_qposadr[model.joint(name+'_free').id]
+        data.qpos[address:address+3]=R@objects[name+'_position_world'][t]+shift
+        quat=Rotation.from_matrix(R@Rotation.from_quat(objects[name+'_quaternion_xyzw'][t]).as_matrix()).as_quat()
+        data.qpos[address+3:address+7]=quat[[3,0,1,2]]
+    mujoco.mj_forward(model,data);rows.append(data.qpos.copy())
+    T=np.eye(4);T[:3,:3]=R@camera['c2w'][t,:3,:3];T[:3,3]=R@camera['c2w'][t,:3,3]+shift;cam_poses.append(T)
+q=np.asarray(rows);cam_poses=np.asarray(cam_poses)
+np.savez_compressed(OUT/'motion.npz',qpos=q,time=hand['time'],fps=30.,camera_world_from_cv=cam_poses,
+                    hand_joint_names=names,valid=hand['detection_valid'],
+                    left_valid=left['detection_valid'] if left is not None else np.zeros(len(rows),dtype=bool),
+                    object_pose_note='RGB-D partial-surface ICP estimates; not contact-optimized or dynamic replay')
+renderer=mujoco.Renderer(model,height=360,width=640);option=mujoco.MjvOption();option.sitegroup[:]=0;option.geomgroup[3:]=0
+cid=model.camera('source_camera').id;cap=cv2.VideoCapture(str(source/'color.mp4'))
+video_name='original_vs_l20_bimanual.mp4' if left is not None else 'original_vs_l20_objects.mp4'
+writer=imageio.get_writer(OUT/video_name,fps=30,codec='libx264',quality=8,macro_block_size=1)
+for t,row in enumerate(q):
+    data.qpos[:]=row
+    model.cam_pos[cid]=cam_poses[t,:3,3]
+    quat=Rotation.from_matrix(cam_poses[t,:3,:3]@np.diag([1,-1,-1])).as_quat();model.cam_quat[cid]=quat[[3,0,1,2]]
+    mujoco.mj_forward(model,data);renderer.update_scene(data,camera='source_camera',scene_option=option)
+    sim=renderer.render().copy();ok,color=cap.read();assert ok
+    color=cv2.cvtColor(cv2.resize(color,(640,360)),cv2.COLOR_BGR2RGB)
+    panel=np.concatenate([color,sim],1)
+    cv2.rectangle(panel,(0,0),(1280,29),(20,25,30),-1)
+    cv2.putText(panel,f'RGB-D input | frame {t+1}/352',(12,20),cv2.FONT_HERSHEY_SIMPLEX,.5,(255,255,255),1)
+    label='L20 both hands + CAD | estimated kinematic poses' if left is not None else 'L20 right + CAD | estimated kinematic poses'
+    cv2.putText(panel,label,(650,20),cv2.FONT_HERSHEY_SIMPLEX,.45,(255,255,255),1)
+    writer.append_data(panel)
+    if t in [0,176,351]:cv2.imwrite(str(OUT/f'preview_{t:04d}.png'),cv2.cvtColor(panel,cv2.COLOR_RGB2BGR))
+    if t%80==0:print('render RGBD replay',t,len(q),flush=True)
+writer.close();renderer.close();cap.release()
+shutil.copy2(BASE/'l20_right_stable/trajectory.csv',OUT/'hand_trajectory.csv')
+if left is not None:shutil.copy2(BASE/'l20_left_stable/trajectory.csv',OUT/'left_hand_trajectory.csv')
+print('REPLAY_COMPLETE',OUT,flush=True)

scripts/repair_red_box_topology.py

@@ -0,0 +1,14 @@
+"""Split one collinear T-junction; preserve all original vertices and surface geometry."""
+from pathlib import Path
+import json,trimesh,numpy as np
+root=Path(__file__).resolve().parents[1];out=root/'output/depth_ablation_red_20260916';m=trimesh.load(root/'docs/上半.stl');old=m.copy()
+c=np.bincount(m.edges_unique_inverse);boundary=m.edges_unique[c==1];edge=max(boundary,key=lambda e:np.linalg.norm(m.vertices[e[0]]-m.vertices[e[1]]));a,b=edge
+v=m.vertices;ab=v[b]-v[a];t=(v-v[a])@ab/(ab@ab);dist=np.linalg.norm(v-(v[a]+t[:,None]*ab),axis=1);inside=np.flatnonzero((t>1e-7)&(t<1-1e-7)&(dist<1e-9));assert len(inside)==2
+faces=m.faces.tolist();hit=[i for i,f in enumerate(faces) if a in f and b in f];assert len(hit)==1
+face=faces.pop(hit[0]);k=next(k for k in range(3) if face[k] in edge and face[(k+1)%3] in edge);a,b,third=face[k],face[(k+1)%3],face[(k+2)%3]
+inside=sorted(inside,key=lambda q:np.linalg.norm(v[q]-v[a]));chain=[a,*inside,b];faces.extend([[int(x),int(y),int(third)] for x,y in zip(chain[:-1],chain[1:])]);m=trimesh.Trimesh(vertices=v.copy(),faces=faces,process=False)
+assert m.is_watertight and m.is_winding_consistent and m.volume>0
+assert np.array_equal(m.vertices,old.vertices) and abs(m.area-old.area)<1e-10 and abs(m.volume-old.volume)<1e-10
+m.export(out/'red_box_closed.ply')
+(out/'mesh_repair.json').write_text(json.dumps({'method':'Split original long boundary edge at two existing collinear vertices (T-junction); no added vertices or geometric hole filling','before_faces':len(old.faces),'after_faces':len(m.faces),'watertight':m.is_watertight,'winding_consistent':m.is_winding_consistent,'area_change_m2':m.area-old.area,'volume_change_m3':m.volume-old.volume},indent=2))
+print('Topology repaired with unchanged geometry',m.is_watertight)

scripts/replay_yesterday_spider_gpu.py

@@ -0,0 +1,19 @@
+"""Independent no-optimizer GPU replay and reference-control baseline."""
+import os
+from pathlib import Path
+ROOT=Path(__file__).resolve().parents[1];OUT=ROOT/'output/foundationpose_spider_20260915';TASK=OUT/'datasets/processed/current/l20/bimanual/boxes'
+os.environ['WARP_CACHE_PATH']=str(ROOT/'.spider_cache/warp');os.environ.setdefault('MUJOCO_GL','osmesa')
+import numpy as np,json,mujoco,mujoco_warp as mw,warp as wp
+r=np.load(TASK/'0/trajectory_kinematic_act.npz');a=np.load(TASK/'0/trajectory_mjwp_act.npz');ctrl=a['ctrl'].reshape(-1,56);expected=a['qpos'].reshape(-1,66)
+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[-12:].any() and not m.actuator_biasprm[-12:].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=512,njmax=1536)
+ with wp.ScopedCapture() as capture:mw.step(wm,wd)
+ for i,u in enumerate(ctrl):
+  wd.ctrl.assign(np.stack([r['ctrl'][i],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.nanmax(np.abs(rows[:,1]-expected))),zero_assistance=True,steps=len(rows),note='Fresh two-world GPU rollout; numerical contact divergence can differ from 96-world MPC replay.')
+(OUT/'independent_gpu_replay.json').write_text(json.dumps(report,indent=2));print(json.dumps(report,indent=2),flush=True)

scripts/report_collision_fix.py

@@ -0,0 +1,12 @@
+from pathlib import Path
+import json,numpy as np,mujoco,cv2
+R=Path(__file__).resolve().parents[1];O=R/'output/collision_fix_20260915';T=O/'datasets/processed/current/l20/bimanual/boxes';v=json.loads((O/'validation.json').read_text());phys=json.loads((O/'physics_validation.json').read_text()) if (O/'physics_validation.json').exists() else None
+m=mujoco.MjModel.from_xml_path(str(T/'scene_act.xml'));ref=np.load(T/'0/trajectory_kinematic_act.npz');np.savetxt(O/'reference_trajectory.csv',np.c_[ref['time'],ref['qpos'],ref['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='')
+c=cv2.VideoCapture(str(O/'hand_collision_before_after.mp4'));n=0
+while c.read()[0]:n+=1
+c.release();assert n==352;(O/'video_integrity.json').write_text(json.dumps({'decoded_frames':n,'expected':352,'passed':True},indent=2))
+b=v['before_same_new_collision_model'];a=v['corrected_reference'];lines=['# 双手与红/蓝盒碰撞修复','', '**状态：参考轨迹的手物碰撞修复已通过；Spider 自由物体动态抓取验证仍失败，仍有短时穿透和明显物体偏离。**','', '输入仍为 `docs/20260915_171525`。FoundationPose 物体位姿及有效标记沿用上一轮。修复输出独立保存，旧结果未覆盖。','', '## 修复内容','', '- 两个 CAD 的 T 形网格连接修复：只拆分共线三角形边，顶点、表面积与体积不变；两个模型均闭合。','- 红/蓝盒碰撞由各 20 个凸块改为 94/96 个有效凸块；手掌与拇指根部分离视觉网格和分块碰撞网格。它们仍是碰撞近似。','- 逐连杆检测碰撞，用解析接触法向与运动学雅可比构造分离约束；保持关节限位及原 URDF 线性 mimic。','- 接触时沿物体运动延续上一帧手姿，加相邻帧运动限制，避免从盒子一侧突然跳到另一侧。','- 手腕及物体旋转使用四元数 SLERP 插值，再选择连续欧拉角分支供 Spider 使用。逐个检查并修正全部 400 Hz 插值状态。','- 增加 2.5 mm 碰撞安全间隙，覆盖凸分解与原始表面的部分误差。','- 物理接触参数从较软的时间常数改为 5 ms，阻抗 `.999 .9999 .0001`，接触求解迭代提高到 80 次；手腕平移最大推力 20 N、旋转最大力矩 2 Nm、手指最大力矩 0.1 Nm（仿真假设）；初始桌面向下校正 9.84 mm，消除盒子初态与桌面的重叠。这是仿真对齐假设，不是新观测。','', '## 参考轨迹验证','', '以下前后对比使用同一套新碰撞几何，避免把更换模型本身算作轨迹改善。','', '| 指标 | 修复前 | 修复后 |','|---|---:|---:|',f'| 全 352 帧最大手物穿透/mm | {b["max_hand_object_penetration_mm"]:.2f} | {a["max_hand_object_penetration_mm"]:.2f} |',f'| 穿透超过 0.5 mm 的帧数 | {b["frames_over_0_5mm"]} | {a["frames_over_0_5mm"]} |',f'| 推断接触目标平均间隙/mm | {b["inferred_contact_gap_mean_mm"]:.2f} | {a["inferred_contact_gap_mean_mm"]:.2f} |', '',f'全部 4,681 个高频状态：最大手物穿透 **{v["interpolation"]["max_penetration_after_mm"]:.3f} mm**。状态均有限；物体坐标未更改。关节限位最大超出 {a["max_joint_limit_violation_rad"]:.3g} rad，mimic 最大残差 {a["max_mimic_error_rad"]:.3g} rad。','',f'独立使用原始手部视觉网格的 {v["cad_probe_scope"].split()[0]} 个表面采样点/帧，对闭合原始 CAD 检查：红盒最大侵入 **{v["upper_exact_CAD_surface_probes"]["max_inside_mm"]:.2f} mm**，蓝盒 **{v["lower_exact_CAD_surface_probes"]["max_inside_mm"]:.2f} mm**，超过 1 mm 的帧数均为零。采样检查不是完整网格相交证明。','', '## 代价与边界','', '- 本次修复范围是双手与两个盒子的碰撞，不包含手部自碰撞或手与桌面的完整验证。','- 原始手—物体配准偏差较大，修复有明显手腕位移；右/左手相对旧接触参考的修正中位数分别为 '+f'{v["wrist_correction_mm"]["right"]["median"]:.1f}/{v["wrist_correction_mm"]["left"]["median"]:.1f} mm。这不能当作已校准的人手绝对位姿。','- 指尖接触目标来自距离推断，平均间隙仍约 '+f'{a["inferred_contact_gap_mean_mm"]:.1f} mm。防穿透通过不等于稳定抓取通过。','- 蓝盒第 177 帧起被遮挡，仍保持最后可靠参考位姿；这部分没有新增观测。','']
+if phys:
+ lines+=['## SPIDER 实际物理滚动','',f'完整 {phys["steps"]} 步 / {phys["duration_s"]:.2f} 秒，全部有限值：{phys["all_finite"]}；物体真实执行器增益为零：{phys["zero_object_actuator_gains"]}。', '',f'独立 CPU 碰撞检测检查每个保存的 GPU 状态：最大穿透 **{phys["max_penetration_mm"]:.2f} mm**，逐步最大穿透中位数 **{phys["median_step_max_penetration_mm"]:.2f} mm**，超过 1 mm 的步数 {phys["steps_over_1mm"]}。', '',f'物体中心最大误差（仅可观测段）：红盒 {phys["upper"]["observed_centroid_error_max_mm"]:.1f} mm，蓝盒 {phys["lower"]["observed_centroid_error_max_mm"]:.1f} mm；20 mm 抓取跟踪门槛通过：**{phys["grasp_tracking_20mm_passed"]}**。','', f'实际发生手物接近/接触的物理步数 {phys["steps_with_hand_object_contact"]}/{phys["steps"]}；最大物理关节限位超出 {phys["max_joint_limit_violation_rad"]:.4f} rad。碰撞变少也可能来自接触丢失，不能独立证明抓取成功。','参考轨迹修复和真实物理结果分开保存；没有通过强制写回物体位姿制造成功抓取。','']
+lines+=['## 文件','', '- `hand_collision_before_after.mp4`：原视频 / 修复前参考 / 修复后参考。视频明确标为运动学对比。','- `reference_video_rate.npz`、`reference_trajectory.csv`：修复后的手与物体参考及高频关节/控制数据。','- `validation.json`：同模型前后碰撞、关节限位、mimic、原 CAD 表面探针。','- `interpolation_validation.json`：全部高频状态检查。开启 buffer 时，`max_penetration_before_mm` 字段表示距所要求间隙的不足，见 `before_metric`；after 字段始终表示实际穿透。','- `spider_collision_rollout.mp4`：修复参考与实际 Spider 自由物体滚动对比。','- `physics_validation.json`、`physics_motion.npz`：实际 Spider 物理结果（失败验证，不能视为可用抓取控制）。','- `dynamics_probe.json`：同一组保存控制量的 CPU 参数对比；CPU 中小于 1 mm 的结果未在本次 GPU 优化中完整复现。','- `collision_v2/`、`hand_collision/`：新碰撞几何；`datasets/processed/current/l20/bimanual/boxes/scene_act.xml` 为完整场景。','', '## 复现','', '```bash','.venv/bin/python scripts/build_box_collision_v2.py','.venv/bin/python scripts/build_palm_collision_v2.py','.venv/bin/python scripts/fix_yesterday_hand_collision.py','.venv/bin/python scripts/verify_fix_interpolation.py --video --buffer','.venv/bin/python scripts/interpolate_collision_reference.py','.venv/bin/python scripts/verify_fix_interpolation.py --buffer','.venv/bin/python scripts/align_collision_floor.py','.venv/bin/python scripts/configure_collision_physics.py','SPIDER_TASK_OUT=$PWD/output/collision_fix_20260915 .spider/bin/python scripts/run_yesterday_spider.py','.venv/bin/python scripts/audit_collision_fix.py','.venv/bin/python scripts/audit_collision_physics.py','.venv/bin/python scripts/render_collision_fix.py','.venv/bin/python scripts/report_collision_fix.py','```','', '旧几何探索碎片在 `collision/` 中，仅供排查，不由新场景使用。']
+(O/'README.md').write_text('\n'.join(lines)+'\n');print('REPORT_COMPLETE',n)

scripts/report_spider_dynamics_fix.py

@@ -0,0 +1,17 @@
+"""Evidence report for the corrected SPIDER free-object collision run."""
+from pathlib import Path
+import json,hashlib,numpy as np,cv2,mujoco
+ROOT=Path(__file__).resolve().parents[1];O=ROOT/'output/spider_dynamics_fix_20260915';p=O/'datasets/processed/current/l20/bimanual/boxes';v=json.loads((O/'physics_validation.json').read_text());old=json.loads((ROOT/'output/collision_fix_20260915/physics_validation.json').read_text());probe=json.loads((O/'gpu_contact_probe.json').read_text());replay=json.loads((O/'control_replay.json').read_text());reset=json.loads((O/'state_restore_check.json').read_text());surface=json.loads((O/'dynamic_surface_validation.json').read_text());a=np.load(O/'physics_motion.npz');m=mujoco.MjModel.from_xml_path(str(p/'scene_act.xml'))
+qnames=[m.joint(j).name for j in range(m.njnt)];unames=[m.actuator(j).name for j in range(m.nu)];np.savetxt(O/'spider_trajectory.csv',np.c_[a['time'],a['qpos'],a['ctrl']],delimiter=',',header=','.join(['time_s']+qnames+['ctrl_'+n for n in unames]),comments='')
+video=O/'spider_collision_rollout.mp4';cap=cv2.VideoCapture(str(video));fps=cap.get(cv2.CAP_PROP_FPS);n=0
+while True:
+ ok,frame=cap.read()
+ if not ok:break
+ n+=1
+cap.release();integrity={'decoded_frames':n,'fps':fps,'expected_frames':352,'passed':n==352 and abs(fps-30)<1e-6};(O/'video_integrity.json').write_text(json.dumps(integrity,indent=2));assert integrity['passed']
+status='碰撞小于 1 mm 门槛通过' if v['max_penetration_mm']<1 else '碰撞小于 1 mm 门槛未通过'
+lines=['# Spider 动态碰撞继续修复','',f'状态：**{status}；自由物体抓取跟踪 20 mm 门槛：{v["grasp_tracking_20mm_passed"]}。**','',f'完整 {v["steps"]} 步 / {v["duration_s"]:.2f} 秒，所有执行状态有限。输入 `docs/20260915_171525`，两只 L20、红盒和蓝盒。物体执行器在真实执行阶段保持零增益；没有把物体固定到手上。','', '## 完整动态结果','', '| 指标 | 上轮 | 本轮 |','|---|---:|---:|',f'| 最大手盒穿透/mm | {old["max_penetration_mm"]:.3f} | {v["max_penetration_mm"]:.3f} |',f'| 穿透超过 1 mm 的物理步数 | {old["steps_over_1mm"]} | {v["steps_over_1mm"]} |',f'| 红盒有效观测段质心平均误差/mm | {old["upper"]["observed_centroid_error_mean_mm"]:.1f} | {v["upper"]["observed_centroid_error_mean_mm"]:.1f} |',f'| 蓝盒有效观测段质心平均误差/mm | {old["lower"]["observed_centroid_error_mean_mm"]:.1f} | {v["lower"]["observed_centroid_error_mean_mm"]:.1f} |', '', '低穿透不代表抓取成功。物体位姿跟踪仍需单独通过验证；几何接触目标没有触觉真值，手指接触保持和抓取质量尚未解决。', '', '## 已修复的代码问题','', '- 虚拟辅助伺服力改为 `kp*ctrl - kp*q - kd*qvel`，并完整保存和恢复辅助增益。','- 候选轨迹重采样复制包含当前步的控制前缀，避免状态与控制历史不一致（当前任务默认关闭此功能，属于潜在路径修复）。','- 接触奖励实际乘入配置权重；双手状态按含被动关节的自由度划分。','- 新候选批次同步约束求解初值，避免遗留失败候选的 warmstart。','- 零物体辅助断言检查全部仿射偏置项。','', '## GPU 接触数值问题','', '旧的 `solimp=.999 .9999 .0001`、`solref=.005 1` 在当前 GPU 模型中导致蓝盒接触桌面后弹飞；同控制 CPU 重放未出现该早期爆炸。短段对照改为 `solimp=.9 .95 .001 .5 2`、`solref=.02 1`，并以 2 mm 提前量激活接触约束。碰撞网格没有扩大；检测使用实际几何距离。提前生效是数值缓冲，不能视为真实接触位置精度。','', '| 同一短段控制的参数对照 | 最大手盒穿透/mm | 最大物体质心位移/mm |','|---|---:|---:|']
+for x in probe:
+ lines.append(f'| {x["name"]} | {x["max_penetration_mm"]:.3f} | {x["max_object_com_displacement_mm"]:.2f} |')
+lines+=['','## 验证与边界','', '- 5 项回归测试通过：伺服平衡/恢复、控制重采样因果性、双手自由度索引、奖励权重和批次 warmstart 同步。',f'- GPU 状态恢复专项：仅状态恢复差异 {reset["state_only_max_qpos_difference"]:.3g}；状态和参数恢复差异 {reset["state_and_parameters_max_qpos_difference"]:.3g}。这些是下一步 qpos 混合米/弧度的最大差异。',f'- 独立重放 {replay["steps"]} 步，首步最大 qpos 差异 {replay["gpu_first_step_max_qpos_difference"]:.3g}，全段最大差异 {replay["gpu_max_qpos_difference"]:.3g}（混合米/弧度）。长段仍不能视为严格复现的控制轨迹。','- 每个保存的物理状态由 CPU 独立碰撞检测审计；凸分解几何仍是近似，未验证连续两步之间或手部自碰撞。',f'- 原始手网格表面独立采样：每帧 {surface["hand_surface_samples_per_frame"]} 点，共 {surface["sampled_frames"]} 帧；红盒最大侵入 {surface["upper"]["max_inside_mm"]:.3f} mm，蓝盒 {surface["lower"]["max_inside_mm"]:.3f} mm。不是完整网格相交证明。',f'- 最大关节限位超出 {v["max_joint_limit_violation_rad"]:.4f} rad；该动态轨迹不应直接用于硬件。','- 物体质量 0.12 kg、摩擦和执行器力限制均为仿真假设。','- 蓝盒第 177 帧起缺少可靠 FoundationPose 观测，沿用保持最后有效位姿的参考，不代表测到了遮挡后的运动。','- FoundationPose 原始位姿、有效标记和运动学防穿透参考沿用上一轮；`reference_inherited_*` 仅为继承证据。','', '## 输出','', '- `spider_collision_rollout.mp4`：原视频 / 防穿透运动学参考 / 本轮真实自由物体动态结果，352 帧已完整解码。','- `physics_validation.json`、`physics_motion.npz`：全物理步穿透与跟踪审计。','- `spider_trajectory.csv`：关节轨迹及控制。','- `gpu_contact_probe.json`：匹配控制的参数对照。','- `control_replay.npz/json`、`state_restore_check.json`：重放及状态恢复检查。','- `spider_fixes.patch`：上游本地代码修复；`hard_contact_attempt/`、`stable_short/`：失败和短段对照存档。','', '## 复现','', '```bash','.spider/bin/python tests/test_spider_dynamics_contract.py','SPIDER_TASK_OUT="$PWD/output/spider_dynamics_fix_20260915" .spider/bin/python scripts/run_yesterday_spider.py','SPIDER_TASK_OUT="$PWD/output/spider_dynamics_fix_20260915" .venv/bin/python scripts/audit_collision_physics.py','.spider/bin/python scripts/check_spider_dynamics_replay.py','SPIDER_TASK_OUT="$PWD/output/spider_dynamics_fix_20260915" .venv/bin/python scripts/render_collision_fix.py --physics','.venv/bin/python scripts/report_spider_dynamics_fix.py','```','']
+(O/'README.md').write_text('\n'.join(lines));print(status);print(integrity)

scripts/report_yesterday_spider.py

@@ -0,0 +1,21 @@
+from pathlib import Path
+import json,cv2,numpy as np
+ROOT=Path(__file__).resolve().parents[1];O=ROOT/'output/foundationpose_spider_20260915'
+v=json.loads((O/'validation.json').read_text());p=json.loads((O/'preparation.json').read_text());extra={}
+for name in ['foundationpose_overlay.mp4','original_foundationpose_spider.mp4']:
+ c=cv2.VideoCapture(str(O/name));count=0
+ while c.read()[0]:count+=1
+ c.release();extra[name]={'decoded_frames':count,'expected_frames':352,'passed':count==352}
+(O/'video_integrity.json').write_text(json.dumps(extra,indent=2))
+lines=['# 昨日双手抓取：FoundationPose + SPIDER','', '输入：`docs/20260915_171525`，352 帧。复用昨日 RGB-D 校正后的 Dyn-HaMR/L20 双手轨迹；没有重新做人手推理。','', '## 已完成的链路','', '1. FoundationPose 根据 RGB、对齐深度、内参与两个 STL 逐帧估计物体相机坐标位姿。','2. 使用昨日 RGB-D 相机里程计统一手、物体坐标；用初始深度拟合桌面方向。','3. 逐指尖选择最近可观测物体，生成可切换的接触目标；接触阈值 25 mm，释放阈值 35 mm。','4. 先做有界关节/手腕平移接触与碰撞初始化，再运行真正的 `third_party/spider/examples/run_mjwp.py`，使用 MuJoCo Warp 物理滚动优化。','5. 最终实际滚动物体执行器增益为零；物体不是逐帧强制写回参考轨迹。','', '## 观测边界','', '- 红盒 307/352 帧通过可见颜色覆盖与深度一致性筛选，未通过帧插值补全。','- 蓝盒 177/352 帧可用；177 帧起遮挡，参考位姿保持最后可用世界位姿，并停止将其作为新的接触目标来源。蓝盒遮挡段不是观测结果，后续红蓝相对装配也不是已验证事实。','- 有效判定不是外部真值：颜色像素 >500、投影覆盖 >65%、重叠像素深度中位残差 <20 mm。蓝盒遮挡边界另行显式屏蔽。','- 接触是几何假设，不是触觉标签。左右手均可切换到红盒接触。','- 物体质量各 0.12 kg、摩擦系数 0.8 是仿真假设；桌面方向来自深度，不是 IMU。','- 每个物体采用 20 个 CoACD 凸分块；孔洞和凹槽碰撞仍是近似。物体关节增加 0.001 armature 抑制欧拉角坐标翻转时的数值病态，非实测惯量。','', '## 本次实际结果','', '| 指标 | 昨日手 + 新物体位姿 | 接触/碰撞初始化 | SPIDER 物理结果 |','|---|---:|---:|---:|']
+for label,key in [('最大手物穿透/mm','max_hand_object_penetration_mm'),('逐帧最大穿透中位数/mm','median_frame_max_penetration_mm'),('推断接触目标平均间隙/mm','inferred_contact_target_gap_mean_mm'),('有关联手物碰撞的帧数','hand_object_contact_frames')]:
+ vals=[v[k][key] for k in ['original_hands_new_objects','kinematic_contact_warmstart','spider']];lines.append('| '+label+' | '+' | '.join(f'{x:.2f}' for x in vals)+' |')
+for name,cn in [('upper','红盒'),('lower','蓝盒')]:
+ r=v['spider'][name];lines+=['',f'{cn}可观测帧的位置误差：平均 **{r["observed_position_error_mean_mm"]:.1f} mm**，最大 **{r["observed_position_error_max_mm"]:.1f} mm**；平均旋转误差 **{r["observed_rotation_error_mean_deg"]:.1f}°**。误差对照 FoundationPose 参考，并非外部真值。']
+lines+=['',f'物理软约束残差：视频抽样最大关节限位超出 {v["spider"]["max_joint_limit_violation_rad"]:.4f} rad，最大 mimic 偏差 {v["spider"]["max_mimic_residual_rad"]:.4f} rad。', '',f'完整物理轨迹：{v["full_steps"]} 步，{v["duration_s"]:.2f} 秒，全部有限值：{v["all_states_finite"]}。20 mm 最大物体位置跟踪门槛通过：**{v["tracking_20mm_gate_passed"]}**。', '', '穿透统计使用碰撞近似模型，按视频 30 Hz 抽样；不能当成全部 400 Hz 物理步的最大穿透，也不能代表 STL 精确有符号距离。', '', '## 文件','', '- `original_foundationpose_spider.mp4`：原视频 / FoundationPose+接触初始化参考 / 实际 SPIDER 物理结果。','- `foundationpose_overlay.mp4`：物体投影检查，逐帧显示有效标记。','- `foundationpose_objects.npz`：原始相机/世界位姿与有效标记。','- `object_reference.npz`：明确补全后的仿真参考与源世界到仿真变换。','- `comparison_motion.npz`：原手、新参考、Spider 视频帧轨迹及完整物理轨迹。','- `spider_trajectory.csv`：完整物理 qpos 与控制量，关节顺序见表头。','- `validation.json`、`preparation.json`、`video_integrity.json`：验证和假设。','- `independent_gpu_replay.npz/json`：保存控制量的独立 GPU 重放与未优化控制基线。','', '## 适配与复现','', '两个 L20 各有 27 状态自由度、22 个执行器通道。包装器修正上游使用 actuator 数量切分双手状态奖励的索引；原始 L20 线性 mimic 被保留。由于存在双手换物接触，关闭“每只手永远对应固定物体”的拇指位移启发式，保留物体参考虚拟辅助衰减及逐指接触奖励。实际滚动始终无物体辅助。', '', '```bash','source scripts/foundationpose_env.sh','.venv/bin/python scripts/track_yesterday_boxes.py','.venv/bin/python scripts/decompose_yesterday_boxes.py','.venv/bin/python scripts/prepare_yesterday_spider.py','.spider/bin/python scripts/run_yesterday_spider.py','.spider/bin/python scripts/replay_yesterday_spider_gpu.py','.venv/bin/python scripts/evaluate_yesterday_spider.py','.venv/bin/python scripts/audit_yesterday_geometry.py','.venv/bin/python scripts/report_yesterday_spider.py','```','', '运行需要 GPU；未提供硬件控制命令，也未验证真实机械手稳定抓取。']
+if not v['tracking_20mm_gate_passed']:lines.insert(2,'**状态：完整链路已跑通，但物理抓取跟踪未通过 20 mm 门槛；不能宣称已经修复稳定抓取。**\n')
+if (O/'geometry_audit.json').exists():
+ g=json.loads((O/'geometry_audit.json').read_text());z=g['full_rate_spider_collision'];lines+=['','## 独立几何检查','',f'全部 {z["steps"]} 个物理步的最大手物碰撞穿透：{z["max_hand_object_penetration_mm"]:.2f} mm（CoACD 近似几何）。','', '红盒精确闭合 CAD 的 42 个手部关键点探针结果见 `geometry_audit.json`。这是点探针检查，不能替代完整机械手网格相交检测。']
+if 'independent_replay_integrity' in v:
+ x=v['independent_replay_integrity'];lines+=['','## 保存控制量重放','',f'独立 GPU 重放全部有限值：{x["all_finite"]}；相对原 Spider 轨迹最大 qpos 数值差：{x["max_difference_from_native_spider_qpos"]:.6g}（混合位置/角度单位）。独立重放未复现原 Spider 物体轨迹，存在明显接触动力学差异，不能作为已验证可重放的抓取控制。']
+(O/'README.md').write_text('\n'.join(lines)+'\n');print(json.dumps(extra,indent=2))

scripts/retarget_l20_video.py

@@ -23,11 +23,13 @@ def main():
     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)
+    p.add_argument('--side', choices=['right','left'], default='right')
     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()
+    fixed, urdf = build_assets(out, a.side)
+    asset = ASSET.parent/a.side.upper()
+    original = ET.parse(asset / f'linkerhand_g20_{a.side}.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}
@@ -109,7 +111,9 @@ def main():
         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
+    times = np.asarray(human['time'], dtype=np.float64) if 'time' in human else np.arange(count) / fps
+    assert times.shape == (count,) and np.isfinite(times).all() and np.all(np.diff(times) > 0)
+    times = times - times[0]
     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,

scripts/rl_track_reference.py

@@ -0,0 +1,194 @@
+"""PPO tracking policy: kinematic reference as PD targets + residual actions, MuJoCo Warp parallel physics.
+
+Reward (DexMachina/ManipTrans style): object pose tracking + landmark imitation + joint-space bc + contact-point term
+- action-rate/magnitude penalties. Virtual object controller (VOC) with decaying gains assists early training.
+Reference state initialization: episodes start at random demo frames. Eval: full demo from t=0, VOC off, deterministic.
+"""
+import os, sys, json, time, math, argparse
+from pathlib import Path
+ROOT = Path(__file__).resolve().parents[1]
+os.environ['WARP_CACHE_PATH'] = str(ROOT / '.spider_cache/warp'); os.environ.setdefault('MUJOCO_GL', 'osmesa')
+import numpy as np, torch, torch.nn as nn, mujoco, mujoco_warp as mw, warp as wp
+p = argparse.ArgumentParser()
+p.add_argument('--out', type=Path, default=ROOT / 'output/rl_track_20260915'); p.add_argument('--nworld', type=int, default=512); p.add_argument('--iters', type=int, default=1500)
+p.add_argument('--horizon', type=int, default=24); p.add_argument('--decimation', type=int, default=8); p.add_argument('--eval-every', type=int, default=50); p.add_argument('--seed', type=int, default=0)
+p.add_argument('--voc-kp', type=float, default=200.); p.add_argument('--resume', type=Path); p.add_argument('--smoke', action='store_true')
+p.add_argument('--phase-control', action='store_true', help='extra action: reference playback rate in [0,1.5]'); p.add_argument('--wrist-pos-scale', type=float, default=.03); p.add_argument('--wrist-rot-scale', type=float, default=.15); p.add_argument('--finger-scale', type=float, default=.4); p.add_argument('--prog-weight', type=float, default=.2); p.add_argument('--rate-max', type=float, default=1.5); p.add_argument('--seg-start', type=int, default=-1, help='segment mode: episodes start in [seg-start, seg-start+seg-jitter] (400 Hz index)'); p.add_argument('--seg-end', type=int, default=-1); p.add_argument('--seg-jitter', type=int, default=80); p.add_argument('--prog-mode', choices=['bonus', 'scale'], default='bonus', help='bonus: + w*(rate-1); scale: tracking reward multiplied by rate')
+p.add_argument('--voc-start', type=float, default=1.0); p.add_argument('--voc-zero-iters', type=int, default=0, help='iterations (from start) until assistance is forced to zero; 0 = 70%% of --iters'); p.add_argument('--contact-start-frac', type=float, default=0.0); p.add_argument('--vel-penalty', type=float, default=0.0)
+A = p.parse_args(); A.out.mkdir(parents=True, exist_ok=True); torch.manual_seed(A.seed); np.random.seed(A.seed)
+dev = torch.device('cuda:0'); wp.init()
+# ---------------- reference ----------------
+R = dict(np.load(A.out / 'reference.npz')); N = len(R['qpos']); ref = {k: torch.as_tensor(R[k], device=dev, dtype=torch.float32) for k in ['qpos', 'qvel', 'ctrl', 'contact', 'obj_pos', 'obj_quat', 'tip_pos', 'landmark_pos', 'cp_local']}
+cp_obj = torch.as_tensor(R['cp_obj'], device=dev, dtype=torch.long); OBJ = [int(x) for x in R['obj_body_ids']]; TIPS = [int(x) for x in R['tip_site_ids']]; LM = [int(x) for x in R['landmark_site_ids']]; WRIST = [int(x) for x in R['wrist_qpos_adr']]
+m = mujoco.MjModel.from_xml_path(str(R['scene'])); phys = json.loads(Path(str(R['physics_parameters'])).read_text()); m.opt.iterations = phys['solver_iterations']; m.opt.ls_iterations = phys['ls_iterations']
+assert not m.actuator_gainprm[44:].any(); NQ, NU = m.nq, m.nu; HAND_Q = 54; NA_H = 44; NA = NA_H + (1 if A.phase_control else 0)
+ctrl_lo = torch.as_tensor(m.actuator_ctrlrange[:NA_H, 0], device=dev); ctrl_hi = torch.as_tensor(m.actuator_ctrlrange[:NA_H, 1], device=dev); ctrl_lim = torch.as_tensor(m.actuator_ctrllimited[:NA_H].astype(bool), device=dev)
+ACT_SCALE = torch.tensor(([A.wrist_pos_scale] * 3 + [A.wrist_rot_scale] * 3 + [A.finger_scale] * 16) * 2, device=dev)
+d0 = mujoco.MjData(m); d0.qpos[:] = R['qpos'][0]; mujoco.mj_forward(m, d0)
+with wp.ScopedDevice('cuda:0'):
+    wm = mw.put_model(m); wd = mw.put_data(m, d0, nworld=A.nworld, nconmax=1024, njmax=3072)
+    with wp.ScopedCapture() as cap: mw.step(wm, wd)
+    graph = cap.graph
+T_ = lambda a: wp.to_torch(a)
+qpos, qvel, ctrl, xfrc, site_xpos, xpos, xquat, cvel = T_(wd.qpos), T_(wd.qvel), T_(wd.ctrl), T_(wd.xfrc_applied), T_(wd.site_xpos), T_(wd.xpos), T_(wd.xquat), T_(wd.cvel)
+EXTRA_RESET = [T_(getattr(wd, k)) for k in ['qacc_warmstart', 'qacc', 'qfrc_applied', 'act', 'act_dot', 'efc_force'] if hasattr(wd, k) and getattr(wd, k) is not None and getattr(wd, k).shape[0] == A.nworld]
+NW = A.nworld; DEC = A.decimation; CTRL_STEPS = N // DEC
+def kin():
+    torch.cuda.synchronize(); mw.kinematics(wm, wd); wp.synchronize()
+def sim_steps(n):
+    torch.cuda.synchronize()
+    for _ in range(n): wp.capture_launch(graph)
+    wp.synchronize()
+# ---------------- helpers ----------------
+def quat_mul(a, b):
+    w1, x1, y1, z1 = a.unbind(-1); w2, x2, y2, z2 = b.unbind(-1)
+    return torch.stack([w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2, w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2, w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2, w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2], -1)
+def quat_conj(q): return q * torch.tensor([1, -1, -1, -1], device=q.device)
+def quat_rotate(q, v):
+    qv = torch.cat([torch.zeros_like(v[..., :1]), v], -1); return quat_mul(quat_mul(q, qv), quat_conj(q))[..., 1:]
+def rotvec_err(q_ref, q):   # rotation vector taking q to q_ref (world frame)
+    dq = quat_mul(q_ref, quat_conj(q)); dq = dq * torch.sign(dq[..., :1] + 1e-12); ang = 2 * torch.acos(dq[..., 0].clamp(-1, 1)); s = torch.sqrt((1 - dq[..., 0] ** 2).clamp_min(1e-9)); return dq[..., 1:] / s[..., None] * ang[..., None], ang
+# ---------------- env state ----------------
+t_idx = torch.zeros(NW, dtype=torch.long, device=dev); t_pos = torch.zeros(NW, device=dev); max_len = torch.zeros(NW, dtype=torch.long, device=dev); ep_len = torch.zeros(NW, dtype=torch.long, device=dev); prev_a = torch.zeros(NW, NA, device=dev)
+voc_gain = torch.full((NW,), float(A.voc_start), device=dev); voc_gain[:16] = 0.   # first 16 worlds never get object assistance
+OBJ_QADR = torch.arange(54, 66, device=dev); OBJ_VADR = OBJ_QADR - 0   # nv == nq here (all hinge/slide)
+assert m.nv == m.nq
+CONTACT_FRAMES = torch.nonzero(ref['contact'][:N - 2 * DEC * 60].sum(-1) > 0).squeeze(-1)
+def reset(mask, start=None):
+    idx = torch.nonzero(mask).squeeze(-1)
+    if len(idx) == 0: return
+    if start is None and A.seg_start >= 0:
+        s = A.seg_start + torch.randint(0, max(1, A.seg_jitter), (len(idx),), device=dev)
+    elif start is None:
+        s = torch.randint(0, N - 2 * DEC * 60, (len(idx),), device=dev)
+        if A.contact_start_frac > 0 and len(CONTACT_FRAMES):
+            pick = torch.rand(len(idx), device=dev) < A.contact_start_frac; s = torch.where(pick, CONTACT_FRAMES[torch.randint(0, len(CONTACT_FRAMES), (len(idx),), device=dev)], s)
+    else: s = torch.full((len(idx),), int(start), device=dev, dtype=torch.long)
+    qpos[idx] = ref['qpos'][s]; qvel[idx] = ref['qvel'][s] * 0.; ctrl[idx] = ref['ctrl'][s]; xfrc[idx] = 0
+    for arr in EXTRA_RESET: arr[idx] = 0
+    qpos[idx] = ref['qpos'][s]; t_idx[idx] = s; t_pos[idx] = s.float(); max_len[idx] = (((A.seg_end if A.seg_end > 0 else N) - s).float() / DEC * 1.5).long() + 5; ep_len[idx] = 0; prev_a[idx] = 0
+def obj_state():
+    op = xpos[:, OBJ]; oq = xquat[:, OBJ]; ov = qvel[:, OBJ_QADR].view(NW, 2, 6); return op, oq, ov[..., :3], ov[..., 3:]
+def apply_voc(t):
+    op, oq, lv, av = obj_state(); pr = ref['obj_pos'][t]; qr = ref['obj_quat'][t]; g = voc_gain[:, None, None]
+    F = (A.voc_kp * (pr - op) - 10. * lv).clamp(-20, 20) * g; rv, _ = rotvec_err(qr, oq); Tq = (2. * rv - .1 * av).clamp(-1, 1) * g
+    xfrc[:, OBJ, :3] = F; xfrc[:, OBJ, 3:] = Tq
+def observe(t):
+    hq = qpos[:, :HAND_Q]; hv = qvel[:, :HAND_Q]; hq_ref = ref['qpos'][t, :HAND_Q]; op, oq, lv, av = obj_state(); pr = ref['obj_pos'][t]; qr = ref['obj_quat'][t]
+    rv, ang = rotvec_err(qr, oq); t2 = (t + 20).clamp(max=N - 1); t3 = (t + 40).clamp(max=N - 1)
+    tips = site_xpos[:, TIPS]; rel = (tips[:, :, None, :] - op[:, None, :, :]).reshape(NW, -1)
+    obs = torch.cat([hq, hv * .05, hq - hq_ref, (op - pr).reshape(NW, -1), oq.reshape(NW, -1), rv.reshape(NW, -1), lv.reshape(NW, -1) * .2, av.reshape(NW, -1) * .05, (ref['obj_pos'][t2] - op).reshape(NW, -1), (ref['obj_pos'][t3] - op).reshape(NW, -1), rel, ref['contact'][t], (t.float() / N)[:, None], voc_gain[:, None], prev_a], -1)
+    return obs
+def reward(t, a):
+    op, oq, lv, av = obj_state(); pr = ref['obj_pos'][t]; qr = ref['obj_quat'][t]; ep = (op - pr).norm(dim=-1); _, ang = rotvec_err(qr, oq)
+    r_obj = (torch.exp(-20 * ep) * torch.exp(-3 * ang)).mean(-1)
+    lm = site_xpos[:, LM]; e_lm = (lm - ref['landmark_pos'][t]).norm(dim=-1).mean(-1); r_imi = torch.exp(-20 * e_lm)
+    dq = qpos[:, :HAND_Q] - ref['qpos'][t, :HAND_Q]; w = torch.ones(HAND_Q, device=dev); w[WRIST[:3]] = 10; w[WRIST[6:9]] = 10; r_bc = torch.exp(-2 * (dq.abs() * w).mean(-1))
+    con = ref['contact'][t] > .5; cpl = ref['cp_local'][t]; co = cp_obj[t]; oq_sel = torch.gather(oq, 1, co[..., None].expand(-1, -1, 4)); op_sel = torch.gather(op, 1, co[..., None].expand(-1, -1, 3))
+    target = quat_rotate(oq_sel, cpl) + op_sel; dtip = (site_xpos[:, TIPS] - target).norm(dim=-1); rc = torch.exp(-50 * dtip); n_on = con.sum(-1); r_con = torch.where(n_on > 0, (rc * con).sum(-1) / n_on.clamp_min(1), torch.full_like(r_obj, .5))
+    pen = .05 * ((a - prev_a[:, :NA_H]) ** 2).mean(-1) + .01 * (a ** 2).mean(-1)
+    if A.vel_penalty > 0:
+        tn = (t + DEC).clamp(max=N - 1); v_ref = (ref['obj_pos'][tn] - ref['obj_pos'][t]) / (DEC * float(m.opt.timestep)); dv = ((lv - v_ref) ** 2).sum(-1).clamp(max=4.).mean(-1); pen = pen + A.vel_penalty * dv
+    track = 1.0 * r_obj + .3 * r_imi + .1 * r_bc + .5 * r_con; r = track - pen
+    return r, dict(r_obj=r_obj, r_imi=r_imi, r_bc=r_bc, r_con=r_con, obj_err_cm=ep.mean(-1) * 100, obj_rot_deg=ang.mean(-1) * 57.3, lm_err_cm=e_lm * 100, track=track, pen=pen, rot_each=ang * 57.3, err_each=ep * 100)
+FAIL_THR = torch.tensor(.15, device=dev)
+def step(a):
+    t = t_idx; hand = ref['ctrl'][t, :NA_H] + a[:, :NA_H] * ACT_SCALE; hand = torch.where(ctrl_lim, hand.clamp(ctrl_lo, ctrl_hi), hand); ctrl[:, :NA_H] = hand; ctrl[:, NA_H:] = ref['ctrl'][t, NA_H:]
+    rate = (1. + .5 * a[:, NA_H]).clamp(0., A.rate_max) if A.phase_control else torch.ones(NW, device=dev)
+    apply_voc(t); sim_steps(DEC); t_pos.add_(DEC * rate); t_idx[:] = t_pos.round().long().clamp(max=N - 1); ep_len.add_(1); t = t_idx
+    r, info = reward(t, a[:, :NA_H]); op, oq, _, _ = obj_state(); ep = (op - ref['obj_pos'][t]).norm(dim=-1).max(-1).values
+    if A.phase_control:
+        if A.prog_mode == 'scale': r = info['track'] * rate - info['pen']
+        else: r = r + A.prog_weight * (rate - 1.)
+        info['rate'] = rate
+    wrist_err = torch.stack([(qpos[:, WRIST[:3]] - ref['qpos'][t][:, WRIST[:3]]).norm(dim=-1), (qpos[:, WRIST[6:9]] - ref['qpos'][t][:, WRIST[6:9]]).norm(dim=-1)], -1).max(-1).values
+    seg_end = A.seg_end if A.seg_end > 0 else N - 1
+    bad = ~torch.isfinite(qpos).all(-1) | (ep > FAIL_THR) | (wrist_err > .25) | (ep_len >= max_len); trunc = (t_pos >= seg_end) & ~bad
+    r = torch.where(bad, r - 1., r); done = bad | trunc; prev_a[:] = a; info['fail'] = bad.float(); info['trunc'] = trunc.float(); info['nan_worlds'] = (~torch.isfinite(qpos).all(-1)).float().sum().expand(1)
+    return r, done, trunc, info
+# ---------------- PPO ----------------
+class RunningNorm:
+    def __init__(s, n): s.mean = torch.zeros(n, device=dev); s.var = torch.ones(n, device=dev); s.count = 1e-4
+    def update(s, x):
+        x = x[torch.isfinite(x).all(-1)]
+        if len(x) < 2: return
+        bm, bv, bc = x.mean(0), x.var(0, unbiased=False), x.shape[0]; delta = bm - s.mean; tot = s.count + bc
+        s.mean = s.mean + delta * bc / tot; s.var = (s.var * s.count + bv * bc + delta ** 2 * s.count * bc / tot) / tot; s.count = tot
+    def __call__(s, x): return torch.nan_to_num((x - s.mean) / torch.sqrt(s.var + 1e-6), nan=0., posinf=10., neginf=-10.).clamp(-10, 10)
+    def state(s): return dict(mean=s.mean, var=s.var, count=s.count)
+def mlp(i, o, h=(512, 256, 128)):
+    layers = []; last = i
+    for hh in h: layers += [nn.Linear(last, hh), nn.ELU()]; last = hh
+    return nn.Sequential(*layers, nn.Linear(last, o))
+reset(torch.ones(NW, dtype=torch.bool, device=dev)); kin(); OBS = observe(t_idx).shape[1]
+actor, critic = mlp(OBS, NA).to(dev), mlp(OBS, 1).to(dev); log_std = nn.Parameter(torch.full((NA,), math.log(.5), device=dev))
+opt = torch.optim.Adam(list(actor.parameters()) + list(critic.parameters()) + [log_std], lr=3e-4); norm = RunningNorm(OBS); lr = 3e-4
+start_iter = 0
+if A.resume:
+    ck = torch.load(A.resume, map_location=dev); actor.load_state_dict(ck['actor']); critic.load_state_dict(ck['critic']); log_std.data[:] = ck['log_std']; norm.mean, norm.var, norm.count = ck['norm']['mean'], ck['norm']['var'], ck['norm']['count']; voc_gain[16:] = min(ck.get('voc', 1.), float(A.voc_start)); start_iter = ck.get('iter', 0); lr = ck.get('lr', lr)
+    for g_ in opt.param_groups: g_['lr'] = lr
+GAMMA, LAM, CLIP, EPOCHS, MB = .99, .95, .2, 5, 4; H = A.horizon
+from torch.utils.tensorboard import SummaryWriter
+tb = SummaryWriter(str(A.out / 'tb')); csv = open(A.out / 'train_log.csv', 'a')
+def evaluate(it):
+    res_all = {}
+    for mode in ['det', 'sto']:
+        res_all[mode] = evaluate_mode(it, mode)
+    res = res_all['det']; res.update({k + '_sto': v for k, v in res_all['sto'].items() if k not in ('iter',)})
+    (A.out / 'eval_log.jsonl').open('a').write(json.dumps(res) + '\n'); print('EVAL', json.dumps(res), flush=True)
+    torch.save(dict(actor=actor.state_dict(), critic=critic.state_dict(), log_std=log_std.data, norm=norm.state(), voc=float(voc_gain[16:].mean()), iter=it, lr=lr, obs_dim=OBS), A.out / f'ckpt_iter{it:05d}.pt'); torch.save(dict(actor=actor.state_dict(), critic=critic.state_dict(), log_std=log_std.data, norm=norm.state(), voc=float(voc_gain[16:].mean()), iter=it, lr=lr, obs_dim=OBS), A.out / 'ckpt_latest.pt')
+    reset(torch.ones(NW, dtype=torch.bool, device=dev)); kin()
+def evaluate_mode(it, mode):
+    reset(torch.ones(NW, dtype=torch.bool, device=dev), start=max(0, A.seg_start)); kin(); g_save = voc_gain.clone(); voc_gain[:] = 0; errs = []; rots = []; rot_each = []; err_each = []; traj = []; times = []; alive = torch.ones(NW, dtype=torch.bool, device=dev); seg_end = A.seg_end if A.seg_end > 0 else N - 1
+    with torch.no_grad():
+        for k in range(int(CTRL_STEPS * 1.5) + 5):
+            o = norm(observe(t_idx.clamp(max=N - 1))); mu = actor(o); a = (mu if mode == 'det' else torch.distributions.Normal(mu, log_std.exp()).sample()).clamp(-3, 3); r, done, trunc, info = step(a)
+            alive &= ~(info['fail'] > 0); errs.append(info['obj_err_cm']); rots.append(info['obj_rot_deg']); rot_each.append(info['rot_each']); err_each.append(info['err_each']); traj.append(qpos[0].clone()); times.append(float(k * DEC * m.opt.timestep))
+            if t_pos[0] >= seg_end or (t_pos >= seg_end).all(): break
+    E = torch.stack(errs); Rr = torch.stack(rots); RE = torch.stack(rot_each); EE = torch.stack(err_each); voc_gain[:] = g_save; ok = torch.isfinite(E).all(0) & torch.isfinite(Rr).all(0); E = E[:, ok]; Rr = Rr[:, ok]; alive = alive[ok]; RE = RE[:, ok]; EE = EE[:, ok]
+    nlast = max(1, len(errs) // 10); final_rot = RE[-nlast:].mean(0); final_err = EE[-nlast:].mean(0)   # per world, per object, last 10% of the rollout
+    res = dict(iter=it, finite_worlds=int(ok.sum()), final_rot_deg_red_median=float(final_rot[:, 0].median()), final_rot_deg_blue_median=float(final_rot[:, 1].median()), flip_success_blue_frac=float(((final_rot[:, 1] < 30) & (final_err[:, 1] < 10)).float().mean()), red_ok_frac=float(((final_rot[:, 0] < 30) & (final_err[:, 0] < 10)).float().mean()), phase_reached_frac=float((t_pos[0] - max(0, A.seg_start)) / (seg_end - max(0, A.seg_start))), eval_steps=len(errs), obj_err_cm_mean=float(E.mean()), obj_err_cm_max_over_time_mean=float(E.max(0).values.mean()), obj_rot_deg_mean=float(Rr.mean()), survived_frac=float(alive.float().mean()), success_5cm_frac=float((E.max(0).values < 5).float().mean()), success_10cm_frac=float((E.max(0).values < 10).float().mean()))
+    np.savez_compressed(A.out / (f'eval_iter{it:05d}.npz' if mode == 'det' else f'eval_iter{it:05d}_sto.npz'), qpos=torch.stack(traj).cpu().numpy(), time=np.array(times), metrics=json.dumps(res), final_rot_deg=final_rot.cpu().numpy(), final_err_cm=final_err.cpu().numpy())
+    return res
+robj_hist = []
+iters = 3 if A.smoke else A.iters
+for it in range(start_iter, start_iter + iters):
+    t0 = time.perf_counter(); obs_b = torch.zeros(H, NW, OBS, device=dev); act_b = torch.zeros(H, NW, NA, device=dev); mu_b = torch.zeros(H, NW, NA, device=dev); std_old = log_std.exp().detach().clone(); logp_b = torch.zeros(H, NW, device=dev); rew_b = torch.zeros(H, NW, device=dev); done_b = torch.zeros(H, NW, device=dev); val_b = torch.zeros(H + 1, NW, device=dev); agg = {}
+    with torch.no_grad():
+        for k in range(H):
+            o_raw = observe(t_idx.clamp(max=N - 1)); norm.update(o_raw); o = norm(o_raw); mu = actor(o); std = log_std.exp(); dist = torch.distributions.Normal(mu, std); a = dist.sample().clamp(-3, 3); logp = dist.log_prob(a).sum(-1); v = critic(o).squeeze(-1)
+            r, done, trunc, info = step(a); r = torch.nan_to_num(r, nan=-1., posinf=1., neginf=-1.)
+            # bootstrap truncated episodes with value of next state
+            if trunc.any():
+                kin(); vn = critic(norm(observe(t_idx.clamp(max=N - 1)))).squeeze(-1); r = torch.where(trunc & ~(info['fail'] > 0), r + GAMMA * vn, r)
+            obs_b[k], act_b[k], logp_b[k], rew_b[k], done_b[k], val_b[k], mu_b[k] = o, a, logp, r, done.float(), v, mu
+            for kk, vv in info.items(): agg.setdefault(kk, []).append(vv.mean().item())
+            reset(done); kin()
+        val_b[H] = critic(norm(observe(t_idx.clamp(max=N - 1)))).squeeze(-1)
+    adv = torch.zeros(H, NW, device=dev); last = torch.zeros(NW, device=dev)
+    for k in reversed(range(H)):
+        nd = 1. - done_b[k]; delta = rew_b[k] + GAMMA * val_b[k + 1] * nd - val_b[k]; last = delta + GAMMA * LAM * nd * last; adv[k] = last
+    ret = adv + val_b[:H]; adv_n = (adv - adv.mean()) / (adv.std() + 1e-8)
+    B = H * NW; ob, ab, lpb, rb, advb, vb, mub = obs_b.reshape(B, -1), act_b.reshape(B, -1), logp_b.reshape(B), ret.reshape(B), adv_n.reshape(B), val_b[:H].reshape(B), mu_b.reshape(B, -1); kls = []
+    for ep in range(EPOCHS):
+        perm = torch.randperm(B, device=dev)
+        for mb in range(MB):
+            i = perm[mb * B // MB:(mb + 1) * B // MB]; mu_new = actor(ob[i]); dist = torch.distributions.Normal(mu_new, log_std.exp()); lp = dist.log_prob(ab[i]).sum(-1); ratio = torch.exp(lp - lpb[i])
+            s1 = ratio * advb[i]; s2 = ratio.clamp(1 - CLIP, 1 + CLIP) * advb[i]; pl = -torch.min(s1, s2).mean(); vpred = critic(ob[i]).squeeze(-1); vcl = vb[i] + (vpred - vb[i]).clamp(-CLIP, CLIP); vl = torch.max((vpred - rb[i]) ** 2, (vcl - rb[i]) ** 2).mean()
+            loss = pl + .5 * vl; opt.zero_grad(); loss.backward(); nn.utils.clip_grad_norm_(list(actor.parameters()) + list(critic.parameters()) + [log_std], 1.); opt.step()
+            with torch.no_grad():   # analytic KL(old || new) between diagonal Gaussians (rsl_rl style)
+                s_new = log_std.exp(); kl = (torch.log(s_new / std_old) + (std_old ** 2 + (mub[i] - mu_new) ** 2) / (2 * s_new ** 2) - .5).sum(-1).mean().item(); kls.append(kl)
+            if kl > .02: lr = max(1e-5, lr / 1.5)
+            elif kl < .005: lr = min(1e-3, lr * 1.5)
+            for g_ in opt.param_groups: g_['lr'] = lr
+    # VOC curriculum: decay when object tracking is good; hard schedule guarantees zero by 70% of training
+    robj_hist.append(np.mean(agg['r_obj'])); gated = len(robj_hist) >= 10 and np.mean(robj_hist[-10:]) > .6
+    zero_iters = A.voc_zero_iters if A.voc_zero_iters > 0 else int(.7 * A.iters); sched = max(0., A.voc_start * (1. - (it - start_iter) / zero_iters)); g = voc_gain[16:].mean().item()
+    if gated: g *= .97
+    g = min(g, sched); g = 0. if g < .02 else g; voc_gain[16:] = g
+    fps = B * DEC / (time.perf_counter() - t0); row = dict(iter=it, fps=int(fps), rew=float(rew_b.mean()), ep_len=float(ep_len.float().mean()), voc=g, lr=lr, kl=float(np.mean(kls)), std=float(log_std.exp().mean()), **{k: float(np.mean(v)) for k, v in agg.items()})
+    for k, v in row.items(): tb.add_scalar(k, v, it)
+    csv.write(json.dumps(row) + '\n'); csv.flush()
+    if it % 5 == 0 or A.smoke: print(' '.join(f'{k}={v:.3g}' if isinstance(v, float) else f'{k}={v}' for k, v in row.items()), flush=True)
+    if (it + 1) % A.eval_every == 0 or A.smoke and it == start_iter + iters - 1: evaluate(it + 1)
+print('TRAIN_DONE', flush=True)

scripts/run_dynhamr_rgbd_20260915.py

@@ -0,0 +1,49 @@
+"""Run both-hand Dyn-HaMR with measured intrinsics and RGB-D odometry."""
+import json
+import os
+from pathlib import Path
+import subprocess
+import time
+import numpy as np
+
+ROOT = Path(__file__).resolve().parents[1]
+OUT = ROOT / 'output/20260915_171525_dynhamr'
+OUT.mkdir(parents=True, exist_ok=True)
+SEQ = '20260915_171525'
+dataset = OUT / 'dataset'
+(dataset / 'videos').mkdir(parents=True, exist_ok=True)
+video = dataset / 'videos' / (SEQ + '.mp4')
+if not video.exists():
+    video.symlink_to(ROOT / 'docs' / SEQ / 'color.mp4')
+camera = np.load(ROOT / 'output' / SEQ / 'rgbd_camera.npz')
+assert camera['c2w'].shape == (352, 4, 4)
+for kind, data in [('pose', camera['c2w']), ('intrinsics', np.tile(camera['intrinsics'], (352, 1)))]:
+    directory = OUT / 'rgbd_cameras' / kind
+    directory.mkdir(parents=True, exist_ok=True)
+    np.savez(directory / (SEQ + '.npz'), data=data.astype(np.float32), inds=np.arange(352))
+(OUT / 'camera_provenance.json').write_text(json.dumps(dict(
+    source=str(ROOT / 'output' / SEQ / 'rgbd_camera.npz'),
+    method=str(camera['method']), adapter='ViPE file schema only; cameras estimated from D405 RGB-D',
+    hand_reconstruction='Dyn-HaMR', hands=['left', 'right']), indent=2))
+dyn = ROOT / 'third_party/Dyn-HaMR'
+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',
+           MPLCONFIGDIR='/tmp/matplotlib-dynhamr', HYDRA_FULL_ERROR='1',
+           PYOPENGL_PLATFORM='egl', LD_LIBRARY_PATH='')
+env['PATH'] = str(dyn / '.dynhamr/bin') + ':' + env['PATH']
+env['PYTHONPATH'] = ':'.join(str(dyn / p) for p in ['dyn-hamr', 'third-party/hamer', 'third-party/hamer/third-party/ViTPose'])
+cmd = [str(dyn / '.dynhamr/bin/python'), '-u', 'run_opt.py', 'data=video_vipe',
+       'data.root=' + str(dataset), 'data.seq="' + SEQ + '"',
+       'data.vipe_dir=' + str(OUT / 'rgbd_cameras'), 'data.frame_opts.fps=30',
+       'is_static=False', 'model.opt_scale=False', 'run_opt=True', 'run_vis=True',
+       'run_prior=True', '+data.vipe_pipeline=dynhamr_cameras',
+       'HMP.vid_path=' + str(video), 'hydra.run.dir=' + str(OUT / 'optimization')]
+def status(stage, **kw):
+    (OUT / 'status.json').write_text(json.dumps(dict(stage=stage,
+        updated=time.strftime('%Y-%m-%d %H:%M:%S'), pid=os.getpid(), **kw), indent=2))
+status('RUNNING', command=cmd)
+with (OUT / 'run.log').open('a') as log:
+    result = subprocess.run(cmd, cwd=dyn / 'dyn-hamr', env=env, stdout=log, stderr=subprocess.STDOUT)
+status('RECONSTRUCTION_PROCESS_EXITED' if result.returncode == 0 else 'FAILED', returncode=result.returncode)
+raise SystemExit(result.returncode)

scripts/run_red_handflow_comparison.py

@@ -0,0 +1,15 @@
+"""Prepare RGB-only HandFlow baseline for both hands with fixed camera calibration."""
+import os,subprocess,json
+from pathlib import Path
+ROOT=Path(__file__).resolve().parents[1];OUT=ROOT/'output/depth_ablation_red_20260916';m=json.loads((ROOT/'docs/20260916_104026/intrinsics.json').read_text())
+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',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'))
+for side in ['right','left']:
+ dst=OUT/side;dst.mkdir(exist_ok=True)
+ if (dst/'handflow_results.npz').exists():continue
+ cmd=[str(ROOT/'.venv/bin/python'),'-u','scripts/demo.py','--input',str(OUT/'input_315.mkv'),'--fm_ckpt',str(ROOT/'weights/handflow_denoiser.pt'),'--intrinsics',','.join(str(m[k]) for k in ['fx','fy','cx','cy']),'--fix_camera','--side',side,'--output_dir',str(dst),'--save_npz',str(dst/'handflow_results.npz')]
+ print('Running',side,flush=True)
+ for attempt in range(2):
+  if (dst/'handflow_results.npz').exists():break
+  with (dst/'inference.log').open('a') as f:subprocess.run(cmd,cwd=ROOT,env=env,stdout=f,stderr=subprocess.STDOUT,check=True)
+ assert (dst/'handflow_results.npz').exists(), f'Missing final results: {side}'
+print('Both RGB-only baselines complete',flush=True)

scripts/run_red_mirrored_handflow.py

@@ -0,0 +1,16 @@
+"""Prepare RGB-only HandFlow baseline for both hands with fixed camera calibration."""
+import os,subprocess,json
+from pathlib import Path
+ROOT=Path(__file__).resolve().parents[1];OUT=ROOT/'output/depth_ablation_red_20260916';m=json.loads((ROOT/'docs/20260916_104026/intrinsics.json').read_text())
+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',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'))
+m['cx']=m['width']-1-m['cx']
+for side in ['left_mirrored']:
+ dst=OUT/side;dst.mkdir(exist_ok=True)
+ if (dst/'handflow_results.npz').exists():continue
+ cmd=[str(ROOT/'.venv/bin/python'),'-u','scripts/demo.py','--input',str(OUT/'input_315_mirrored.mkv'),'--fm_ckpt',str(ROOT/'weights/handflow_denoiser.pt'),'--intrinsics',','.join(str(m[k]) for k in ['fx','fy','cx','cy']),'--fix_camera','--side','right','--output_dir',str(dst),'--save_npz',str(dst/'handflow_results.npz')]
+ print('Running',side,flush=True)
+ for attempt in range(2):
+  if (dst/'handflow_results.npz').exists():break
+  with (dst/'inference.log').open('a') as f:subprocess.run(cmd,cwd=ROOT,env=env,stdout=f,stderr=subprocess.STDOUT,check=True)
+ assert (dst/'handflow_results.npz').exists(), f'Missing final results: {side}'
+print('Mirrored-left RGB-only baseline complete',flush=True)

scripts/run_rgbd_20260915.py

@@ -0,0 +1,47 @@
+"""Resume the 20260915_171525 RGB-D right-hand reconstruction."""
+import os
+from pathlib import Path
+import subprocess
+import json
+import time
+import argparse
+import numpy as np
+
+parser=argparse.ArgumentParser()
+parser.add_argument('--left',action='store_true')
+args=parser.parse_args()
+
+ROOT=Path(__file__).resolve().parents[1]
+SOURCE=ROOT/'docs/20260915_171525'
+OUT=ROOT/('output/20260915_171525/handflow_left_mirrored' if args.left else 'output/20260915_171525/handflow')
+OUT.mkdir(parents=True,exist_ok=True)
+meta=json.loads((SOURCE/'intrinsics.json').read_text())
+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',str(SOURCE/'color.mp4'),
+     '--fm_ckpt',str(ROOT/'weights/handflow_denoiser.pt'),'--intrinsics',
+     ','.join(str(meta[k]) for k in ['fx','fy','cx','cy']),'--side','right',
+     '--rotation_smooth_window','5','--output_dir',str(OUT),'--save_npz',str(OUT/'handflow_results.npz')]
+if args.left:
+    cmd[cmd.index('--input')+1]=str(OUT.parent/'left_mirrored.mkv')
+    intr=np.array([meta[k] for k in ['fx','fy','cx','cy']]);intr[2]=meta['width']-1-intr[2]
+    c=np.load(OUT.parent/'rgbd_camera.npz');reflection=np.diag([-1.,1.,1.,1.])
+    np.savez(OUT/'camera_slam.npz',intrinsics=intr,c2w=reflection@c['c2w']@reflection)
+status=OUT.parent/'status.json'
+for attempt in range(2):
+    if (OUT/'handflow_results.npz').exists():break
+    status.write_text(json.dumps(dict(stage='RECONSTRUCTION_RUNNING',attempt=attempt,command=cmd,time=time.time()),indent=2))
+    with (OUT/'inference.log').open('a') as log:
+        result=subprocess.run(cmd,cwd=ROOT,env=env,stdout=log,stderr=subprocess.STDOUT)
+    if result.returncode:
+        status.write_text(json.dumps(dict(stage='RECONSTRUCTION_FAILED',returncode=result.returncode,time=time.time()),indent=2))
+        raise SystemExit(result.returncode)
+if not (OUT/'handflow_results.npz').exists():raise RuntimeError('No final reconstruction arrays')
+status.write_text(json.dumps(dict(stage='RECONSTRUCTION_DONE_PENDING_DEPTH_AND_RETARGET',time=time.time()),indent=2))

scripts/run_yesterday_spider.py

@@ -0,0 +1,38 @@
+"""Actual upstream SPIDER, with L20 passive-joint reward indexing corrected locally."""
+import os,sys,json,importlib.util,argparse
+from pathlib import Path
+ROOT=Path(__file__).resolve().parents[1];OUT=Path(os.environ.get('SPIDER_TASK_OUT',str(ROOT/'output/foundationpose_spider_20260915')))
+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['OMP_NUM_THREADS']='4'
+sys.path.insert(0,str(ROOT/'third_party/spider'))
+import torch
+torch.set_num_threads(4)
+from spider.config import Config
+import spider.simulators.mjwp as sim
+original=sim._weight_diff_qpos
+def weights(config):
+ if config.embodiment_type!='bimanual':return original(config)
+ # 27 state DOFs per L20 (6 wrist+21 finger), versus 22 actuator channels.
+ half=(config.nv-12)//2;w=torch.full((config.nv,),config.joint_rew_scale,device=config.device)
+ for start in [0,half]:w[start:start+3]=config.base_pos_rew_scale;w[start+3:start+6]=config.base_rot_rew_scale
+ for start in [config.nv-12,config.nv-6]:w[start:start+3]=config.pos_rew_scale;w[start+3:start+6]=config.rot_rew_scale
+ return w
+sim._weight_diff_qpos=weights
+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)
+# Cross-object regrasp invalidates upstream one-object-per-hand delta heuristic.
+# Retain reference object assistance schedule and per-finger contact reward.
+module.compute_contact_point_delta=lambda *args,**kwargs: None
+get_qpos_original=module.get_qpos
+def checked_qpos(config,env):
+ q=get_qpos_original(config,env)
+ if not torch.isfinite(q[0]).all():raise RuntimeError('Nonfinite executed state: stop instead of saving invalid trajectory')
+ return q
+module.get_qpos=checked_qpos
+p=argparse.ArgumentParser();p.add_argument('--pilot',action='store_true');p.add_argument('--steps',type=int);args=p.parse_args();c=json.loads((OUT/'config.json').read_text())
+if args.pilot:c.update(max_sim_steps=80,num_samples=32,max_num_iterations=2)
+if args.steps:c['max_sim_steps']=args.steps
+if (OUT/'physics_parameters.json').exists():
+ settings=json.loads((OUT/'physics_parameters.json').read_text());setup_original=sim.setup_mj_model
+ def configured_model(config):
+  m=setup_original(config);m.opt.iterations=settings['solver_iterations'];m.opt.ls_iterations=settings['ls_iterations'];return m
+ sim.setup_mj_model=configured_model;module.setup_mj_model=configured_model
+module.main(Config(**c));print('SPIDER_RUN_COMPLETE',flush=True)

scripts/smooth_object_reference.py

@@ -0,0 +1,200 @@
+"""Physically-consistent object reference: rest segments frozen (and snapped to the table),
+held segments rigidly bound to the holding hand, free segments lightly smoothed, camera odometry smoothed.
+
+Input : output/registered_spider_20260915/foundationpose_objects.npz (raw FoundationPose, raw odometry)
+        output/20260915_171525_dynhamr/registered/ (hands, raw-odometry world)
+Output: output/final_spider_20260915/foundationpose_objects.npz (same schema; *_valid all True where a pose is
+        defined, *_observed_raw keeps the original acceptance flags), rgbd_camera_smooth.npz, object_segments.npz,
+        object_smoothing.json. Validation renders the CAD into the sensor depth before/after.
+"""
+import os, sys, json
+os.environ.setdefault('OMP_NUM_THREADS', '4')
+from pathlib import Path
+ROOT = Path(__file__).resolve().parents[1]
+sys.path.insert(0, str(ROOT / 'third_party/FoundationPose')); sys.path.insert(0, str(ROOT / 'scripts'))
+import numpy as np, cv2, torch, trimesh, open3d as o3d, nvdiffrast.torch as dr
+from scipy.ndimage import gaussian_filter1d
+from scipy.spatial.transform import Rotation as R, Slerp
+from Utils import nvdiffrast_render
+from red_box_distance import signed_distance
+
+SRC = ROOT / 'docs/20260915_171525'; OLD = ROOT / 'output/registered_spider_20260915'; HAND = ROOT / 'output/20260915_171525_dynhamr/registered'
+OUT = ROOT / 'output/final_spider_20260915'; OUT.mkdir(exist_ok=True)
+meta = json.loads((SRC / 'intrinsics.json').read_text()); fp = dict(np.load(OLD / 'foundationpose_objects.npz')); N = 352
+K = fp['K']; c2w_raw = fp['c2w']; w2cR = np.transpose(c2w_raw[:, :3, :3], (0, 2, 1)); tO = -np.einsum('tij,tj->ti', w2cR, c2w_raw[:, :3, 3])
+SIG_CAM, SIG_FREE, SIG_HELD, BLEND = 2., 2., 4., 4
+REST_WIN, REST_POS, REST_ROT, REST_MIN = 15, .006, 2., 10      # 0.5 s window, 6 mm, 2 deg, min 10 frames
+ENGAGE, RELEASE, MIN_TIPS = .025, .035, 2
+
+def smooth_rot(rot, sigma, idx=None):
+    n = len(rot); out = []
+    for t in range(n):
+        ix = np.arange(max(0, t - int(3 * sigma)), min(n, t + int(3 * sigma) + 1)); w = np.exp(-.5 * ((ix - t) / sigma) ** 2)
+        out.append(rot[ix].mean(weights=w))
+    return R.concatenate(out)
+def smooth_T(T, sigma):
+    out = T.copy(); out[:, :3, 3] = gaussian_filter1d(T[:, :3, 3], sigma, axis=0, mode='nearest'); out[:, :3, :3] = smooth_rot(R.from_matrix(T[:, :3, :3]), sigma).as_matrix(); return out
+def interp_T(T, valid):
+    idx = np.flatnonzero(valid); t = np.arange(len(T)); out = T.copy(); clip = np.clip(t, idx[0], idx[-1])
+    out[:, :3, 3] = np.stack([np.interp(t, idx, T[idx, k, 3]) for k in range(3)], 1); out[:, :3, :3] = Slerp(idx, R.from_matrix(T[idx, :3, :3]))(clip).as_matrix(); out[:, 3, :] = [0, 0, 0, 1]; return out
+def inv(T): o = np.eye(4); o[:3, :3] = T[:3, :3].T; o[:3, 3] = -T[:3, :3].T @ T[:3, 3]; return o
+
+# --- camera: smoothed odometry ---
+c2w_s = smooth_T(c2w_raw, SIG_CAM)
+np.savez_compressed(OUT / 'rgbd_camera_smooth.npz', c2w=c2w_s, intrinsics=np.array([meta[k] for k in ['fx', 'fy', 'cx', 'cy']]), time=fp['time'], valid=fp['camera_valid'], method='registered_spider c2w, Gaussian sigma 2 frames on position and SO(3)')
+# --- hands in camera frame (independent of odometry) ---
+hands = {}
+for side in ['right', 'left']:
+    hj = np.load(HAND / f'human_joints_{side}.npz', allow_pickle=True); mo = np.load(HAND / f'l20_{side}_stable/motion.npz', allow_pickle=True)
+    tips_w = hj['joints'][:, [4, 8, 12, 16, 20]] + hj['wrist_world'][:, None]; tips_c = np.einsum('tij,tkj->tki', w2cR, tips_w) + tO[:, None]
+    Th = np.tile(np.eye(4), (N, 1, 1)); Th[:, :3, :3] = np.einsum('tij,tjk->tik', w2cR, mo['wrist_world_R']); Th[:, :3, 3] = np.einsum('tij,tj->ti', w2cR, mo['wrist_world']) + tO
+    hands[side] = dict(tips_cam=tips_c, T_cam=Th)
+# --- table plane (frame 0, camera-0 frame), as in prepare ---
+cap = cv2.VideoCapture(str(SRC / 'color.mp4')); ok, img0 = cap.read(); cap.release(); hsv0 = cv2.cvtColor(img0, cv2.COLOR_BGR2HSV)
+dep0 = cv2.imread(str(SRC / 'depth/000000.png'), -1) * meta['depth_scale_m']; yy, xx = np.indices(dep0.shape)
+mask = (xx > 100) & (xx < 780) & (yy > 200) & (yy < 460) & (hsv0[:, :, 1] < 50) & (dep0 > .2) & (dep0 < 1)
+pts = np.stack([(xx - K[0, 2]) * dep0 / K[0, 0], (yy - K[1, 2]) * dep0 / K[1, 1], dep0], -1)[mask][::3]
+pc = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(pts)); plane, _ = pc.segment_plane(.004, 3, 1000); n_cam = np.array(plane[:3]); off = plane[3]
+if n_cam[2] > 0: n_cam, off = -n_cam, -off          # normal points up (toward the camera side)
+# --- meshes ---
+scenes = {}
+for name, stl in [('upper', '上半.stl'), ('lower', '下半.stl')]:
+    closed = trimesh.load(ROOT / f'output/collision_fix_20260915/collision_v2/{name}_closed.ply', process=False); assert closed.is_watertight
+    sc = o3d.t.geometry.RaycastingScene(); sc.add_triangles(o3d.t.geometry.TriangleMesh.from_legacy(o3d.geometry.TriangleMesh(o3d.utility.Vector3dVector(closed.vertices), o3d.utility.Vector3iVector(closed.faces))))
+    scenes[name] = dict(scene=sc, closed=closed, render=trimesh.load(ROOT / 'docs' / stl))
+def tip_dist(name, T_cam, tips_cam):
+    local = (tips_cam - T_cam[:3, 3]) @ T_cam[:3, :3]; return np.abs(signed_distance(scenes[name]['scene'], scenes[name]['closed'], local))
+
+ctx = dr.RasterizeCudaContext(); mt = {}
+for name in scenes:
+    m_ = scenes[name]['render']; mt[name] = {'pos': torch.as_tensor(m_.vertices, device='cuda', dtype=torch.float32), 'faces': torch.as_tensor(m_.faces, device='cuda', dtype=torch.int32), 'vnormals': torch.zeros((len(m_.vertices), 3), device='cuda'), 'vertex_color': torch.ones((len(m_.vertices), 3), device='cuda')}
+def render(name, T):
+    with torch.inference_mode(): _, d, _ = nvdiffrast_render(K=K, H=480, W=848, ob_in_cams=torch.as_tensor(T[None], device='cuda', dtype=torch.float32), glctx=ctx, mesh_tensors=mt[name])
+    return d[0].cpu().numpy()
+_cap = cv2.VideoCapture(str(SRC / 'color.mp4')); _frames = {}
+def frame_data(f):
+    if f not in _frames:
+        _cap.set(cv2.CAP_PROP_POS_FRAMES, f); ok, b = _cap.read(); assert ok
+        z = cv2.imread(str(SRC / 'depth' / f'{f:06d}.png'), -1).astype('float32') * meta['depth_scale_m']; h = cv2.cvtColor(b, cv2.COLOR_BGR2HSV); _frames[f] = (z, h)
+    return _frames[f]
+def depth_residual(name, f, T):
+    z, h = frame_data(f); hue = h[:, :, 0]
+    cm = (((hue < 12) | (hue > 170)) if name == 'upper' else ((hue > 95) & (hue < 135))) & (h[:, :, 1] > 90) & (h[:, :, 2] > 40) & (z > .1) & (z < 1.); cm[:200] = False
+    d = render(name, T); over = (d > 0) & cm
+    return float(np.median(np.abs(d[over] - z[over])) * 1000) if over.sum() > 200 else None
+report = {'parameters': dict(camera_sigma=SIG_CAM, free_sigma=SIG_FREE, held_relative_sigma=SIG_HELD, blend_frames=BLEND, rest_window=REST_WIN, rest_pos_m=REST_POS, rest_rot_deg=REST_ROT, engage_m=ENGAGE, release_m=RELEASE, min_tips=MIN_TIPS), 'objects': {}}
+out = dict(fp); labels = {}
+for name in ['upper', 'lower']:
+    T_raw = fp[name + '_T_camera'].copy(); valid = fp[name + '_valid'].astype(bool).copy(); obs = valid.copy()
+    last = N if name == 'upper' else 177                     # lower unobservable after 176: hold
+    T = interp_T(T_raw, valid); T[last:] = T[last - 1]
+    # holder per frame (camera frame, hysteresis per hand)
+    holder = np.full(N, '', dtype=object); engaged = {s: np.zeros(5, bool) for s in hands}
+    for t in range(N):
+        best, best_n = '', 0
+        for s in hands:
+            dist = tip_dist(name, T[t], hands[s]['tips_cam'][t]); engaged[s] = np.where(engaged[s], dist < RELEASE, dist < ENGAGE)
+            n_on = int(engaged[s].sum())
+            if n_on >= MIN_TIPS and (n_on > best_n or (n_on == best_n and s == holder[t - 1] if t else False)): best, best_n = s, n_on
+        holder[t] = best
+    # rest test on lightly pre-smoothed poses, over a 0.5 s window
+    pre = smooth_T(T, 2.); rest = np.zeros(N, bool); h = REST_WIN // 2
+    for t in range(N):
+        a, b = max(0, t - h), min(N - 1, t + h); dp = np.linalg.norm(pre[b, :3, 3] - pre[a, :3, 3]); dr_ = (R.from_matrix(pre[a, :3, :3]).inv() * R.from_matrix(pre[b, :3, :3])).magnitude() * 180 / np.pi
+        rest[t] = dp < REST_POS and dr_ < REST_ROT
+    rest[last:] = True
+    lab = np.array(['free'] * N, dtype=object); lab[rest] = 'rest'
+    for t in range(N):
+        if not rest[t] and holder[t]: lab[t] = 'held:' + holder[t]
+    # drop short rest runs; then merge rest-(free with no hand)-rest, since a static object cannot move by itself
+    t = 0
+    while t < N:
+        u = t
+        while u < N and lab[u] == lab[t]: u += 1
+        if lab[t] == 'rest' and u - t < REST_MIN: lab[t:u] = 'free'
+        t = u
+    changed = True
+    while changed:
+        changed = False; t = 0
+        while t < N:
+            u = t
+            while u < N and lab[u] == lab[t]: u += 1
+            if lab[t] == 'free' and t > 0 and u < N and lab[t - 1] == 'rest' and lab[u] == 'rest' and not any(holder[t:u]):
+                lab[t:u] = 'rest'; changed = True
+            t = u
+    # per-segment poses
+    S = T.copy(); alt = T.copy(); segs = []; t = 0; depth_split = 0
+    while t < N:
+        u = t
+        while u < N and lab[u] == lab[t]: u += 1
+        kind = lab[t]; sl = slice(t, u)
+        if kind == 'rest':
+            # consensus pose: among the median and sampled observed poses, take the one the sensor depth agrees with best
+            fr = [f for f in range(t, u, 3) if obs[f]]
+            Tm = np.eye(4); Tm[:3, :3] = R.from_matrix(T[sl, :3, :3]).mean().as_matrix(); Tm[:3, 3] = np.median(T[sl, :3, 3], axis=0)
+            cands = [('median', Tm)] + [(f'obs{f}', T[f]) for f in range(t, u, max(1, (u - t) // 12)) if obs[f]]
+            def score(Tc): rs = [depth_residual(name, f, Tc) for f in fr]; rs = [x for x in rs if x is not None]; return float(np.median(rs)) if rs else 1e9
+            scored = [(score(Tc), lab_, Tc) for lab_, Tc in cands]; best_score, best_lab, Tr = min(scored, key=lambda x: x[0]); Tr = Tr.copy()
+            cl = scenes[name]['closed']; hv = (cl.vertices @ Tr[:3, :3].T + Tr[:3, 3]) @ n_cam + off; gap = float(hv.min())   # signed height of lowest vertex above the table
+            snapped = False
+            if abs(gap) < .03:
+                Ts = Tr.copy(); Ts[:3, 3] -= n_cam * gap; snap_score = score(Ts)
+                if snap_score <= best_score + 1.0: Tr, snapped, best_score = Ts, True, snap_score
+            # split test: if the consensus pose disagrees with the sensor for a sustained run, the object actually moved
+            per = [(f, depth_residual(name, f, Tr), depth_residual(name, f, T[f])) for f in fr]; bad = [f for f, a, b in per if a is not None and b is not None and a > b + 3.0]
+            runs = []; 
+            for f in bad:
+                if runs and f - runs[-1][-1] <= 3: runs[-1].append(f)
+                else: runs.append([f])
+            long_runs = [r for r in runs if len(r) >= 3]
+            if long_runs and u - t >= 2 * REST_MIN and depth_split < 3:
+                cut = long_runs[0][0] if long_runs[0][0] - t >= REST_MIN else long_runs[0][-1] + 1
+                if t + REST_MIN <= cut <= u - REST_MIN:
+                    lab[cut:cut + 3] = 'free'; depth_split += 1; continue   # re-segment from t with the new boundary
+            S[sl] = Tr; segs.append(dict(kind='rest', start=t, end=u - 1, table_gap_mm=gap * 1000, snapped=snapped, pose_source=best_lab, depth_residual_mm=best_score, median_pose_residual_mm=scored[0][0], split_runs=[(r[0], r[-1]) for r in long_runs]))
+        elif kind.startswith('held'):
+            s = kind.split(':')[1]; Th = hands[s]['T_cam'][sl]; rel = np.array([inv(Th[i]) @ T[t + i] for i in range(u - t)])
+            rel_s = smooth_T(rel, SIG_HELD) if u - t > 2 else rel; S[sl] = np.array([Th[i] @ rel_s[i] for i in range(u - t)])
+            alt[sl] = smooth_T(T[sl], SIG_FREE) if u - t > 2 else T[sl]
+            segs.append(dict(kind=kind, start=t, end=u - 1, relative_position_2nd_diff_rms_mm_before=float(np.sqrt((np.linalg.norm(np.diff(rel[:, :3, 3], n=2, axis=0), axis=1) ** 2).mean()) * 1000) if u - t > 2 else 0., after=float(np.sqrt((np.linalg.norm(np.diff(rel_s[:, :3, 3], n=2, axis=0), axis=1) ** 2).mean()) * 1000) if u - t > 2 else 0.))
+        else:
+            S[sl] = smooth_T(T[sl], SIG_FREE) if u - t > 2 else T[sl]; segs.append(dict(kind='free', start=t, end=u - 1))
+        t = u
+    # held segments: keep the hand-bound version only if the sensor depth agrees at least as well as plain smoothing
+    for sg in segs:
+        if not sg['kind'].startswith('held') or sg['end'] - sg['start'] < 3: continue
+        fr = [f for f in range(sg['start'], sg['end'] + 1, 3) if obs[f]]
+        rb = [depth_residual(name, f, S[f]) for f in fr]; ra = [depth_residual(name, f, alt[f]) for f in fr]; rb = [x for x in rb if x is not None]; ra = [x for x in ra if x is not None]
+        sg['depth_residual_bound_mm'] = float(np.median(rb)) if rb else None; sg['depth_residual_smoothed_mm'] = float(np.median(ra)) if ra else None
+        if rb and ra and np.median(rb) > np.median(ra) + 1.0: S[sg['start']:sg['end'] + 1] = alt[sg['start']:sg['end'] + 1]; sg['chosen'] = 'own-trajectory smoothing'
+        else: sg['chosen'] = 'hand-bound'
+    # blend across segment boundaries
+    B = S.copy()
+    for i in range(1, len(segs)):
+        b = segs[i]['start']; a0, a1 = max(0, b - BLEND), min(N, b + BLEND); rot = R.from_matrix(np.stack([S[a0, :3, :3], S[a1 - 1, :3, :3]])); sl = Slerp([a0, a1 - 1], rot)
+        for k in range(a0, a1):
+            w = (k - a0) / max(1, a1 - 1 - a0); B[k, :3, 3] = (1 - w) * S[a0, :3, 3] + w * S[a1 - 1, :3, 3]; B[k, :3, :3] = sl(k).as_matrix()
+    # keep observed frames close to observation: report deviation
+    dev = np.linalg.norm(B[obs, :3, 3] - T_raw[obs, :3, 3], axis=1) * 1000; rdev = (R.from_matrix(B[obs, :3, :3]).inv() * R.from_matrix(T_raw[obs, :3, :3])).magnitude() * 180 / np.pi
+    def jit(X): p = X[:, :3, 3]; rr = R.from_matrix(X[:, :3, :3]); return dict(pos_2nd_diff_rms_mm=float(np.sqrt((np.linalg.norm(np.diff(p, n=2, axis=0), axis=1) ** 2).mean()) * 1000), pos_step_p95_mm=float(np.percentile(np.linalg.norm(np.diff(p, axis=0), axis=1), 95) * 1000), rot_step_p95_deg=float(np.percentile((rr[:-1].inv() * rr[1:]).magnitude() * 180 / np.pi, 95)))
+    out[name + '_T_camera'] = B; out[name + '_T_world'] = c2w_s @ B; out[name + '_valid'] = np.r_[np.ones(last, bool), np.zeros(N - last, bool)]; out[name + '_observed_raw'] = obs
+    labels[name] = lab.astype(str)
+    report['objects'][name] = dict(segments=segs, frames_rest=int((lab == 'rest').sum()), frames_held=int(np.char.startswith(lab.astype(str), 'held').sum()), frames_free=int((lab == 'free').sum()), jitter_before=jit(T[:last]), jitter_after=jit(B[:last]), deviation_from_observed_mm=dict(median=float(np.median(dev)), p95=float(np.percentile(dev, 95)), max=float(dev.max())), deviation_rot_deg=dict(median=float(np.median(rdev)), p95=float(np.percentile(rdev, 95))))
+out['c2w'] = c2w_s
+np.savez_compressed(OUT / 'foundationpose_objects.npz', **out); np.savez_compressed(OUT / 'object_segments.npz', **{k: v for k, v in labels.items()})
+# --- validation against the sensor depth (every 3rd frame) ---
+val = {n: {'raw': [], 'smoothed': []} for n in scenes}
+for t in range(0, N, 3):
+    for name in scenes:
+        if not fp[name + '_valid'][t]: continue
+        for key, T in [('raw', fp[name + '_T_camera'][t]), ('smoothed', out[name + '_T_camera'][t])]:
+            r = depth_residual(name, t, T)
+            if r is not None: val[name][key].append(r)
+for name in scenes: report['objects'][name]['depth_residual_mm'] = {k: dict(median=float(np.median(v)), p95=float(np.percentile(v, 95)), frames=len(v)) for k, v in val[name].items()}
+report['camera'] = dict(pos_2nd_diff_rms_mm_before=float(np.sqrt((np.linalg.norm(np.diff(c2w_raw[:, :3, 3], n=2, axis=0), axis=1) ** 2).mean()) * 1000), after=float(np.sqrt((np.linalg.norm(np.diff(c2w_s[:, :3, 3], n=2, axis=0), axis=1) ** 2).mean()) * 1000), max_change_mm=float(np.linalg.norm(c2w_s[:, :3, 3] - c2w_raw[:, :3, 3], axis=1).max() * 1000))
+report['table_plane_camera0'] = dict(normal=n_cam.tolist(), offset=float(off))
+(OUT / 'object_smoothing.json').write_text(json.dumps(report, indent=2, default=str)); print(json.dumps({k: v for k, v in report.items() if k != 'objects'}, indent=1))
+for name in scenes:
+    r = report['objects'][name]; print(name, 'rest/held/free', r['frames_rest'], r['frames_held'], r['frames_free'], '| jitter before', {k: round(v, 2) for k, v in r['jitter_before'].items()}, '| after', {k: round(v, 2) for k, v in r['jitter_after'].items()})
+    print('   deviation from observed', {k: round(v, 1) for k, v in r['deviation_from_observed_mm'].items()}, '| depth residual', r['depth_residual_mm'])
+    print('   segments:', [(s['kind'], s['start'], s['end'], ('snap' if s.get('snapped') else '') + (f"gap{s['table_gap_mm']:.0f}" if 'table_gap_mm' in s else '') + (f" {s['chosen']} b{s['depth_residual_bound_mm']:.1f}/s{s['depth_residual_smoothed_mm']:.1f}" if s.get('chosen') and s.get('depth_residual_bound_mm') is not None else '')) for s in r['segments']])

scripts/smooth_project_reference.py

@@ -0,0 +1,114 @@
+"""Smooth the collision-corrected L20 reference, then re-project every frame to be collision-free (boxes + table).
+
+Smoothing is applied separately: wrist position (Gaussian), wrist rotation (SO(3) local weighted mean),
+independent finger joints (Savitzky-Golay, then exact mimic expansion). Objects are untouched.
+Projection: per frame, minimal weighted change s.t. linearised contact distances >= clearance and joint limits.
+Env: HF_FIX_OUT (input fix dir), HF_FIX_OLD (prepare dir), HF_SMOOTH_OUT (output dir), HF_TABLE=1 to include the table,
+     HF_SIGMA_POS (frames, default 2), HF_SIGMA_ROT (default 2), HF_SG_WINDOW (default 9), HF_CLEARANCE (m, default .0025).
+"""
+import os, json, shutil
+os.environ.setdefault('MUJOCO_GL', 'osmesa'); os.environ['OPENBLAS_NUM_THREADS'] = '1'; os.environ['OMP_NUM_THREADS'] = '1'
+from pathlib import Path
+import numpy as np, mujoco
+from scipy.optimize import minimize
+from scipy.ndimage import gaussian_filter1d
+from scipy.signal import savgol_filter
+from scipy.spatial.transform import Rotation as R
+import l20_model_source as source
+ROOT = Path(__file__).resolve().parents[1]
+IN = Path(os.environ['HF_FIX_OUT']); OLD = Path(os.environ['HF_FIX_OLD']); OUT = Path(os.environ['HF_SMOOTH_OUT']); OUT.mkdir(parents=True, exist_ok=True)
+TABLE = os.environ.get('HF_TABLE', '0') == '1'; PAIRS = [{1, 2}, {1, 4}] if TABLE else [{1, 2}]
+SIG_POS = float(os.environ.get('HF_SIGMA_POS', '2')); SIG_ROT = float(os.environ.get('HF_SIGMA_ROT', '2')); SG = int(os.environ.get('HF_SG_WINDOW', '11')); CLEAR = float(os.environ.get('HF_CLEARANCE', '.0025'))
+source.REPO_ROOT = ROOT / 'third_party/l20_assets'
+TIN = IN / 'datasets/processed/current/l20/bimanual/boxes'; TOUT = OUT / 'datasets/processed/current/l20/bimanual/boxes'; (TOUT / '0').mkdir(parents=True, exist_ok=True)
+shutil.copy2(TIN / 'scene_act.xml', TOUT / 'scene_act.xml'); shutil.copy2(TIN / 'task_info.json', TOUT / 'task_info.json')
+cfg = json.loads((IN / 'config.json').read_text()); cfg['dataset_dir'] = str(OUT / 'datasets'); (OUT / 'config.json').write_text(json.dumps(cfg, indent=2))
+for name in ['collision_v2', 'hand_collision']:
+    if not (OUT / name).exists(): os.symlink(os.path.relpath(IN / name, OUT), OUT / name)
+m = mujoco.MjModel.from_xml_path(str(TOUT / 'scene_act.xml')); d = mujoco.MjData(m)
+ref = dict(np.load(IN / 'reference_video_rate.npz')); q0 = ref['qpos'].copy(); N = len(q0); contact = ref['contact']; cp = ref['contact_pos']
+meta = {}
+for side in ['right', 'left']:
+    k = source.HandKinematics(OLD / f'model_{side}/l20_{side}.xml', side)
+    wa = np.array([m.jnt_qposadr[m.joint(f'{side}_hand_{s}').id] for s in ['pos_x', 'pos_y', 'pos_z', 'rot_x', 'rot_y', 'rot_z']]); ja = np.array([m.jnt_qposadr[m.joint(f'{side}_{n}').id] for n in k.joint_names])
+    B = np.zeros((m.nv, 22)); B[wa, :6] = np.eye(6); B[ja, 6:] = k.expansion
+    meta[side] = dict(k=k, wa=wa, ja=ja, B=B, sites=[m.site(f'{side}_hand_{f}_track').id for f in source.FINGERS])
+
+def smooth_rot_euler(e, sigma):
+    rot = R.from_euler('XYZ', e); out = []
+    for t in range(len(e)):
+        ix = np.arange(max(0, t - int(3 * sigma)), min(len(e), t + int(3 * sigma) + 1)); w = np.exp(-.5 * ((ix - t) / sigma) ** 2); out.append(rot[ix].mean(weights=w))
+    es = R.concatenate(out).as_euler('XYZ'); es += 2 * np.pi * np.round((e - es) / (2 * np.pi))   # stay on the original continuous branch
+    return es
+def smooth(q):
+    qs = q.copy()
+    for side, mt in meta.items():
+        k, wa, ja = mt['k'], mt['wa'], mt['ja']
+        qs[:, wa[:3]] = gaussian_filter1d(q[:, wa[:3]], SIG_POS, axis=0, mode='nearest'); qs[:, wa[3:]] = smooth_rot_euler(q[:, wa[3:]], SIG_ROT)
+        ind = q[:, ja][:, k.independent_indices]; ind = savgol_filter(ind, SG, 2, axis=0, mode='nearest'); ind = np.clip(ind, k.lower, k.upper)
+        qs[:, ja] = np.array([k.expand(a) for a in ind])
+    return qs
+def contacts(side, B, derivatives=True):
+    rows, dist = [], []
+    for c in d.contact:
+        g0, g1 = map(int, c.geom); types = {int(m.geom_contype[g0]), int(m.geom_contype[g1])}
+        if types not in PAIRS: continue
+        hg = g0 if m.geom_contype[g0] == 1 else g1
+        if not m.geom(hg).name.startswith(side + '_'): continue
+        dist.append(float(c.dist))
+        if derivatives:
+            jac = np.zeros((3, m.nv)); mujoco.mj_jac(m, d, jac, None, c.pos, int(m.geom_bodyid[hg])); normal = c.frame[:3] * (1 if hg == g1 else -1); rows.append(normal @ jac @ B)
+    return np.asarray(rows).reshape(-1, 22), np.array(dist)
+def put(side, x, base):
+    mt = meta[side]; d.qpos[:] = base; d.qpos[mt['wa']] = x[:6]; d.qpos[mt['ja']] = mt['k'].expand(x[6:]); mujoco.mj_fwdPosition(m, d)
+W = np.r_[[1.] * 3, [.05] * 3, [.01] * 16]     # metres, radians: prefer moving fingers, then wrist rotation, then wrist position
+STEP = np.r_[[.008] * 3, [.12] * 3, [.15] * 16]   # per-frame bound relative to the previous projected frame: 8 mm, 7 deg, 8.6 deg
+def solve(side, x, base, prev):
+    k, B = meta[side]['k'], meta[side]['B']
+    for it in range(10):
+        put(side, x, base); A, ds = contacts(side, B)
+        if not len(ds) or ds.min() >= CLEAR - 2e-5: break
+        lo = np.r_[[-.03] * 3, [-.2] * 3, k.lower - x[6:]]; hi = np.r_[[.03] * 3, [.2] * 3, k.upper - x[6:]]
+        if prev is not None: lo = np.maximum(lo, prev - STEP - x); hi = np.minimum(hi, prev + STEP - x); lo = np.minimum(lo, 0); hi = np.maximum(hi, 0)
+        H = np.diag((W / np.r_[[.01] * 3, [.05] * 3, [.05] * 16]) ** 2)
+        fit = minimize(lambda z: .5 * z @ H @ z, np.zeros(22), jac=lambda z: H @ z, bounds=list(zip(lo, hi)), constraints=[dict(type='ineq', fun=lambda z: A @ z + ds - CLEAR - 3e-4, jac=lambda z: A)], method='SLSQP', options={'ftol': 1e-10, 'maxiter': 100})
+        x = x + fit.x
+    put(side, x, base); _, ds = contacts(side, B, False); short = max(0., CLEAR - ds.min()) if len(ds) else 0.
+    return x, short
+def project(q):
+    qp = q.copy(); moves = {s: [] for s in meta}; worst = 0.; fallback = 0; prev = {s: None for s in meta}
+    for f in range(N):
+        for side, mt in meta.items():
+            k, wa, ja = mt['k'], mt['wa'], mt['ja']; base = qp[f].copy(); x0 = np.r_[base[wa], base[ja][k.independent_indices]]
+            x, short = solve(side, x0.copy(), base, prev[side])
+            if short > 2e-4 and prev[side] is not None:   # temporal bound made it infeasible: release the bound for this frame
+                x, short = solve(side, x0.copy(), base, None); fallback += 1
+            worst = max(worst, short); put(side, x, base); qp[f] = d.qpos.copy(); moves[side].append(float(np.linalg.norm(x[:3] - x0[:3]) * 1000)); prev[side] = x
+    print('projection: frames with released temporal bound', fallback, flush=True)
+    return qp, moves, worst
+def stats(q, label):
+    out = {}
+    for side, mt in meta.items():
+        wa, ja, k = mt['wa'], mt['ja'], mt['k']; p = q[:, wa[:3]]; step = np.linalg.norm(np.diff(p, axis=0), axis=1) * 1000; acc = np.linalg.norm(np.diff(p, n=2, axis=0), axis=1) * 1000
+        rot = R.from_euler('XYZ', q[:, wa[3:]]); rs = (rot[:-1].inv() * rot[1:]).magnitude() * 180 / np.pi; fj = q[:, ja][:, k.independent_indices]; fs = np.abs(np.diff(fj, axis=0)).max(1) * 180 / np.pi; fa = np.sqrt((np.diff(fj, n=2, axis=0) ** 2).mean(1)) * 180 / np.pi
+        gaps = []; pen = []
+        for f in range(N):
+            d.qpos[:] = q[f]; mujoco.mj_fwdPosition(m, d); on = contact[f, (0 if side == 'right' else 5):(5 if side == 'right' else 10)].astype(bool)
+            if on.any(): gaps.extend(np.linalg.norm(d.site_xpos[mt['sites']][on] - cp[f][(0 if side == 'right' else 5):(5 if side == 'right' else 10)][on], axis=1) * 1000)
+            _, ds = contacts(side, mt['B'], False); pen.append(max(0., -ds.min()) * 1000 if len(ds) else 0.)
+        out[side] = dict(wrist_step_mm=dict(median=float(np.median(step)), p95=float(np.percentile(step, 95)), max=float(step.max())), wrist_2nd_diff_rms_mm=float(np.sqrt((acc ** 2).mean())), wrist_rot_step_deg=dict(p95=float(np.percentile(rs, 95)), max=float(rs.max())), finger_step_deg=dict(p95=float(np.percentile(fs, 95)), max=float(fs.max())), finger_2nd_diff_rms_deg=float(fa.mean()), contact_gap_mm=dict(median=float(np.median(gaps)), p95=float(np.percentile(gaps, 95))) if gaps else None, max_penetration_mm=float(max(pen)), frames_penetrating_over_0_5mm=int(np.sum(np.array(pen) > .5)))
+    print(label, json.dumps(out), flush=True); return out
+report = {'parameters': dict(sigma_pos=SIG_POS, sigma_rot=SIG_ROT, sg_window=SG, clearance_m=CLEAR, table=TABLE), 'input': stats(q0, 'input')}
+q1 = smooth(q0); report['after_smoothing_only'] = stats(q1, 'smoothed')
+q2, moves, worst = project(q1); report['after_projection'] = stats(q2, 'projected'); report['projection_move_mm'] = {s: dict(median=float(np.median(v)), max=float(np.max(v))) for s, v in moves.items()}; report['worst_clearance_shortfall_mm'] = worst * 1000
+q3 = smooth(q2); q3[:, :] = q3; qs = q3.copy()
+# second, lighter pass: blend half-way toward the re-smoothed solution, then project again
+q3 = .5 * (q2 + q3); q4, moves2, worst2 = project(q3); report['after_second_pass'] = stats(q4, 'pass2'); report['projection_move_pass2_mm'] = {s: dict(median=float(np.median(v)), max=float(np.max(v))) for s, v in moves2.items()}
+assert np.array_equal(q4[:, 54:], q0[:, 54:]) and np.isfinite(q4).all()
+np.savez_compressed(OUT / 'reference_video_rate.npz', qpos=q4, contact=contact, contact_pos=cp, time=ref['time'], camera_world_from_cv=ref['camera_world_from_cv'])
+np.savez_compressed(OUT / 'smoothing_comparison.npz', before=q0, after=q4, time=ref['time'])
+times = np.load(IN / 'datasets/processed/current/l20/bimanual/boxes/0/trajectory_kinematic_act.npz')['time']; t = ref['time']
+qi = np.stack([np.interp(times, t, q4[:, i]) for i in range(m.nq)], 1); vel = np.gradient(qi, .0025, axis=0); vel[0] = 0; ctrl = qi[:, m.jnt_qposadr[m.actuator_trnid[:, 0]]]
+idx = np.minimum(np.searchsorted(t, times), N - 1); ci = np.stack([np.interp(times, t, cp.reshape(N, -1)[:, j]) for j in range(30)], 1).reshape(-1, 10, 3)
+np.savez_compressed(TOUT / '0/trajectory_kinematic_act.npz', qpos=qi, qvel=vel, ctrl=ctrl, contact=contact[idx], contact_pos=ci, time=times)
+(OUT / 'smoothing_report.json').write_text(json.dumps(report, indent=2)); print('SMOOTH_PROJECT_DONE', flush=True)

scripts/smooth_refitted_objects.py

@@ -0,0 +1,35 @@
+from pathlib import Path
+import json,numpy as np
+from scipy.ndimage import gaussian_filter1d
+from scipy.spatial.transform import Rotation
+ROOT=Path(__file__).resolve().parents[1];B=ROOT/'output/20260915_171525_dynhamr';a=dict(np.load(B/'object_refit_tracked/object_poses.npz'));report={}
+for n in ['upper','lower']:
+ center=np.array([0, .00415 if n=='upper' else -.03335, .2185 if n=='upper' else .21975])
+ r=Rotation.from_quat(a[n+'_quaternion_xyzw']);c=r.apply(np.tile(center,(352,1)))+a[n+'_position_world'];cs=gaussian_filter1d(c,2,axis=0)
+ rs=[]
+ for t in range(352):
+  ix=np.arange(max(0,t-6),min(352,t+7));rs.append(r[ix].mean(weights=np.exp(-.5*((ix-t)/2)**2)).as_quat())
+ rr=Rotation.from_quat(rs)
+ # Resolve visible-face ambiguity using the source video: flat red face and circular blue feature.
+ rr=rr*Rotation.from_matrix(np.diag([-1.,-1.,1.]))
+ a[n+'_position_world']=cs-rr.apply(np.tile(center,(352,1)));a[n+'_quaternion_xyzw']=rr.as_quat()
+ angles=np.rad2deg((rr[:-1].inv()*rr[1:]).magnitude());steps=np.linalg.norm(np.diff(cs,axis=0),axis=1)*1000
+ report[n]={'max_center_step_mm':float(steps.max()),'max_rotation_step_deg':float(angles.max()),'center_smoothing_max_change_mm':float(np.linalg.norm(cs-c,axis=1).max()*1000)}
+ assert np.isfinite(cs).all() and angles.max()<20 and steps.max()<30
+# Video inspection: lower part becomes occluded during placement (frames 180-214),
+# and the common CAD assembly is visible from frame 215. This is an explicit assumption.
+from scipy.spatial.transform import Slerp
+hold_p=a['lower_position_world'][176].copy();hold_q=a['lower_quaternion_xyzw'][176].copy()
+a['lower_position_world'][177:190]=hold_p;a['lower_quaternion_xyzw'][177:190]=hold_q
+for t in range(190,215):
+ alpha=(t-189)/26
+ a['lower_position_world'][t]=(1-alpha)*hold_p+alpha*a['upper_position_world'][t]
+ a['lower_quaternion_xyzw'][t]=Slerp([0,1],Rotation.from_quat([hold_q,a['upper_quaternion_xyzw'][t]]))([alpha]).as_quat()[0]
+a['lower_position_world'][215:]=a['upper_position_world'][215:]
+a['lower_quaternion_xyzw'][215:]=a['upper_quaternion_xyzw'][215:]
+a['lower_keyframe_valid'][177:]=False
+a['lower_assembly_assumed']=np.arange(352)>=215
+a['lower_occluded_transition_assumed']=(np.arange(352)>=177)&(np.arange(352)<215)
+report['assembly_assumption']={'from_frame':215,'transition_frames':[177,214],'basis':'Visual inspection and shared CAD assembly coordinates; not independent measured lower poses.'}
+np.savez_compressed(B/'object_aligned/object_poses.npz',**a)
+(B/'object_aligned/smoothing_validation.json').write_text(json.dumps(report,indent=2));print(report)

scripts/smooth_table_constrained.py

@@ -0,0 +1,96 @@
+"""Smooth the corrected reference and project hand poses above the table."""
+from pathlib import Path
+import json, shutil
+import numpy as np
+import mujoco
+from scipy.signal import savgol_filter
+
+ROOT = Path(__file__).resolve().parents[1]
+SRC = ROOT / "output/spider_dynamics_fix_20260915"
+OUT = ROOT / "output/desktop_smooth_fix_20260915"
+OUT.mkdir(exist_ok=True)
+shutil.copy2(SRC / "reference_video_rate.npz", OUT / "reference_before_smoothing.npz")
+shutil.copy2(SRC / "datasets/processed/current/l20/bimanual/boxes/scene_act.xml", OUT / "scene_act.xml")
+
+z = np.load(SRC / "reference_video_rate.npz")
+arrays = {k: z[k].copy() for k in z.files}
+q = arrays["qpos"]
+original = q.copy()
+m = mujoco.MjModel.from_xml_path(str(OUT / "scene_act.xml"))
+d = mujoco.MjData(m)
+# Filter each hand component independently. The object six-DOF states are kept
+# exactly as estimated; only hand states are regularized.
+window, poly = (9, 2)
+for sl in (slice(0, 27), slice(27, 54)):
+    q[:, sl] = savgol_filter(q[:, sl], window_length=window, polyorder=poly, axis=0, mode="interp")
+
+# Recompute the exact five linear-mimic joints from the MuJoCo equality rows.
+for e in range(m.neq):
+    j1, j2 = m.eq_obj1id[e], m.eq_obj2id[e]
+    a1, a2 = m.jnt_qposadr[j1], m.jnt_qposadr[j2]
+    p = m.eq_data[e, :5]
+    q[:, a1] = sum(p[k] * q[:, a2] ** k for k in range(5))
+
+floor_ids = [i for i in range(m.ngeom) if "_floor" in (m.geom(i).name or "")]
+for i in floor_ids:
+    m.geom_conaffinity[i] = 3
+hand_geom = {
+    "right": [i for i in range(m.ngeom) if m.geom_contype[i] == 1 and (m.geom(i).name or "").startswith("right_")],
+    "left": [i for i in range(m.ngeom) if m.geom_contype[i] == 1 and (m.geom(i).name or "").startswith("left_")],
+}
+hand_vertices = {}
+for side in ("right", "left"):
+    hand_vertices[side] = []
+    for i in hand_geom[side]:
+        mid = int(m.geom_dataid[i])
+        if mid < 0:
+            continue
+        # Collision meshes are convex approximations; use the original visual
+        # mesh vertices to detect the visible hand crossing the table.
+        start = int(m.mesh_vertadr[mid]); end = start + int(m.mesh_vertnum[mid])
+        vertices = m.mesh_vert[start:end].copy()
+        hand_vertices[side].append((i, vertices[::max(1, len(vertices)//128)]))
+
+def min_floor_dist(side):
+    floor_z = float(d.geom_xpos[floor_ids[0]][2])
+    vals = []
+    for i, vertices in hand_vertices[side]:
+        world = vertices @ d.geom_xmat[i].reshape(3, 3).T + d.geom_xpos[i]
+        vals.append(float(world[:, 2].min() - floor_z))
+    return min(vals) if vals else 1.0
+
+projected = []
+for f in range(len(q)):
+    d.qpos[:] = q[f]
+    mujoco.mj_forward(m, d)
+    for side, sl in (("right", slice(0, 27)), ("left", slice(27, 54))):
+        # Binary search the smallest wrist-z lift giving 0.5 mm clearance.
+        base = q[f].copy()
+        d.qpos[:] = base; mujoco.mj_forward(m, d)
+        if min_floor_dist(side) < 0.0005:
+            lo, hi = 0.0, 0.15
+            for _ in range(24):
+                mid = (lo + hi) / 2
+                trial = base.copy(); trial[sl.start + 2] += mid
+                d.qpos[:] = trial; mujoco.mj_forward(m, d)
+                if min_floor_dist(side) >= 0.0005: hi = mid
+                else: lo = mid
+            q[f, sl.start + 2] += hi
+            projected.append((f, side, hi))
+
+arrays["qpos"] = q
+dt = float(np.median(np.diff(arrays["time"])))
+arrays["qvel"] = np.gradient(q, dt, axis=0); arrays["qvel"][0] = 0
+arrays["ctrl"] = q[:, m.jnt_qposadr[m.actuator_trnid[:, 0]]]
+np.savez_compressed(OUT / "reference_video_rate.npz", **arrays)
+report = {
+    "frames": len(q), "filter": {"type": "Savitzky-Golay", "window": window, "polyorder": poly},
+    "projected_hand_table_contacts": len(projected),
+    "max_table_lift_mm": float(max((x[2] for x in projected), default=0) * 1000),
+    "max_hand_change_mm": float(max(np.linalg.norm(q[:, :3]-original[:, :3], axis=1).max(), np.linalg.norm(q[:, 27:30]-original[:, 27:30], axis=1).max()) * 1000),
+    "object_qpos_unchanged": bool(np.array_equal(q[:, -12:], original[:, -12:])),
+    "all_finite": bool(np.isfinite(q).all()),
+    "note": "Table projection uses the simulation floor plane and hand collision geoms; object pose remains fixed."
+}
+(OUT / "smoothing_table_report.json").write_text(json.dumps(report, indent=2))
+print(json.dumps(report, indent=2))

scripts/stabilize_l20_temporal.py

@@ -18,6 +18,7 @@ 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')
+    p.add_argument('--side', choices=['right','left'], default='right')
     a = p.parse_args()
     a.output.mkdir(parents=True, exist_ok=True)
     with np.load(a.source/'motion.npz') as f:
@@ -28,7 +29,7 @@ def main():
     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()
+    original = ET.parse(ROOT/f'third_party/l20_assets/L20/{a.side.upper()}/linkerhand_g20_{a.side}.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 = {}
@@ -99,9 +100,9 @@ def main():
         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']:
+    for filename in ['l20_moving.xml',f'l20_{a.side}.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'))
+    model = mujoco.MjModel.from_xml_path(str(a.output/f'l20_{a.side}.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)]

scripts/track_red_box_foundationpose.py

@@ -0,0 +1,76 @@
+"""Offline RGB-D red-box tracking, with explicit partial-view and quality flags."""
+import os
+os.environ.setdefault('OMP_NUM_THREADS','4')
+import sys,json,time,argparse,logging
+from pathlib import Path
+import cv2,numpy as np
+ROOT=Path(__file__).resolve().parents[1]
+sys.path.insert(0,str(ROOT/'third_party/FoundationPose'))
+from estimater import FoundationPose,ScorePredictor,PoseRefinePredictor
+from Utils import nvdiffrast_render,make_mesh_tensors
+import torch,trimesh,nvdiffrast.torch as dr
+from scipy.spatial.transform import Rotation
+
+def redmask(b,z):
+ h=cv2.cvtColor(b,cv2.COLOR_BGR2HSV)
+ m=(((h[:,:,0]<10)|(h[:,:,0]>170))&(h[:,:,1]>115)&(h[:,:,2]>65)&(z>.1)&(z<.85)).astype('uint8')
+ m[:150]=0
+ n,l,st,c=cv2.connectedComponentsWithStats(m,8)
+ if n<2:return m*0
+ j=1+np.argmax(st[1:,4]);return (l==j).astype('uint8')
+
+def main():
+ a=argparse.ArgumentParser();a.add_argument('--end',type=int,default=893);a.add_argument('--init',type=int,default=30);a.add_argument('--output',default='output/foundationpose_red_20260916');args=a.parse_args()
+ out=ROOT/args.output;out.mkdir(parents=True,exist_ok=True);(out/'snapshots').mkdir(exist_ok=True)
+ logging.basicConfig(level=logging.WARNING)
+ src=ROOT/'docs/20260916_104026';meta=json.loads((src/'intrinsics.json').read_text());K=np.array([[meta['fx'],0,meta['cx']],[0,meta['fy'],meta['cy']],[0,0,1.]])
+ cap=cv2.VideoCapture(str(src/'color.mp4'));frames=[]
+ while True:
+  ok,b=cap.read()
+  if not ok:break
+  frames.append(b)
+ cap.release();N=min(len(frames),args.end);assert len(frames)==meta['frames']
+ mesh=trimesh.load(ROOT/'docs/上半.stl');mesh.visual.vertex_colors=np.tile([200,35,35,255],(len(mesh.vertices),1))
+ ctx=dr.RasterizeCudaContext();est=FoundationPose(model_pts=mesh.vertices,model_normals=mesh.vertex_normals,mesh=mesh,glctx=ctx,debug=0,debug_dir=str(out/'debug'))
+ original_tensors=make_mesh_tensors(mesh);H,W=frames[0].shape[:2]
+ poses=np.full((N,4,4),np.nan);records=[None]*N;init_state=None
+ sequence=list(range(args.init,N))+list(range(args.init-1,-1,-1));start=time.time()
+ for i in sequence:
+  b=frames[i];z=cv2.imread(str(src/'depth'/f'{i:06d}.png'),-1).astype(np.float32)*meta['depth_scale_m'];z[(z<.1)|(z>.85)]=0;m=redmask(b,z);yy,xx=np.nonzero(m)
+  row={'frame':i,'red_pixels':len(xx),'status':'not_observable','accepted':False};pic=b.copy()
+  # After the red box is put aside it is severely cropped; do not invent its full pose.
+  observable=i<=335  # Manually reviewed red manipulation interval for this recording.
+  if i==args.init-1:est.pose_last=init_state.clone()
+  if observable:
+   rgb=cv2.cvtColor(b,cv2.COLOR_BGR2RGB)
+   if i==args.init:
+    cv2.imwrite(str(out/'initial_mask.png'),m*255);cv2.imwrite(str(out/'initial_frame.png'),b)
+    pose=est.register(K=K,rgb=rgb,depth=z,ob_mask=m,iteration=5);init_state=est.pose_last.clone()
+   else:pose=est.track_one(rgb=rgb,depth=z,K=K,iteration=2)
+   poses[i]=pose
+   with torch.inference_mode():
+    _,rd,_=nvdiffrast_render(K=K,H=H,W=W,ob_in_cams=torch.as_tensor(pose[None],device='cuda'),glctx=ctx,mesh_tensors=original_tensors)
+   rd=rd[0].cpu().numpy();render=rd>0;overlap=render&(m>0)&(z>0)
+   coverage=float((render&(m>0)).sum()/max(1,m.sum()));err=float(np.median(np.abs(rd[overlap]-z[overlap]))) if overlap.any() else None
+   partial_left=bool(i>=315 or (m[:,:3]>0).any())
+   accepted=coverage>.65 and err is not None and err<.02 and not partial_left
+   row.update(status='partial_view' if partial_left else ('tracked' if accepted else 'low_confidence'),accepted=accepted,red_coverage=coverage,depth_median_error_m=err)
+   # Show all predicted surfaces, including those behind the hand, for honest overlay QA.
+   pic[render]=(pic[render]*.65+np.array([0,220,0])*.35).astype('uint8')
+   contours,_=cv2.findContours(render.astype('uint8'),cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE);cv2.drawContours(pic,contours,-1,(0,255,0),1)
+  records[i]=row
+  cv2.putText(pic,f'{i:04d} {row["status"]}',(15,30),0,.7,(0,255,255),2)
+  if i%30==0 or i in [args.init,89,119,149,179,209,239,269,299,329]:cv2.imwrite(str(out/'snapshots'/f'{i:06d}.jpg'),pic)
+  if i%30==0:print(json.dumps({**row,'elapsed_s':round(time.time()-start,1)}),flush=True);np.save(out/'poses_checkpoint.npy',poses)
+ np.savez_compressed(out/'poses.npz',T_camera_from_object=poses,time_s=(np.array(meta['timestamps_ms'][:N])-meta['timestamps_ms'][0])/1000,accepted=np.array([r['accepted'] for r in records]),frame_index=np.arange(N),K=K)
+ (out/'frame_metrics.json').write_text(json.dumps(records,indent=2))
+ # Export original STL origin, and its geometric bounding-box center, separately.
+ rows=[]
+ for i,T in enumerate(poses):
+  q=Rotation.from_matrix(T[:3,:3]).as_quat() if np.isfinite(T).all() else np.full(4,np.nan)
+  center=T[:3,:3]@mesh.bounds.mean(0)+T[:3,3]
+  rows.append([i,(meta['timestamps_ms'][i]-meta['timestamps_ms'][0])/1000,*T[:3,3],*q,*center,int(records[i]['accepted'])])
+ np.savetxt(out/'poses.csv',rows,delimiter=',',header='frame,time_s,origin_x_m,origin_y_m,origin_z_m,qx,qy,qz,qw,center_x_m,center_y_m,center_z_m,accepted',comments='')
+ summary={'frames':N,'estimated':int(np.isfinite(poses).all((1,2)).sum()),'accepted_by_heuristics':sum(r['accepted'] for r in records),'init_frame':args.init,'offline_backward_frames':args.init,'coordinate':'original STL object coordinates to fixed OpenCV camera: x right, y down, z forward; meters assumed and checked by depth residual','manual_reviewed_tracking_interval_inclusive':[0,335],'mesh':'docs/上半.stl','mesh_extents_m':mesh.extents.tolist(),'boundary':'Predicted poses, not ground truth. Color/depth quality gates are heuristics; no contact or dynamics validation. Unobservable frames are NaN, not held.','elapsed_s':time.time()-start}
+ (out/'summary.json').write_text(json.dumps(summary,indent=2));print(json.dumps(summary),flush=True)
+if __name__=='__main__':main()

scripts/track_yesterday_boxes.py

@@ -0,0 +1,52 @@
+"""FoundationPose RGB-D tracking for both CAD parts, preserving camera/world transforms."""
+import os
+os.environ.setdefault('OMP_NUM_THREADS','4')
+import sys,json,time,subprocess
+from pathlib import Path
+ROOT=Path(__file__).resolve().parents[1];sys.path.insert(0,str(ROOT/'third_party/FoundationPose'))
+import cv2,numpy as np,torch,trimesh,nvdiffrast.torch as dr
+from estimater import FoundationPose,ScorePredictor,PoseRefinePredictor
+from Utils import nvdiffrast_render,make_mesh_tensors
+OUT=ROOT/'output/foundationpose_spider_20260915';SRC=ROOT/'docs/20260915_171525';m=json.loads((SRC/'intrinsics.json').read_text());N=m['frames'];K=np.array([[m['fx'],0,m['cx']],[0,m['fy'],m['cy']],[0,0,1.]])
+cam=np.load(ROOT/'output/20260915_171525_dynhamr/rgbd_camera.npz');c=cv2.VideoCapture(str(SRC/'color.mp4'));frames=[]
+while True:
+ ok,b=c.read()
+ if not ok:break
+ frames.append(b)
+assert len(frames)==N
+ctx=dr.RasterizeCudaContext();score=ScorePredictor();refine=PoseRefinePredictor();arrays={'K':K,'time':np.array(m['timestamps_ms'])/1000-m['timestamps_ms'][0]/1000,'c2w':cam['c2w'],'camera_valid':cam['valid']};meshes={};allmetrics={}
+for name,file,init,color in [('upper','上半.stl',0,[210,35,35,255]),('lower','下半.stl',80,[35,65,210,255])]:
+ mesh=trimesh.load(ROOT/'docs'/file);mesh.visual.vertex_colors=np.tile(color,(len(mesh.vertices),1));meshes[name]=mesh
+ est=FoundationPose(model_pts=mesh.vertices,model_normals=mesh.vertex_normals,mesh=mesh,scorer=score,refiner=refine,glctx=ctx,debug=0,debug_dir=str(OUT/name/'debug'));mt=make_mesh_tensors(mesh);P=np.full((N,4,4),np.nan);valid=np.zeros(N,bool);metrics=[None]*N;start_state=None
+ for i in list(range(init,N))+list(range(init-1,-1,-1)):
+  b=frames[i];z=cv2.imread(str(SRC/'depth'/f'{i:06d}.png'),-1).astype('float32')*m['depth_scale_m'];z[(z<.1)|(z>1.)]=0
+  h=cv2.cvtColor(b,cv2.COLOR_BGR2HSV);hue=h[:,:,0];mask=((((hue<12)|(hue>170)) if name=='upper' else ((hue>95)&(hue<135)))&(h[:,:,1]>90)&(h[:,:,2]>40)&(z>.1)&(z<1.)).astype('uint8');mask[:200]=0
+  num,lab,st,_=cv2.connectedComponentsWithStats(mask,8)
+  if num>1:mask=(lab==1+np.argmax(st[1:,4])).astype('uint8')
+  if i==init-1:est.pose_last=start_state.clone()
+  rgb=cv2.cvtColor(b,cv2.COLOR_BGR2RGB)
+  if i==init:
+   cv2.imwrite(str(OUT/name/'initial_mask.png'),mask*255);p=est.register(K=K,rgb=rgb,depth=z,ob_mask=mask,iteration=5);start_state=est.pose_last.clone()
+  else:p=est.track_one(rgb=rgb,depth=z,K=K,iteration=2)
+  P[i]=p
+  with torch.inference_mode():_,depth,_=nvdiffrast_render(K=K,H=480,W=848,ob_in_cams=torch.as_tensor(p[None],device='cuda'),glctx=ctx,mesh_tensors=mt)
+  depth=depth[0].cpu().numpy();over=(depth>0)&(mask>0)&(z>0);coverage=float(over.sum()/max(1,mask.sum()));err=float(np.median(np.abs(depth[over]-z[over]))) if over.any() else 1.
+  valid[i]=mask.sum()>500 and coverage>.65 and err<.02
+  # The lower part becomes occluded during assembly; independent lower tracking is not reliable there.
+  if name=='lower' and i>=177:valid[i]=False
+  metrics[i]={'frame':i,'valid':bool(valid[i]),'visible_color_pixels':int(mask.sum()),'coverage':coverage,'depth_residual_m':err}
+  if i%60==0:print(name,metrics[i],flush=True)
+ arrays[name+'_T_camera']=P;arrays[name+'_T_world']=cam['c2w']@P;arrays[name+'_valid']=valid;allmetrics[name]=metrics
+np.savez_compressed(OUT/'foundationpose_objects.npz',**arrays);(OUT/'object_metrics.json').write_text(json.dumps(allmetrics,indent=2))
+writer=subprocess.Popen(['ffmpeg','-y','-v','error','-f','rawvideo','-pix_fmt','bgr24','-s','848x480','-r','30','-i','-','-an','-c:v','libx264','-crf','20','-pix_fmt','yuv420p',str(OUT/'foundationpose_overlay.mp4')],stdin=subprocess.PIPE)
+for i,b in enumerate(frames):
+ pic=b.copy()
+ for name,col in [('upper',[0,220,0]),('lower',[220,180,0])]:
+  with torch.inference_mode():_,d,_=nvdiffrast_render(K=K,H=480,W=848,ob_in_cams=torch.tensor(arrays[name+'_T_camera'][i:i+1],device='cuda',dtype=torch.float32),glctx=ctx,mesh_tensors=make_mesh_tensors(meshes[name]))
+  mask=d[0].cpu().numpy()>0
+  if arrays[name+'_valid'][i]:pic[mask]=(pic[mask]*.65+np.array(col)*.35).astype('uint8')
+ cv2.putText(pic,f'{i:03d} red:{arrays["upper_valid"][i]} blue:{arrays["lower_valid"][i]}',(8,25),0,.55,(0,255,255),2)
+ writer.stdin.write(pic.tobytes())
+ if i in [0,40,80,120,160,200,240,280,351]:cv2.imwrite(str(OUT/f'objects_{i:04d}.jpg'),pic)
+writer.stdin.close();assert writer.wait()==0
+print('FOUNDATIONPOSE_DONE', {n:int(arrays[n+'_valid'].sum()) for n in meshes},flush=True)

scripts/unmirror_left_rgbd.py

@@ -0,0 +1,20 @@
+"""Restore mirrored-right inference to left-hand geometry in the original world."""
+from pathlib import Path
+import numpy as np
+from scipy.spatial.transform import Rotation
+ROOT=Path(__file__).resolve().parents[1];BASE=ROOT/'output/20260915_171525'
+src=BASE/'handflow_left_mirrored';out=BASE/'handflow_left';out.mkdir(exist_ok=True)
+M=np.diag([-1.,1.,1.]);M4=np.diag([-1.,1.,1.,1.])
+h=dict(np.load(src/'handflow_results.npz'));j=dict(np.load(src/'human_joints.npz'))
+for k in ['verts_cam','verts_world','trans']:h[k]=h[k]@M
+h['c2w']=M4@h['c2w']@M4
+h['pose']=h['pose'].copy();h['pose'][:,:3]=Rotation.from_matrix(M@Rotation.from_rotvec(h['pose'][:,:3]).as_matrix()@M).as_rotvec()
+h['intrinsics']=h['intrinsics'].copy();h['intrinsics'][2]=848-1-h['intrinsics'][2]
+h['side']='left';h['source']=str(ROOT/'docs/20260915_171525/color.mp4')
+h['convention_note']='Canonical right prediction from mirrored video, geometry reflected into actual left camera/world; body-pose fields remain canonical and are not native MANO-left parameters.'
+for k in ['joints','joints_raw','joints_world_raw','wrist_world','wrist_world_raw']:
+    if k in j:j[k]=j[k]@M
+j['root_orient']=Rotation.from_matrix(M@Rotation.from_rotvec(j['root_orient']).as_matrix()@M).as_rotvec()
+j['source']=str(out/'handflow_results.npz');j['side']='left'
+np.savez_compressed(out/'handflow_results.npz',**h);np.savez_compressed(out/'human_joints.npz',**j)
+print('Unmirrored left geometry',len(j['joints']))

scripts/verify_fix_interpolation.py

@@ -0,0 +1,45 @@
+"""Check and project all 400 Hz interpolated hand/object states, keeping objects fixed."""
+import os
+os.environ['OPENBLAS_NUM_THREADS']='1';os.environ['OMP_NUM_THREADS']='1'
+from pathlib import Path
+import json,sys,numpy as np,mujoco
+from scipy.optimize import minimize
+import l20_model_source as source
+R=Path(__file__).resolve().parents[1];O=Path(os.environ.get('HF_FIX_OUT',R/'output/collision_fix_20260915'));T=O/'datasets/processed/current/l20/bimanual/boxes';OLD=Path(os.environ.get('HF_FIX_OLD',R/'output/foundationpose_spider_20260915'));source.REPO_ROOT=R/'third_party/l20_assets';m=mujoco.MjModel.from_xml_path(str(T/'scene_act.xml'));d=mujoco.MjData(m);video_mode='--video' in sys.argv;clearance=.0025 if '--buffer' in sys.argv else 0.;r=np.load(O/'reference_video_rate.npz' if video_mode else T/'0/trajectory_kinematic_act.npz');arrays={k:r[k] for k in r.files};q=arrays['qpos'].copy();original=q.copy();meta={}
+for side in ['right','left']:
+ k=source.HandKinematics(OLD/f'model_{side}/l20_{side}.xml',side);wa=np.array([m.jnt_qposadr[m.joint(side+'_hand_'+s).id] for s in ['pos_x','pos_y','pos_z','rot_x','rot_y','rot_z']]);ja=np.array([m.jnt_qposadr[m.joint(side+'_'+n).id] for n in k.joint_names]);B=np.zeros((m.nv,22));B[wa,:6]=np.eye(6);B[ja,6:]=k.expansion;meta[side]=(k,wa,ja,B)
+def contacts(side=None,B=None):
+ ds=[];rows=[]
+ for c in d.contact:
+  g0,g1=map(int,c.geom)
+  if {int(m.geom_contype[g0]),int(m.geom_contype[g1])} not in ([{1,2},{1,4}] if os.environ.get('HF_TABLE','0')=='1' else [{1,2}]):continue
+  hg=g0 if m.geom_contype[g0]==1 else g1
+  if side is not None and not m.geom(hg).name.startswith(side+'_'):continue
+  ds.append(float(c.dist))
+  if B is not None:
+   J=np.zeros((3,m.nv));mujoco.mj_jac(m,d,J,None,c.pos,int(m.geom_bodyid[hg]));rows.append((c.frame[:3]*(1 if hg==g1 else -1))@J@B)
+ return np.asarray(rows).reshape(-1,22),np.asarray(ds)
+pre=[];post=[];changed=[]
+for f in range(len(q)):
+ d.qpos[:]=q[f];mujoco.mj_fwdPosition(m,d);_,ds=contacts();pen=max(0,clearance-ds.min()) if len(ds) else 0;pre.append(pen)
+ if pen>.00005:
+  for side,(k,wa,ja,B) in meta.items():
+   for it in range(15):
+    A,ds=contacts(side,B)
+    if not len(ds) or ds.min()>clearance-.00002:break
+    x=np.r_[d.qpos[wa],d.qpos[ja][k.independent_indices]];scale=np.r_[[.01]*3,[.1]*3,[.1]*16];lo=np.maximum(-scale,np.r_[[-np.inf]*6,k.lower]-x);hi=np.minimum(scale,np.r_[[np.inf]*6,k.upper]-x);H=np.diag(1/scale**2)
+    fit=minimize(lambda z:.5*z@H@z,np.zeros(22),jac=lambda z:H@z,bounds=list(zip(lo,hi)),constraints=[dict(type='ineq',fun=lambda z:A@z+ds-clearance-.0003,jac=lambda z:A)],method='SLSQP',options={'ftol':1e-9,'maxiter':80})
+    old=d.qpos.copy();best=None
+    for alpha in [1.,.5,.25]:
+     y=x+alpha*fit.x;d.qpos[:]=old;d.qpos[wa]=y[:6];d.qpos[ja]=k.expand(y[6:]);mujoco.mj_fwdPosition(m,d);_,test=contacts(side);p=max(0,clearance-test.min()) if len(test) else 0
+     if best is None or p<best[0]:best=(p,d.qpos.copy())
+    d.qpos[:]=best[1];mujoco.mj_fwdPosition(m,d)
+  q[f]=d.qpos.copy();changed.append(f)
+ _,ds=contacts();post.append(max(0,-ds.min()) if len(ds) else 0)
+ if f%800==0:print('interpolation',f,'max before/after mm',max(pre)*1000,max(post)*1000,flush=True)
+arrays['qpos']=q;arrays['qvel']=np.gradient(q,1/30 if video_mode else .0025,axis=0);arrays['qvel'][0]=0;arrays['ctrl']=q[:,m.jnt_qposadr[m.actuator_trnid[:,0]]]
+np.savez_compressed(O/('video_before_projection.npz' if video_mode else 'interpolation_before_projection.npz'),qpos=original,time=arrays['time']);np.savez_compressed(O/'reference_video_rate.npz' if video_mode else T/'0/trajectory_kinematic_act.npz',**arrays)
+report={'requested_safety_clearance_mm':clearance*1000,'before_metric':'clearance shortfall' if clearance else 'penetration','steps':len(q),'max_penetration_before_mm':float(max(pre)*1000),'max_penetration_after_mm':float(max(post)*1000),'projected_steps':len(changed),'all_finite':bool(np.isfinite(q).all()),'object_coordinates_unchanged':bool(np.array_equal(q[:,-12:],original[:,-12:])),'max_additional_wrist_translation_mm':float(max(np.linalg.norm(q[:,:3]-original[:,:3],axis=1).max(),np.linalg.norm(q[:,27:30]-original[:,27:30],axis=1).max())*1000),'clearance_gate_0_5mm':bool(max(post)<.0005)}
+(O/('video_rate_validation.json' if video_mode else 'interpolation_validation.json')).write_text(json.dumps(report,indent=2));print(json.dumps(report,indent=2),flush=True)
+
+if not report['clearance_gate_0_5mm']:raise RuntimeError('Collision projection failed; inspect saved validation before running physics')

scripts/verify_foundationpose_setup.py

@@ -0,0 +1,32 @@
+"""Exercise compiled extensions, GPU rendering, official weights and red-box initialization."""
+import sys,json
+from pathlib import Path
+ROOT=Path(__file__).resolve().parents[1]
+sys.path.insert(0,str(ROOT/'third_party/FoundationPose'))
+import numpy as np
+import torch,trimesh
+import nvdiffrast.torch as dr
+from estimater import FoundationPose,ScorePredictor,PoseRefinePredictor
+import Utils
+assert torch.cuda.is_available()
+assert Utils.mycpp is not None and hasattr(Utils.mycpp,'cluster_poses')
+ctx=dr.RasterizeCudaContext()
+pos=torch.tensor([[[-.5,-.5,0,1],[.5,-.5,0,1],[0,.5,0,1]]],dtype=torch.float32,device='cuda')
+tri=torch.tensor([[0,1,2]],dtype=torch.int32,device='cuda')
+rast,_=dr.rasterize(ctx,pos,tri,resolution=[64,64])
+assert torch.isfinite(rast).all() and (rast[...,3]>0).sum()>0
+score=ScorePredictor();refine=PoseRefinePredictor()
+outputs={}
+with torch.inference_mode():
+ for name,predictor in [('score',score),('refine',refine)]:
+  h,w=predictor.cfg.input_resize
+  a=torch.zeros((1,predictor.cfg.c_in,h,w),device='cuda')
+  out=predictor.model(a,a,L=1) if name=='score' else predictor.model(a,a)
+  assert all(torch.isfinite(v).all() for v in out.values())
+  outputs[name]={k:list(v.shape) for k,v in out.items()}
+mesh=trimesh.load(ROOT/'docs/上半.stl')
+est=FoundationPose(model_pts=mesh.vertices,model_normals=mesh.vertex_normals,mesh=mesh,scorer=score,refiner=refine,glctx=ctx,debug=0,debug_dir=str(ROOT/'output/foundationpose_setup/smoke_debug'))
+torch.cuda.synchronize()
+report={'status':'passed','gpu':torch.cuda.get_device_name(),'torch':torch.__version__,'cuda':torch.version.cuda,'mycpp':Utils.mycpp.__file__,'rasterized_pixels':int((rast[...,3]>0).sum()),'network_outputs':outputs,'red_box_rotation_hypotheses':list(est.rot_grid.shape),'scope':'GPU rendering, network forward passes and STL estimator initialization; no recorded-video pose accuracy validation','optional_bundlesdf_mycuda':'not built; not required for model-based FoundationPose'}
+(ROOT/'output/foundationpose_setup/runtime_verification.json').write_text(json.dumps(report,indent=2))
+print(json.dumps(report,indent=2))

scripts/verify_rgbd_bimanual.py

@@ -0,0 +1,69 @@
+"""Independently check both URDF constraints and export common-world trajectories."""
+from pathlib import Path
+import json,xml.etree.ElementTree as E,argparse
+import cv2,mujoco,numpy as np
+from scipy.spatial.transform import Rotation
+ROOT=Path(__file__).resolve().parents[1]
+parser=argparse.ArgumentParser();parser.add_argument('--base',type=Path,default=ROOT/'output/20260915_171525');args=parser.parse_args()
+BASE=args.base.resolve();OUT=BASE/'replay'
+motion=np.load(OUT/'motion.npz');q=motion['qpos'];model=mujoco.MjModel.from_xml_path(str(OUT/'scene.xml'));data=mujoco.MjData(model)
+bundle=dict(time=motion['time'],fps=motion['fps'],camera_world_from_cv=motion['camera_world_from_cv'])
+reports={};samples={side:[] for side in ['right','left']}
+for side in ['right','left']:
+    prefix='' if side=='right' else 'left_';hand=np.load(BASE/f'l20_{side}_stable/motion.npz')
+    names=hand['joint_names'].tolist();addrs=[model.jnt_qposadr[model.joint(prefix+n).id] for n in names]
+    angles=q[:,addrs];assert np.allclose(angles,hand['qpos'],atol=1e-12)
+    root=E.parse(ROOT/f'third_party/l20_assets/L20/{side.upper()}/linkerhand_g20_{side}.urdf').getroot()
+    violation=0.;mimic=0.;fk=0.;positions=[];quats=[]
+    Rshared=np.load(BASE/'l20_right_stable/motion.npz')['scene_rotation'];tshared=np.load(BASE/'l20_right_stable/motion.npz')['scene_translation']
+    for j in root.findall('joint'):
+        if j.get('type')=='fixed':continue
+        col=angles[:,names.index(j.get('name'))];lim=j.find('limit')
+        violation=max(violation,float(np.maximum(float(lim.get('lower'))-col,0).max()),float(np.maximum(col-float(lim.get('upper')),0).max()))
+        mi=j.find('mimic')
+        if mi is not None:
+            mimic=max(mimic,float(np.abs(col-float(mi.get('multiplier','1'))*angles[:,names.index(mi.get('joint'))]-float(mi.get('offset','0'))).max()))
+    for i,row in enumerate(q):
+        data.qpos[:]=row;mujoco.mj_forward(model,data)
+        pos=data.xpos[model.body(prefix+'hand_base_link').id].copy();quat=data.xquat[model.body(prefix+'hand_base_link').id].copy()
+        positions.append(pos);quats.append(quat)
+        localR=Rotation.from_quat(hand['wrist_quat_wxyz'][i,[1,2,3,0]]).as_matrix()
+        expectedR=Rshared@hand['scene_rotation'].T@localR
+        expectedp=Rshared@hand['scene_rotation'].T@(hand['wrist_pos'][i]-hand['scene_translation'])+tshared
+        expected=hand['actual'][i]@expectedR.T+expectedp
+        actual=np.array([data.site_xpos[model.site(prefix+f'landmark_{k:02d}').id] for k in range(21)])
+        fk=max(fk,float(np.abs(actual-expected).max()))
+    positions=np.asarray(positions);quats=np.asarray(quats)
+    for i in range(1,len(quats)):
+        if quats[i]@quats[i-1]<0:quats[i]*=-1
+    bundle.update({side+'_joint_names':np.asarray(names),side+'_joint_position':angles,
+                   side+'_wrist_position':positions,side+'_wrist_quaternion_wxyz':quats,
+                   side+'_valid':hand['detection_valid']})
+    np.savetxt(OUT/f'{side}_hand_trajectory.csv',np.c_[motion['time'],positions,quats,angles],delimiter=',',
+               header=','.join(['time_s','wrist_x','wrist_y','wrist_z','qw','qx','qy','qz']+names),comments='')
+    assert violation<1e-8 and mimic<1e-10 and fk<1e-5
+    reports[side]=dict(frames=len(q),valid_frames=int(hand['detection_valid'].sum()),urdf_limit_violation_rad=violation,mimic_error_rad=mimic,independent_world_fk_max_error_m=fk)
+for name in ['upper','lower']:
+    addr=model.jnt_qposadr[model.joint(name+'_free').id]
+    bundle[name+'_position']=q[:,addr:addr+3];bundle[name+'_quaternion_wxyz']=q[:,addr+3:addr+7]
+    fit=np.load(BASE/'object_poses.npz')
+    observed=np.zeros(len(q),dtype=bool)
+    observed[fit[name+'_keyframes'][fit[name+'_keyframe_valid']]]=True
+    bundle[name+'_accepted_fit_keyframe']=observed
+    bundle[name+'_interpolated_or_held']=~observed
+    for suffix in ['assembly_assumed','occluded_transition_assumed']:
+        key=name+'_'+suffix
+        if key in fit:bundle[key]=fit[key]
+bundle['world_convention']='Common right-hand replay scene transform applied to first-camera RGBD odometry world; display axes, gravity not measured.'
+bundle['valid_semantics']='Per-hand source validity; see human_joints source metadata. Object poses are partial-surface estimates; not contact labels.'
+assert np.isfinite(q).all() and np.all(np.diff(motion['time'])>0)
+np.savez_compressed(OUT/'demonstrations_bimanual.npz',**bundle)
+cap=cv2.VideoCapture(str(OUT/'original_vs_l20_bimanual.mp4'));n=0
+while True:
+    ok,im=cap.read()
+    if not ok:break
+    assert im.shape==(360,1280,3);n+=1
+cap.release();assert n==len(q)
+reports.update(video_decoded_frames=n,time_last_s=float(motion['time'][-1]),finite=True,
+               scope='Dual-hand kinematic retargeting with separate RGBD CAD pose estimates; no contact or dynamics success validation.')
+(OUT/'validation.json').write_text(json.dumps(reports,indent=2));print(json.dumps(reports,indent=2))