File size: 5,765 Bytes
24fe873 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 | """Compare the training robot against the reference assembly.
The training model and the reference assembly are two different things and
drift silently. This script makes the difference visible and exits non-zero
when they disagree, so the gap cannot be missed again.
uv run python training/scripts/check_model_vs_reference.py
What it checks, and what it cannot:
* Body, joint, geom and mass counts, and the joint name sets.
* Per-joint anchor and axis, in world frame at the zero pose, matched by joint
name where the two models share a name.
* Whether the training model is a connected free body (one root with a
freejoint) or a puppet pinned to the world.
It does NOT check that the two articulations are the same mechanism, because
they are not. The reference assembly is a world-pinned puppet: 32 of its 121
bodies attach to the world, so each limb hangs from the world rather than from
the torso. See the report in abstraction.md.
"""
import os
import subprocess
import sys
import tempfile
import mujoco
import numpy as np
ROOT = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
ROBOT_ITEM = os.path.join(ROOT, "robot_item")
TRAINING = os.path.join(ROOT, "twoleg_mjcf", "robot_twoleg.xml")
def build_reference(dst):
sys.path.insert(0, ROBOT_ITEM)
import chest_both_arms_legs_move as ref # noqa: E402
ref.build(dst, True)
return dst
def names(model, objtype):
return {
mujoco.mj_id2name(model, objtype, i)
for i in range(model.njnt if objtype == mujoco.mjtObj.mjOBJ_JOINT else model.nbody)
if mujoco.mj_id2name(model, objtype, i)
}
def joint_table(model):
data = mujoco.MjData(model)
mujoco.mj_forward(model, data)
out = {}
for j in range(model.njnt):
name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, j)
if not name:
continue
body = model.jnt_bodyid[j]
out[name] = {
"anchor_world": data.xpos[body] + data.xmat[body].reshape(3, 3) @ model.jnt_pos[j],
"axis_world": data.xmat[body].reshape(3, 3) @ model.jnt_axis[j],
"type": int(model.jnt_type[j]),
}
return out
def world_pinned(model):
return [i for i in range(1, model.nbody) if model.body_parentid[i] == 0]
def summarize(label, model, path):
pinned = world_pinned(model)
free = [j for j in range(model.njnt) if model.jnt_type[j] == mujoco.mjtJoint.mjJNT_FREE]
print(f"=== {label} ===")
print(f" path : {os.path.relpath(path, ROOT)}")
print(f" bodies : {model.nbody - 1}")
print(f" joints : {model.njnt}")
print(f" actuators : {model.nu}")
print(f" geoms : {model.ngeom}")
print(f" mass : {model.body_mass.sum():.4f} kg")
print(f" world-pinned bodies: {len(pinned)}")
print(f" freejoints : {len(free)}")
print()
return pinned, free
def main():
with tempfile.TemporaryDirectory() as tmp:
ref_path = build_reference(os.path.join(tmp, "reference.xml"))
ref = mujoco.MjModel.from_xml_path(ref_path)
train = mujoco.MjModel.from_xml_path(TRAINING)
ref_pinned, ref_free = summarize("REFERENCE (robot_item/chest_both_arms_legs.xml via chest_move rig)", ref, ref_path)
tr_pinned, tr_free = summarize("TRAINING (twoleg_mjcf/robot_twoleg.xml)", train, TRAINING)
ref_joints = names(ref, mujoco.mjtObj.mjOBJ_JOINT)
tr_joints = names(train, mujoco.mjtObj.mjOBJ_JOINT)
print("=== joint names ===")
print(f" in reference only: {sorted(ref_joints - tr_joints)}")
print(f" in training only : {sorted(tr_joints - ref_joints)}")
print(f" shared : {sorted(ref_joints & tr_joints)}")
print()
mass_ratio = train.body_mass.sum() / ref.body_mass.sum()
print("=== headline deltas ===")
print(f" mass training/reference = {mass_ratio:.3f} "
f"({train.body_mass.sum():.3f} vs {ref.body_mass.sum():.3f} kg)")
print(f" joints training/reference = {train.njnt}/{ref.njnt}")
print(f" bodies training/reference = {train.nbody-1}/{ref.nbody-1}")
print(f" reference is world-pinned = {len(ref_pinned) > 1}")
print(f" training is a free body = {len(tr_free) == 1}")
print()
shared = sorted(ref_joints & tr_joints)
if shared:
print("=== shared joint anchors, world frame at zero pose ===")
rt, tt = joint_table(ref), joint_table(train)
for name in shared:
d = np.linalg.norm(rt[name]["anchor_world"] - tt[name]["anchor_world"])
print(f" {name:20s} delta = {d*1000:8.2f} mm")
print()
print("The reference is a world-pinned puppet with no freejoint, so it cannot be")
print("dropped into a free-body simulation. The training model is generated from it")
print("by generate_twoleg_model.py: same parts, same pivots, re-parented into one")
print("connected tree. Anchors above should read 0.00 mm and the mass ratio 1.000.")
print()
print("Expected differences, all deliberate:")
print(" +1 body the world-aligned base root, because the chest body carries a")
print(" 90-degree rotation that would break projected_gravity as root")
print(" +2 geoms one collision box per foot; the reference's 26 mesh collision")
print(" geoms are visual-only here because mesh-mesh contact on the")
print(" leg brackets blows the solver up")
print(" -2 joints the duplicate waist hinges the reference models twice")
print(" +1 joint the freejoint")
if __name__ == "__main__":
main()
|