55 lines
3.2 KiB
Python
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()
|