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

178 lines
10 KiB
Python

"""Add a measured pedestal and desk around the preserved controller A7 assembly.
Offline visual layout only. No hardware imports or dynamic stepping.
"""
from pathlib import Path
import copy
import hashlib
import json
import xml.etree.ElementTree as ET
import numpy as np
import mujoco
from build_scene import look_at_quat
from layout_clearance import clearance
ROOT = Path(__file__).resolve().parents[1]
CONFIG = ROOT / 'config/measured_layout.json'
SOURCE = ROOT / 'scene/real_assembly.xml'
OUTPUT = ROOT / 'scene/measured_layout.xml'
def fmt(values):
return ' '.join(f'{float(x):.12g}' for x in values)
def box(parent, name, center, full_size, color):
return ET.SubElement(parent, 'geom', name=name, type='box', pos=fmt(center),
size=fmt(np.array(full_size) / 2), rgba=fmt(color), contype='0', conaffinity='0', group='0')
def build():
cfg = json.loads(CONFIG.read_text())
tree = ET.parse(SOURCE)
root = tree.getroot()
root.set('model', 'A7_preserved_assembly_measured_pedestal_desk_OFFLINE')
world = root.find('worldbody')
arm = world.find("body[@name='base_link']")
original_arm = ET.tostring(arm)
old_model = mujoco.MjModel.from_xml_path(str(SOURCE))
fresh_snapshot = None
if cfg.get('snapshot_file'):
fresh_snapshot = json.loads((ROOT / cfg['snapshot_file']).read_text())
key_id = old_model.key('snapshot').id
for side, letter in [('left','L'),('right','R')]:
state = fresh_snapshot['states'][side]['joint_state']
names = [n.upper() for n in state['name']]
values = np.asarray(state['position'], dtype=float)
if len(names) != 7 or len(set(names)) != 7 or values.shape != (7,) or not np.isfinite(values).all():
raise ValueError('Invalid received joint state')
for i in range(1,8):
name = f'{letter}{i}_JOINT'
old_model.key_qpos[key_id, old_model.joint(name).qposadr[0]] = values[names.index(name)]
root.find(".//key[@name='snapshot']").set('qpos', fmt(old_model.key_qpos[key_id]))
old_data = mujoco.MjData(old_model)
mujoco.mj_forward(old_model, old_data)
# Top of the fixed A7 shoulder crossbar, excluding the camera/bag in photos.
base_id = old_model.body('base_link').id
points = []
for gid in range(old_model.ngeom):
if old_model.geom_bodyid[gid] != base_id or old_model.geom_group[gid] != 0:
continue
mid = old_model.geom_dataid[gid]
start, count = old_model.mesh_vertadr[mid], old_model.mesh_vertnum[mid]
verts = old_model.mesh_vert[start:start+count]
points.append(verts @ old_data.geom_xmat[gid].reshape(3,3).T + old_data.geom_xpos[gid])
base_vertices = np.concatenate(points)
offset = cfg['measured']['crossbar_top_height_m'] - float(base_vertices[:,2].max())
measured_offset = offset
fit = cfg.get('display_adjustment')
display_lift = 0.
fit_before = None
if fit and fit['mode'] == 'fit_saved_pose_above_desk':
mujoco.mj_resetDataKeyframe(old_model, old_data, old_model.key(fit['pose']).id)
mujoco.mj_forward(old_model, old_data)
near_x = cfg['measured']['column_xy_m'][0]/2 + cfg['measured']['column_front_to_table_near_m']
fit_before = clearance(old_model, old_data, near_x,
near_x + cfg['estimated_visual_only']['table_depth_m'],
cfg['estimated_visual_only']['table_width_m']/2,
cfg['measured']['table_top_height_m'] - measured_offset)
display_lift = fit['clearance_m'] - fit_before['minimum_clearance_m']
offset += display_lift
crossbar_bottom = offset + float(base_vertices[:,2].min())
world.remove(arm)
wrapper = ET.SubElement(world, 'body', name='robot_floor_mount', pos=fmt([0,0,offset]))
wrapper.append(arm)
assert ET.tostring(arm) == original_arm
for item in list(world):
if item.tag in ('camera', 'light') or item.get('name') == 'measured_table_reference_NOT_collision':
world.remove(item)
ET.SubElement(world, 'light', pos='0 -2 3', dir='0 0 -1', diffuse='.8 .8 .8')
ET.SubElement(world, 'light', pos='2 2 2', dir='-1 -1 -1', diffuse='.5 .5 .5')
ET.SubElement(world, 'geom', name='floor', type='plane', size='3 3 .01', rgba='.20 .23 .26 1', contype='0', conaffinity='0')
measured, assumed = cfg['measured'], cfg['estimated_visual_only']
deck_top = assumed['deck_top_height_m']
deck_thick = assumed['deck_thickness_m']
deck_x = assumed['pedestal_center_x_m']
box(world, 'pedestal_50cm', [deck_x,0,deck_top-deck_thick/2],
[*measured['pedestal_xy_m'],deck_thick], [.55,.58,.61,1])
column_h = crossbar_bottom-deck_top
box(world, 'column_9x6cm', [0,0,deck_top+column_h/2],
[*measured['column_xy_m'],column_h], [.66,.69,.71,1])
# Narrow dark strips suggest extrusion channels without changing dimensions.
for y in [-measured['column_xy_m'][1]/3,0,measured['column_xy_m'][1]/3]:
box(world, f'column_front_groove_{y}', [measured['column_xy_m'][0]/2+.0001,y,deck_top+column_h/2], [.0002,.0025,column_h], [.25,.28,.30,1])
box(world, f'column_back_groove_{y}', [-measured['column_xy_m'][0]/2-.0001,y,deck_top+column_h/2], [.0002,.0025,column_h], [.25,.28,.30,1])
wheel_r = (deck_top-deck_thick)/2
for x in [-.195,.195]:
for y in [-.195,.195]:
ET.SubElement(world,'geom',name=f'caster_{x}_{y}',type='cylinder',
pos=fmt([x+deck_x,y,wheel_r]),size=fmt([wheel_r,.022]),quat='.707106781187 .707106781187 0 0',
rgba='.075 .08 .085 1',contype='0',conaffinity='0')
near = measured['column_xy_m'][0]/2 + measured['column_front_to_table_near_m']
depth, width = (assumed[k] for k in ['table_depth_m','table_width_m'])
top, thickness = measured['table_top_height_m'], measured['table_thickness_m']
cx = near+depth/2
box(world,'desk_top',[cx,0,top-thickness/2],[depth,width,thickness],[.91,.91,.88,1])
for x in [near+.045,near+depth-.045]:
for y in [-width/2+.045,width/2-.045]:
box(world,f'desk_leg_{x}_{y}',[x,y,(top-thickness)/2],[.04,.04,top-thickness],[.71,.73,.73,1])
cameras = {
'overview':([2.15,-2.3,1.85],[.23,0,.55]),
'reference':([1.8,1.85,1.65],[.2,0,.65]),
'side':([.55,-2.4,1.05],[.25,0,.63]),
'top':([.40,0,2.6],[.40,0,.5]),
'left_hand_close':([.95,1.1,1.12],[.4,.2,.80]),
}
for name,(pos,target) in cameras.items():
ET.SubElement(world,'camera',name=name,pos=fmt(pos),quat=('1 0 0 0' if name == 'top' else fmt(look_at_quat(pos,target))),fovy='43')
ET.indent(tree)
tree.write(OUTPUT,encoding='unicode')
model = mujoco.MjModel.from_xml_path(str(OUTPUT))
data = mujoco.MjData(model)
max_err = 0.
# Verify every preserved body and orientation at every original keyframe.
for key in range(old_model.nkey):
mujoco.mj_resetDataKeyframe(old_model,old_data,key)
mujoco.mj_resetDataKeyframe(model,data,key)
mujoco.mj_forward(old_model,old_data);mujoco.mj_forward(model,data)
for bid in range(1,old_model.nbody):
name=old_model.body(bid).name
err=np.linalg.norm(data.body(name).xpos-old_data.xpos[bid]-[0,0,offset])
max_err=max(max_err,float(err))
assert err<1e-9,(name,err)
assert np.max(abs(data.body(name).xmat-old_data.xmat[bid]))<1e-9
assert model.nq == old_model.nq
fit_after = None
if fit_before is not None:
mujoco.mj_resetDataKeyframe(model, data, model.key(fit['pose']).id)
mujoco.mj_forward(model, data)
fit_after = clearance(model, data, near, near+depth, width/2, top)
assert fit_after['minimum_clearance_m'] >= fit['clearance_m'] - 1e-8
info=json.loads((ROOT/'scene/real_assembly.json').read_text())
if fresh_snapshot is not None:
info['snapshot_file'] = cfg['snapshot_file']
info['snapshot_received_at_utc'] = fresh_snapshot['received_at_utc']
info['snapshot_is_live'] = False
info['snapshot_source'] = 'fresh read saved once; not continuous live streaming'
info['input_sha256'][cfg['snapshot_file']] = hashlib.sha256((ROOT / cfg['snapshot_file']).read_bytes()).hexdigest()
info.pop('checks', None) # Original historical FK numbers do not describe this snapshot.
info['snapshot_limit_warnings'] = []
for jid in range(old_model.njnt):
name = old_model.joint(jid).name
if not (name.startswith(('L','R')) and name.endswith('_JOINT')): continue
q = float(old_model.key_qpos[old_model.key('snapshot').id,old_model.jnt_qposadr[jid]])
if old_model.jnt_limited[jid] and not old_model.jnt_range[jid,0] <= q <= old_model.jnt_range[jid,1]:
info['snapshot_limit_warnings'].append({'joint':name,'angle_rad':q,'action':'preserved controller angle, not clamped'})
info.update({'world_frame' :'floor frame: controller base_link translated only along +Z',
'source_scene':str(SOURCE),'source_sha256':hashlib.sha256(SOURCE.read_bytes()).hexdigest(),
'layout':cfg,'measured_controller_base_height_m':measured_offset,
'display_lift_m':display_lift,'display_crossbar_top_height_m':cfg['measured']['crossbar_top_height_m']+display_lift,
'desk_clearance_before':fit_before,'desk_clearance_after':fit_after,
'controller_base_height_m':offset,'column_top_height_m':crossbar_bottom,
'column_height_m':column_h,'table_top_in_controller_base_m':top-offset,
'table_reference':None,'layout_checks':{'preserved_body_max_translation_error_m':max_err,
'all_original_keyframes_checked':old_model.nkey,'all_original_body_orientations_unchanged':True},
'purpose':'Offline visual layout. Estimated desk extents, gap and caster dimensions are not collision evidence.'})
(ROOT/'scene/measured_layout.json').write_text(json.dumps(info,ensure_ascii=False,indent=2)+'\n')
print(json.dumps({k:info[k] for k in ['controller_base_height_m','column_top_height_m','column_height_m','table_top_in_controller_base_m','layout_checks']},indent=2))
print(OUTPUT)
if __name__=='__main__':
build()