Files
a7l20-hug-sim/scripts/capture_workcell_rgbd.py

55 lines
3.2 KiB
Python

"""Render an existing kinematic workcell through a provisional D405 camera.
No hardware API or mj_step. Does not create/fake a cup observation.
"""
import argparse,json,os
from pathlib import Path
os.environ.setdefault('MUJOCO_GL','egl')
import numpy as np
import mujoco
import cv2
ROOT=Path(__file__).resolve().parents[1]
def main():
ap=argparse.ArgumentParser(description=__doc__)
ap.add_argument('--scene',type=Path,default=ROOT/'scene/measured_layout.xml')
ap.add_argument('--out',type=Path,default=ROOT/'data/cup_sim_camera_preview')
a=ap.parse_args()
cfg=json.loads((ROOT/'config/cup_workcell.json').read_text())
cal=json.loads((ROOT/cfg['calibration_source']).read_text())
layout=json.loads((ROOT/'scene/measured_layout.json').read_text())
spec=mujoco.MjSpec.from_file(str(a.scene.resolve()))
T=np.array(cal['T_world_cam_opencv'],float)
T[2,3]+=layout['controller_base_height_m']
if cfg.get('registered_camera_source'):
registered=json.loads((ROOT/cfg['registered_camera_source']).read_text())
T=np.array(registered['T_world_cam_opencv'])
R=T[:3,:3]@np.diag([1.,-1.,-1.]);quat=np.empty(4);mujoco.mju_mat2Quat(quat,R.ravel())
K=np.asarray(cal['K']);w,h=int(cal['width']),int(cal['height'])
cam=spec.worldbody.add_camera(name=cfg['camera_name'],pos=T[:3,3],quat=quat,
fovy=float(np.degrees(2*np.arctan(h/(2*K[1,1])))))
# MuJoCo supports calibrated off-center principal point via pixel intrinsics.
cam.resolution=[w,h];cam.sensor_size=[w/K[0,0],h/K[1,1]]
cam.focal_pixel=[K[0,0],K[1,1]]
cam.principal_pixel=[w/2-.5-K[0,2],h/2-.5-K[1,2]]
m=spec.compile();d=mujoco.MjData(m);mujoco.mj_resetDataKeyframe(m,d,m.key('snapshot').id);mujoco.mj_forward(m,d)
opt=mujoco.MjvOption();opt.geomgroup[2]=0;opt.geomgroup[3]=0;opt.sitegroup[4]=0
with mujoco.Renderer(m,height=h,width=w) as renderer:
renderer.update_scene(d,camera=cfg['camera_name'],scene_option=opt);rgb=renderer.render().copy()
renderer.enable_depth_rendering();renderer.update_scene(d,camera=cfg['camera_name'],scene_option=opt);depth=renderer.render().copy()
out=a.out.resolve();out.mkdir(parents=True,exist_ok=True)
cv2.imwrite(str(out/'rgb.png'),cv2.cvtColor(rgb,cv2.COLOR_RGB2BGR))
mm=np.where(np.isfinite(depth)&(depth>0)&(depth<65),np.rint(depth*1000),0).astype(np.uint16)
cv2.imwrite(str(out/'depth.png'),mm)
meta={'source':'simulated_d405','width':w,'height':h,'K':K.tolist(),'T_world_cam_opencv':T.tolist(),
'depth_unit':'mm(uint16)','calibration_source':cfg['calibration_source'],
'extrinsic_status':('table_plane_registered_provisional_XY_yaw' if cfg.get('registered_camera_source') else 'provisional_saved_calibration_plus_display_base_height'),
'scene':str(a.scene),'pose':'snapshot','cup_observation':'not inferred from this preview',
'table_clearance_m':cfg['table_clearance_m'],'hardware_connected':False}
(out/'camera.json').write_text(json.dumps(meta,indent=2))
(out/'intrinsics.txt').write_text(f'{K[0,0]} {K[1,1]} {K[0,2]} {K[1,2]}\n')
spec.to_xml() # Validate that camera can be serialized for later cup scene.
(out/'preview_scene.xml').write_text(spec.to_xml())
print(out)
if __name__=='__main__':main()