274 lines
14 KiB
Python
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()
|