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>
95 lines
6.3 KiB
Python
95 lines
6.3 KiB
Python
"""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)
|