tars-mini / sim /com_study.py
tuanhqv123's picture
TARS mini: CAD, MuJoCo sim, trained policies and 5-action demo
4b7148d verified
Raw History Blame Contribute Delete
1.89 kB
"""Self-righting vs torso centre-of-mass height (design-for-self-righting, Kessens et al.).
Scripted get-up: legs push to 85 deg, then close back to 0."""
import json
import numpy as np
from multiprocessing import Pool
from model import Params
from getup import getup
def torso_com_from_foot(p):
"""Torso COM height above the torso foot (m): shell at mid-height, 2 height servos near the hip,
electronics either centred (default) or packed with the battery at batt_z."""
m = p.masses()
return m # computed below in job for clarity
def job(a):
layout, ballast, face, mu = a
if layout == "default": # electronics at torso centre, battery low (current model)
kw = dict(cells=0.046 + ballast, ups=0.013, batt_z=-0.045)
else: # ALL electronics packed at the bottom with the battery
kw = dict(cells=0.046 + 0.015 + 0.010 + 0.013 + 0.040 + ballast, board=0, pca=0, ups=0, misc=0, batt_z=-0.070)
p = Params(mu=mu, **kw)
r, s = getup(p, face, 85.0)
m = s.m; tid = s.torso_id
# torso-only COM height above the foot, measured in the standing model
return dict(layout=layout, ballast_g=int(ballast * 1000), face=face, mu=mu, ok=r["ok"],
final_tilt=r["final_tilt"], tilt_after_push=r["tilt_after_push"], swing_peak=r["swing_peak"])
if __name__ == "__main__":
jobs = [(l, b, f, mu) for l in ("default", "low") for b in (0.0, 0.03, 0.06) for f in ("back", "front") for mu in (0.5, 0.8)]
with Pool(9) as p:
R = p.map(job, jobs)
json.dump(R, open("../results/com_study.json", "w"), indent=1)
for r in R:
print(f"{r['layout']:7s} ballast {r['ballast_g']:2d}g lying {r['face']:5s} mu {r['mu']} -> stood up {r['ok']!s:5s} "
f"tilt after push {r['tilt_after_push']:6.1f}° final {r['final_tilt']:5.1f}° servo peak {r['swing_peak']:3.0f}%")