"""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)