Files
a7l20-hug-sim/scene/build_real_assembly.py
T

274 lines
14 KiB
Python

"""Build an OFFLINE inspection model from the downloaded controller arm CAD.
No hardware interfaces. This is a kinematic assembly viewer, not a certified
dynamic/contact model or a real-robot motion plan. The legacy scene is untouched.
"""
from __future__ import annotations
import argparse
import hashlib
import json
from pathlib import Path
import xml.etree.ElementTree as ET
import mujoco
import numpy as np
from build_scene import ACTIVE_JOINTS, MIMIC, prepare_hand, quat_wxyz_from_R, look_at_quat
ROOT = Path(__file__).resolve().parents[1]
ARM_URDF = ROOT / "assets/controller_model/lingzuv2_description.urdf"
MOUNT_FILE = ROOT / "config/left_hand_mount.json"
DEFAULT_SNAPSHOT = ROOT / "data/left_cup_current/arm_snapshot_20260924T084830651325Z.json"
OUT_XML = ROOT / "scene/real_assembly.xml"
OUT_META = ROOT / "scene/real_assembly.json"
TIP_OFFSET = np.array([-.02325, 0., -.052])
def load_arm_spec():
"""Resolve package URIs in memory; never alter the downloaded original."""
root = ET.parse(ARM_URDF).getroot()
for mesh in root.findall(".//mesh"):
original = mesh.attrib["filename"]
filename = ARM_URDF.parent / "meshes" / Path(original).name
if not filename.is_file():
raise FileNotFoundError(filename)
mesh.set("filename", str(filename))
settings = ET.SubElement(root, "mujoco")
ET.SubElement(settings, "compiler", fusestatic="false", discardvisual="false", strippath="false")
spec = mujoco.MjSpec.from_string(ET.tostring(root, encoding="unicode"))
spec.compiler.fusestatic = False
return spec
def snapshot_angles(snapshot, side):
joint_state = snapshot["states"][side]["joint_state"]
names = [name.upper() for name in joint_state["name"]]
values = np.asarray(joint_state["position"], dtype=float)
letter = "L" if side == "left" else "R"
expected = [f"{letter}{i}_JOINT" for i in range(1, 8)]
if len(names) != len(set(names)) or len(names) != len(values):
raise ValueError("Duplicate/mismatched snapshot joint names")
q = np.array([values[names.index(name)] for name in expected])
if not np.isfinite(q).all():
raise ValueError("Nonfinite snapshot joint positions")
return expected, q
def validate_mount(mount):
rotation = np.asarray(mount["rotation_Arm_Tip_to_hand"], dtype=float)
if rotation.shape != (3, 3) or not np.allclose(rotation.T @ rotation, np.eye(3), atol=1e-9):
raise ValueError("Invalid hand mounting rotation")
if not np.isclose(np.linalg.det(rotation), 1):
raise ValueError("Mounting rotation must be right-handed")
translation = np.asarray(mount["translation_L7_to_hand_m"], dtype=float)
tip_to_hand = np.asarray(mount["translation_Arm_Tip_to_hand_m"], dtype=float)
if not np.allclose(translation, TIP_OFFSET + tip_to_hand, atol=1e-9):
raise ValueError("L7/Arm_Tip mounting translations disagree")
if not np.allclose(rotation[:, 2], [0, 0, -1], atol=1e-9):
raise ValueError("Hand and adapter mounting faces must remain parallel")
return rotation, translation
def add_table_reference(spec):
"""Optional visible patch, not a guessed finite collision-safe table volume."""
path = ROOT / "data/table_edge_check_20260924_01/start_edge_audit.json"
if not path.exists():
return None
evidence = json.loads(path.read_text())["edge_evidence"]
normal = np.array(evidence["top_normal_base"], dtype=float)
normal /= np.linalg.norm(normal)
inward = np.array(evidence["inward_normal_base"], dtype=float)
inward -= normal * (normal @ inward)
inward /= np.linalg.norm(inward)
lateral = np.cross(normal, inward)
edge = np.array(evidence["edge_point_base_m"], dtype=float)
point = np.array(evidence["top_point_base_m"], dtype=float)
edge -= normal * ((edge - point) @ normal)
rotation = np.column_stack([inward, lateral, normal])
# 28cm display depth only; the real far/side edges and thickness are unknown.
spec.worldbody.add_geom(name="measured_table_reference_NOT_collision",
type=mujoco.mjtGeom.mjGEOM_BOX, pos=edge + .14 * inward - .001 * normal,
quat=quat_wxyz_from_R(rotation), size=[.14, .21, .001],
rgba=[.36, .61, .66, .25], group=3, contype=0, conaffinity=0)
return {"source": str(path), "normal": normal.tolist(), "point_m": point.tolist(),
"role": "visual-only plane patch; NOT actual table extent/thickness or collision certification",
"geom_group": 3, "hidden_by_default": True}
def build(snapshot_path=DEFAULT_SNAPSHOT):
mount = json.loads(MOUNT_FILE.read_text())
rotation, translation = validate_mount(mount)
snapshot_path = Path(snapshot_path).resolve()
snapshot = json.loads(snapshot_path.read_text())
spec = load_arm_spec()
spec.modelname = "controller_arm_CAD_adapter_left_hand_OFFLINE"
spec.visual.global_.offwidth = 1600
spec.visual.global_.offheight = 1000
spec.visual.headlight.ambient = [.4, .4, .4]
spec.visual.headlight.diffuse = [.7, .7, .7]
# Fixed root, base_link == world. No old scene's +1.2m display offset.
spec.worldbody.add_light(name="key", pos=[1.5, -.5, 1.2], dir=[-1, .3, -1],
diffuse=[.7, .7, .7])
spec.worldbody.add_light(name="fill", pos=[-.3, 1, .8], dir=[.4, -1, -.8],
diffuse=[.5, .5, .5])
for geom in spec.geoms:
if geom.contype or geom.conaffinity:
geom.group = 2 # Hide collision convex hull rendering in the viewer.
else:
geom.group = 0
geom.rgba = [.65, .68, .72, 1]
for side, letter in (("left", "L"), ("right", "R")):
spec.body(f"{letter}7_Link").add_site(name=f"{side}_Arm_Tip", pos=TIP_OFFSET,
size=[.004, .004, .004], rgba=[.9, .5, .1, 1], group=4)
hand, _ = prepare_hand("left", kp=2., kv=.08)
for mesh in hand.meshes:
if mesh.file and not Path(mesh.file).is_absolute():
mesh.file = str((ROOT / "assets/L20/LEFT") / mesh.file)
hand.compiler.meshdir = ""
for body in hand.bodies:
for geom in body.geoms:
if geom.contype or geom.conaffinity:
geom.group = 2
else:
geom.group = 0
geom.rgba = ([.87, .35, .62, 1] if body.name.startswith("pinky")
else [.88, .88, .84, 1])
frame = spec.body("L7_Link").add_frame(pos=translation, quat=quat_wxyz_from_R(rotation))
frame.attach_body(hand.body("hand_base_link"), "left_hand_", "")
# Six-hole face z=0 against L7 z=-52.5mm; four-hole face at outward 36mm.
# Use hand's axial orientation for the adapter: four-hole square stays aligned.
# Arm six-hole azimuth has not been physically confirmed (explicit metadata).
# OCP exports ASCII STL, while MuJoCo's STL decoder requires binary.
import trimesh
adapter_source = ROOT / "assets/hand_adapter/adapter_m.stl"
adapter_binary = ROOT / "assets/hand_adapter/adapter_m_binary.stl"
trimesh.load_mesh(adapter_source, process=False).export(adapter_binary, file_type="stl")
spec.add_mesh(name="physical_adapter", file=str(adapter_binary))
adapter = spec.body("L7_Link").add_body(name="left_adapter",
pos=[-.028, 0, -.0525], quat=quat_wxyz_from_R(rotation))
adapter.add_geom(name="left_adapter_visual", type=mujoco.mjtGeom.mjGEOM_MESH,
meshname="physical_adapter", rgba=[.96, .62, .16, 1],
contype=0, conaffinity=0, density=0, group=0)
adapter.add_site(name="adapter_hand_face", pos=[0, 0, .036], size=[.002] * 3,
rgba=[.1, .8, .3, 1], group=4)
for name, position, target in (
("overview", [1.4, -1.3, .65], [.1, 0, -.25]),
("reference", [1.1, 1.2, .4], [.16, .07, -.28]),
("left_hand_close", [.65, .9, .06], [.40, .215, -.275]),
):
spec.worldbody.add_camera(name=name, pos=position,
quat=look_at_quat(position, target), fovy=42.)
table = add_table_reference(spec)
model = spec.compile()
poses = {name: np.zeros(model.nq) for name in ("snapshot", "zero", "elbow90")}
info = {"schema_version": 1, "hardware_connected": False, "execution_authorized": False,
"purpose": "offline kinematic assembly inspection, NOT validated dynamic grasping",
"world_frame": "controller base_link; no display height offset",
"source_arm_urdf": str(ARM_URDF), "mount_file": str(MOUNT_FILE), "mount": mount,
"snapshot_file": str(snapshot_path), "snapshot_received_at_utc": snapshot.get("received_at_utc"),
"snapshot_is_live": False, "finger_pose": "nominal URDF zero, NOT measured hardware state",
"arms": {}, "hands": {"left": {"base_body": "left_hand_hand_base_link", "prefix": "left_hand_",
"active_joints": ["left_hand_" + name for name in ACTIVE_JOINTS],
"mimic": MIMIC, "pinky_color": "pink"}},
"right_hand": "not modelled: current right-hand mounting not confirmed",
"adapter": {"face_distance_m": .036, "mesh": "assets/hand_adapter/adapter_m.stl",
"axial_placement": "CAD mating faces touching",
"azimuth": "four-hole face aligned with hand; six-hole registration to arm remains unverified",
"dynamic_mass_inertia": "not identified; viewer must not step dynamics"},
"table_reference": table, "snapshot_limit_warnings": [], "keyframes": list(poses),
"safety": "No hardware API. No stepping dynamics. Existing real-motion gates are unchanged."}
for side in ("left", "right"):
names, q = snapshot_angles(snapshot, side)
info["arms"][side] = {"joints": names, "tip_site": f"{side}_Arm_Tip"}
for name, angle in zip(names, q):
jid = model.joint(name).id
poses["snapshot"][model.jnt_qposadr[jid]] = angle
lo, hi = model.jnt_range[jid]
if model.jnt_limited[jid] and not lo <= angle <= hi:
info["snapshot_limit_warnings"].append({"joint": name, "angle_rad": float(angle),
"urdf_range_rad": [float(lo), float(hi)], "action": "preserved, NOT clamped"})
poses["elbow90"][model.jnt_qposadr[model.joint("L4_JOINT").id]] = np.pi / 2
for name, q in poses.items():
controls = []
for actuator_id in range(model.nu):
jid = model.actuator_trnid[actuator_id, 0]
controls.append(q[model.jnt_qposadr[jid]])
spec.add_key(name=name, qpos=q, ctrl=controls)
info["input_sha256"] = {str(path.relative_to(ROOT)): hashlib.sha256(path.read_bytes()).hexdigest()
for path in (ARM_URDF, MOUNT_FILE, snapshot_path)}
return spec, info
def verify_model(model, info):
data = mujoco.MjData(model)
mujoco.mj_resetDataKeyframe(model, data, model.key("snapshot").id)
mujoco.mj_forward(model, data)
snapshot = json.loads((ROOT / info["snapshot_file"]).read_text())
checks = {}
for side in ("left", "right"):
pose = snapshot["states"][side]["pose"]
expected = np.array([pose["position"][key] for key in "xyz"])
expected_quat = np.array([pose["orientation"][key] for key in "wxyz"], dtype=float)
if not np.isfinite(expected).all() or not np.isfinite(expected_quat).all():
raise ValueError(f"{side} snapshot pose contains nonfinite values")
norm = np.linalg.norm(expected_quat)
if not np.isclose(norm, 1., atol=1e-3):
raise ValueError(f"{side} snapshot quaternion is not normalized")
expected_rotation = np.empty(9)
mujoco.mju_quat2Mat(expected_rotation, expected_quat / norm)
position = data.site(f"{side}_Arm_Tip").xpos.copy()
error = float(np.linalg.norm(position - expected))
if not np.isfinite(error) or error > .0001:
raise ValueError(f"{side} controller snapshot FK mismatch: {error * 1000:.4f} mm")
actual_rotation = data.site(f"{side}_Arm_Tip").xmat.reshape(3, 3)
cosine = (np.trace(actual_rotation.T @ expected_rotation.reshape(3, 3)) - 1.) / 2.
angle_deg = float(np.degrees(np.arccos(np.clip(cosine, -1., 1.))))
if angle_deg > .01:
raise ValueError(f"{side} controller snapshot orientation mismatch: {angle_deg:.5f} deg")
checks[f"{side}_snapshot_tip_error_mm"] = error * 1000
checks[f"{side}_snapshot_tip_error_deg"] = angle_deg
mujoco.mj_resetDataKeyframe(model, data, model.key("elbow90").id)
mujoco.mj_forward(model, data)
hand = data.body("left_hand_hand_base_link")
hand_R = hand.xmat.reshape(3, 3)
index = data.body("left_hand_index_metacarpals").xpos.copy()
pinky = data.body("left_hand_pinky_metacarpals").xpos.copy()
face_error = np.linalg.norm(data.site("adapter_hand_face").xpos - hand.xpos)
if not (hand_R[2, 0] < -.7 and hand_R[2, 1] < -.7 and pinky[2] < index[2]):
raise ValueError("Mount does not match palm down / LEFT pinky lower reference")
if face_error > 1e-8:
raise ValueError("Adapter four-hole face and hand mounting plane do not touch")
checks.update({"elbow90_palm_normal_base": hand_R[:, 0].tolist(),
"elbow90_pinky_side_base": hand_R[:, 1].tolist(),
"elbow90_pinky_minus_index_height_mm": float((pinky[2] - index[2]) * 1000),
"adapter_hand_origin_gap_mm": float(face_error * 1000),
"nq": model.nq, "nbody": model.nbody, "status": "kinematic_checks_passed_not_physical_acceptance"})
return checks
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--snapshot", type=Path, default=DEFAULT_SNAPSHOT)
args = parser.parse_args()
spec, info = build(args.snapshot)
model = spec.compile()
info["checks"] = verify_model(model, info)
OUT_XML.write_text(spec.to_xml())
# Independently reload the actual on-disk scene (mesh path/export regression).
info["checks"] = verify_model(mujoco.MjModel.from_xml_path(str(OUT_XML)), info)
OUT_META.write_text(json.dumps(info, indent=2, ensure_ascii=False) + "\n")
print(OUT_XML)
print(json.dumps(info["checks"], indent=2))
for warning in info["snapshot_limit_warnings"]:
print("Snapshot limit discrepancy (displayed unchanged):", warning)
print("Offline only. Hand fingers nominal zero; adapter bolt azimuth awaits physical confirmation.")
if __name__ == "__main__":
main()