"""Physics lab audit, from first principles.
Each test is a rung on a ladder: a bare wheel, then two wheels on a rod, then
the robot. Every rung has a hand-calculated expectation next to the measured
result, so a failure says WHERE the physics went wrong, not just that it did.
python test_lab.py
"""
import math
import mujoco
import numpy as np
R = 0.0625 # wheel radius, m (125mm wheel)
HALFW = 0.020 # half width of the tyre cylinder (40mm wide)
MU = 1.5 # rubber-on-floor sliding friction
def world(extra, timestep=0.001):
return f"""
{extra}
"""
def run(xml, ctrl, secs):
m = mujoco.MjModel.from_xml_string(xml)
d = mujoco.MjData(m)
for _ in range(int(secs / m.opt.timestep)):
for k, v in ctrl.items():
d.ctrl[mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_ACTUATOR, k)] = v
mujoco.mj_step(m, d)
return m, d
def verdict(ok):
return "PASS" if ok else "FAIL <-- physics broken here"
# ---------------------------------------------------------------- test 1
def t1_single_wheel(shape):
geom = {
"cylinder": f'',
"sphere": f'',
}[shape]
xml = world(f"""
{geom}
""") .replace("", """
""")
tau, secs = 0.3, 1.0
m, d = run(xml, {"m": tau}, secs)
x = d.qpos[0]
# torque rolls the wheel: a = tau / (r * (m + I/r^2)); I disc = m r^2 / 2
mass = 0.45
a = tau / R / (mass + 0.5 * mass) # = tau/(1.5 m r)
expect = 0.5 * a * secs ** 2
ok = abs(x - expect) / expect < 0.25
print(f" 1. {shape:9s} wheel, {tau} Nm, {secs}s: rolled {x*1000:6.0f} mm "
f"(theory {expect*1000:.0f} mm) {verdict(ok)}")
return ok
# ---------------------------------------------------------------- test 2
def t2_dumbbell(shape, tau=0.33, secs=1.0, track=0.38):
y = track / 2
g = {
"cylinder": f'',
"sphere": f'',
}[shape]
xml = world(f"""
{g}
{g}
""").replace("", """
""")
m, d = run(xml, {"lm": tau, "rm": 0.0}, secs)
q = d.qpos[3:7]
yaw = math.degrees(math.atan2(2*(q[0]*q[3]+q[1]*q[2]), 1-2*(q[2]**2+q[3]**2)))
x, ypos = d.qpos[0]*1000, d.qpos[1]*1000
# crude expectation: drive force at one wheel, moment about the centre,
# rigid-body yaw. The unpowered wheel free-rolls, so scrub is second order
# for a POINT contact.
F = tau / R
Izz = 2*0.45*y*y + 0.6*(2*y)**2/12
alpha = F * y / Izz
expect = math.degrees(0.5 * alpha * min(secs, 0.5) ** 2) # early, pre-slip
print(f" 2. {shape:9s} pair, one wheel {tau} Nm, {secs}s: "
f"yaw {yaw:+7.1f} deg moved x {x:+5.0f} y {ypos:+5.0f} mm "
f"(free-roll theory says it MUST veer, order {expect:.0f}deg+)")
return yaw
print("PHYSICS LAB AUDIT (bare MuJoCo, no robot, no tether)")
print(f"wheel r={R*1000:.0f}mm, tyre {2*HALFW*1000:.0f}mm wide, floor mu={MU}\n")
print("Does torque roll a single wheel correctly?")
t1_single_wheel("cylinder")
t1_single_wheel("sphere")
print("\nTwo wheels on a rod, ONE powered. It must veer around the dead wheel.")
ycyl = t2_dumbbell("cylinder")
ysph = t2_dumbbell("sphere")
print(f"""
DIAGNOSIS
flat 40mm cylinder : {ycyl:+7.1f} deg
point contact : {ysph:+7.1f} deg
""")
if abs(ysph) > 5 * max(abs(ycyl), 1e-9) or (abs(ysph) > 20 and abs(ycyl) < 5):
print(""" The physics engine is fine - the WHEEL SHAPE is the problem.
A perfectly flat, rigid 40mm-wide cylinder on a rigid floor makes a LINE
contact. To yaw, that line must scrub sideways over its whole width, and
at mu=1.5 the scrub torque exceeds anything the wheels can produce. A real
tyre turns because rubber crowns and deforms to a small patch.
FIX: give the wheel a crowned (point) contact in the model.""")
else:
print(" Both shapes behave alike - the problem is NOT the contact shape.")