"""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)))])