liyang
ae28d55f81
Source: RGB-D -> Dyn-HaMR -> L20 retargeting -> FoundationPose -> reference repair -> SPIDER, documented in docs/PIPELINE_LATEST.md and docs/SETUP_AND_WEIGHTS.md. Adds FoundationPose and nvdiffrast upstream snapshots, requirements/pipeline_venv.txt and the FoundationPose weight manifest/downloader. Assets (Git LFS): weights/ (WiLoR detector, HandFlow denoiser, UniDepth-L), FoundationPose checkpoints, HaMeR checkpoint, Dyn-HaMR HMP model and BMC constraints, L20 URDF/meshes, the 20260915_171525 D405 recording and the two box CADs. MANO models are not redistributed (third_party/hamer/_DATA/data/mano/README.txt). Environments, caches and run outputs excluded. Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
70 lines
4.8 KiB
Python
70 lines
4.8 KiB
Python
"""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))
|