178 lines
10 KiB
Python
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()
|