tars-mini / sim /control_check.py
tuanhqv123's picture
TARS mini: CAD, MuJoCo sim, trained policies and 5-action demo
4b7148d verified
Raw History Blame Contribute Delete
1.8 kB
"""Is lie-down / stand-up CONTROLLED? Measure tilt vs commanded, tilt rate, and floor impact force."""
import math, numpy as np, mujoco
from model import Params
from sim import TarsSim
CHEAP = dict(cells=0.046, ups=0.013, mu=0.8)
def floor_force(s):
f6, tot = np.zeros(6), 0.0
for i in range(s.d.ncon):
c = s.d.contact[i]
if c.geom1 == s.floor or c.geom2 == s.floor:
mujoco.mj_contactForce(s.m, s.d, i, f6); tot += f6[0]
return tot
def trace(seq, p=None):
s = TarsSim(p or Params(**CHEAP)); s.run(0.5)
W = s.m.body_subtreemass[s.torso_id] * 9.81
out = []
orig = s.run
def run(sec):
n = max(1, int(round(sec / s.m.opt.timestep)))
for _ in range(n):
s.step()
if int(round(s.d.time / s.m.opt.timestep)) % 10 == 0:
out.append((s.d.time, s.tilt_deg(), floor_force(s) / W, getattr(s, "phase", ""), s.d.subtree_com[s.torso_id][2]))
s.run = run
for phase, fn in seq:
s.phase = phase; fn(s)
t, tilt, F, ph, zc = map(np.array, zip(*out))
rate = np.gradient(tilt, t)
vz = np.gradient(zc, t)
for name in dict.fromkeys(ph):
k = ph == name
print(f" {name:28s} tilt {tilt[k][0]:5.1f}->{tilt[k][-1]:5.1f}° max tilt-rate {np.abs(rate[k]).max():6.1f}°/s "
f"max COM fall speed {max(0,-vz[k].min())*100:5.1f} cm/s peak floor force {F[k].max():4.2f}×weight")
return s
if __name__ == "__main__":
print("CURRENT routine (legs ramp 0 -> 60° in 3 s, back in 3 s):")
trace([("lie down", lambda s: (s.ramp(3.0, sL=60, sR=60), s.run(0.5))),
("power off", lambda s: (setattr(s, "limp", True), s.run(2.0))),
("stand up", lambda s: (setattr(s, "limp", False), s.ramp(3.0, sL=0, sR=0), s.run(0.8)))])