Files
a7l20-hug-sim/scene/build_cup_workcell.py

57 lines
5.0 KiB
Python

"""Measured RGB-D cup approximation in the preserved A7 workcell; offline."""
from pathlib import Path
import json,hashlib
import numpy as np
from scipy.optimize import least_squares
from scipy.spatial.transform import Rotation
import mujoco
ROOT=Path(__file__).resolve().parents[1]
OUT=ROOT/'data/cup_model_20260928'
def main():
cloud=np.load(OUT/'cup_points.npz');p=cloud['cup'];center=np.median(p[p[:,2]>.23,:2],axis=0)
selected=p[np.linalg.norm(p[:,:2]-center,axis=1)<.065]
side=selected[(selected[:,2]>.025)&(selected[:,2]<.205)][::3];knots=np.array([.025,.075,.11,.205])
fit=least_squares(lambda a:np.linalg.norm(side[:,:2]-a[:2],axis=1)-np.interp(side[:,2],knots,a[2:]),np.r_[center,[.032,.034,.043,.047]],bounds=(np.r_[center-.025,[.02]*4],np.r_[center+.025,[.065]*4]),loss='soft_l1',f_scale=.0015)
center=fit.x[:2];radii=fit.x[2:];height=float(np.quantile(selected[:,2],.999))
cal=json.loads((ROOT/'data/handeye_fresh_20260928_y29kSV/camera.json').read_text())
layout=json.loads((ROOT/'scene/measured_layout.json').read_text())
raw=np.array(cal['T_world_cam_opencv']);raw[2,3]+=layout['controller_base_height_m']
normal=raw[:3,:3]@cloud['plane_normal'];target=np.array([0,0,1.]);axis=np.cross(normal,target);angle=np.arctan2(np.linalg.norm(axis),normal@target)
align=Rotation.from_rotvec(axis/max(np.linalg.norm(axis),1e-12)*angle).as_matrix()
T=raw.copy();T[:3,:3]=align@raw[:3,:3]
top=.755;T[2,3]+=top-(T[:3,:3]@cloud['plane_center']+T[:3,3])[2]
base_cam=cloud['plane_center']+cloud['basis'][:,:2]@center
base=T[:3,:3]@base_cam+T[:3,3];base[2]=top
camera={'width':cal['width'],'height':cal['height'],'K':cal['K'],'T_world_cam_opencv':T.tolist(),
'status':'table-plane registered for offline visualization; XY/yaw retain provisional hand-eye',
'correction_rotation_deg':float(np.degrees(angle)),'correction_camera_z_m':float(T[2,3]-raw[2,3])}
(OUT/'registered_camera.json').write_text(json.dumps(camera,indent=2))
s=mujoco.MjSpec.from_file(str(ROOT/'scene/measured_layout.xml'))
cup=s.worldbody.add_body(name='measured_cup',pos=base)
# Axisymmetric frusta approximate the measured external profile, not internal cavity.
profile=[(0,float(radii[0])),(.025,float(radii[0])),(.075,float(radii[1])),(.11,float(radii[2])),(.205,float(radii[3])),(.220,float(radii[3])),(.235,float(radii[3]))]
for i,((z0,r0),(z1,r1)) in enumerate(zip(profile,profile[1:])):
a=np.linspace(0,2*np.pi,64,endpoint=False);vertices=np.vstack([np.c_[r0*np.cos(a),r0*np.sin(a),np.full(64,z0)],np.c_[r1*np.cos(a),r1*np.sin(a),np.full(64,z1)]])
faces=[]
for k in range(64):
j=(k+1)%64;faces.extend([[k,j,j+64],[k,j+64,k+64]])
for k in range(1,63):faces.extend([[0,k+1,k],[64,64+k,65+k]])
name=f'cup_section_{i}';s.add_mesh(name=name,uservert=vertices.ravel(),userface=np.asarray(faces).ravel())
color=[.035,.038,.04,1] if z0<.11 or z0>=.205 else [.34,.29,.25,1]
if z0>=.220:color=[.65,.68,.7,1]
cup.add_geom(name=name,type=mujoco.mjtGeom.mjGEOM_MESH,meshname=name,rgba=color,contype=0,conaffinity=0)
# Lid ring and raised white latch; hidden details are intentionally approximate.
cup.add_geom(name='cup_lid',type=mujoco.mjtGeom.mjGEOM_CYLINDER,pos=[0,0,.237],size=[float(radii[-1]),.004,0],rgba=[.55,.62,.65,.65],contype=0,conaffinity=0)
cup.add_geom(name='cup_latch',type=mujoco.mjtGeom.mjGEOM_BOX,pos=[0,0,(height+.241)/2],size=[.012,.039,max(.001,(height-.241)/2)],rgba=[.92,.91,.84,1],contype=0,conaffinity=0)
model=s.compile();(ROOT/'scene/cup_workcell.xml').write_text(s.to_xml())
report={'capture':'data/cup_model_20260928','height_m':height,'profile_z_radius_m':profile,'bottom_diameter_m':float(2*radii[0]),'upper_diameter_m':float(2*radii[-1]),'surface_fit_median_residual_m':float(np.median(abs(fit.fun))),
'position_floor_m':base.tolist(),'camera_registration':camera,'handeye_independently_validated':False,
'assumptions':['axisymmetric cup standing normal to fitted support plane','unseen rear surface rotational completion','lid/latch simplified','no measured mass/friction; fixed kinematic object'],
'dimension_uncertainty':'not independently quantified; residual is NOT absolute accuracy','table_margin_m':.005,'physical_execution_authorized':False}
(OUT/'cup_model.json').write_text(json.dumps(report,indent=2))
meta=layout.copy();meta['cup']=report;(ROOT/'scene/cup_workcell.json').write_text(json.dumps(meta,indent=2))
cfg=json.loads((ROOT/'config/cup_workcell.json').read_text());cfg.update({'registered_camera_source':'data/cup_model_20260928/registered_camera.json','real_camera_status':'captured_user_saved_full_cup','cup_model_status':'rgbd_axisymmetric_approximation_built','cup_model':'scene/cup_workcell.xml'});(ROOT/'config/cup_workcell.json').write_text(json.dumps(cfg,indent=2))
print(json.dumps({k:report[k] for k in ['height_m','bottom_diameter_m','upper_diameter_m','position_floor_m','surface_fit_median_residual_m']},indent=2))
if __name__=='__main__':main()