"""Remove initial CAD/table interpenetration before unassisted physics; record the offset."""
from pathlib import Path
import numpy as np,mujoco,xml.etree.ElementTree as E,json
O=Path(__file__).resolve().parents[1]/'output/collision_fix_20260915';T=O/'datasets/processed/current/l20/bimanual/boxes';p=T/'scene_act.xml';m=mujoco.MjModel.from_xml_path(str(p));d=mujoco.MjData(m);d.qpos[:]=np.load(T/'0/trajectory_kinematic_act.npz')['qpos'][0];mujoco.mj_fwdPosition(m,d);z=[]
for g in range(m.ngeom):
 if m.geom_contype[g]!=2:continue
 mid=m.geom_dataid[g];v=m.mesh_vert[m.mesh_vertadr[mid]:m.mesh_vertadr[mid]+m.mesh_vertnum[mid]];world=v@d.geom_xmat[g].reshape(3,3).T+d.geom_xpos[g];z.append(world[:,2].min())
height=float(min(z)-.0005);tree=E.parse(p);floor=tree.getroot().find("worldbody/geom[@name='right_floor']");old=float(floor.get('pos').split()[2]);floor.set('pos',f'0 0 {height}');tree.write(p);report={'old_height_m':old,'new_height_m':height,'offset_m':height-old,'reason':'No object/table initial overlap. Simulation alignment correction, not a new measured plane; object poses unchanged.'};(O/'floor_alignment.json').write_text(json.dumps(report,indent=2));print(report)