tars-mini / sim /sim.py
tuanhqv123's picture
TARS mini: CAD, MuJoCo sim, trained policies and 5-action demo
4b7148d verified
Raw History Blame Contribute Delete
11.7 kB
"""
Simulation core: hobby-servo actuator model, TARS-AI motion replay, metrics.
Servo model (analog hobby servo, e.g. MG90S):
* internal proportional controller, saturating at stall torque within `p_band_deg`
* torque-speed limit of a DC gear-motor: tau_avail = tau_stall * (1 - |w| / w_noload)
* height servos drive the slide through a crank: z = -r * sin(phi), F = tau / (r cos phi)
Motion replay reproduces TARS-AI module_servoctl.move_servos_synchronized():
every servo steps 1 PCA9685 count per loop, delay = 0.02*(1-speed)*easing + overhead.
"""
from __future__ import annotations
import math
import numpy as np
import mujoco
from model import Params, build_xml
US_PER_COUNT = 1e6 / 50 / 4096 # PCA9685 @ 50 Hz -> 4.883 us per count
DEG_PER_US = 180.0 / 2000.0 # hobby servo: 500..2500 us = 180 deg
DEG_PER_COUNT = US_PER_COUNT * DEG_PER_US # 0.4395 deg / count
# TARS-AI config.ini.template (left-side values; right side is mirror-mounted)
HEIGHT_UP, HEIGHT_DOWN, HEIGHT_NEUTRAL = 220, 460, 350
LEG_FWD, LEG_BACK, LEG_NEUTRAL = 160, 460, 300
def pct_to_count(p, lo, hi):
return int(round(lo + (hi - lo) * (p - 1) / 99.0))
class Servo:
"""Analog servo = P controller on drive voltage, saturating at the supply.
Drive torque is clipped at stall torque; back-EMF damping (b = tau_stall / w0) is applied
implicitly by MuJoCo as joint damping, so available torque falls off with speed exactly like
the DC-motor curve tau_stall * (1 - w / w0)."""
def __init__(self, p: Params, height=False):
self.ts = p.tau_stall_h if height else p.tau_stall
self.b = p.servo_b_h if height else p.servo_b
self.kp = self.ts / math.radians(p.p_band_deg)
def drive(self, phi_t, phi):
return float(np.clip(self.kp * (phi_t - phi), -self.ts, self.ts))
class TarsSim:
JOINTS = ("hL", "hR", "sL", "sR")
def __init__(self, p: Params | None = None, record=False, cam=None):
self.p = p or Params()
self.m = mujoco.MjModel.from_xml_string(build_xml(self.p))
self.d = mujoco.MjData(self.m)
self.servo = Servo(self.p)
self.servo_h = Servo(self.p, height=True)
self.qa = {j: self.m.joint(j).qposadr[0] for j in self.JOINTS}
self.va = {j: self.m.joint(j).dofadr[0] for j in self.JOINTS}
self.act = {j: self.m.actuator(j).id for j in self.JOINTS}
self.floor = self.m.geom("floor").id
self.torso_id = self.m.body("torso").id
# servo angle targets (rad). heights: crank angle; swings: leg angle
self.count = {"hL": HEIGHT_NEUTRAL, "hR": HEIGHT_NEUTRAL, "sL": LEG_NEUTRAL, "sR": LEG_NEUTRAL}
self.log = {k: [] for k in ("t", "x", "y", "z", "yaw", "up", "margin", "I_total")}
for j in self.JOINTS:
self.log["tau_" + j] = []
self.record, self.frames, self._r, self.cam_cfg = record, [], None, cam or {}
self._init_pose()
# ---------- kinematics of the 4 servos ----------
def phi_target(self, j):
return math.radians((self.count[j] - (HEIGHT_NEUTRAL if j[0] == "h" else LEG_NEUTRAL)) * DEG_PER_COUNT)
def _init_pose(self):
for j in self.JOINTS:
self.d.qpos[self.qa[j]] = self._joint_from_phi(j, self.phi_target(j))
mujoco.mj_forward(self.m, self.d)
def _joint_from_phi(self, j, phi):
return -self.p.crank_r * math.sin(phi) if j[0] == "h" else phi
def _phi_from_joint(self, j, q, qd):
if j[0] == "h":
s = float(np.clip(-q / self.p.crank_r, -0.999, 0.999))
phi = math.asin(s)
w = -qd / (self.p.crank_r * math.cos(phi))
return phi, w
return q, qd
# ---------- physics step ----------
def step(self):
d, p = self.d, self.p
I = 0.0
taus = {}
for j in self.JOINTS:
q, qd = d.qpos[self.qa[j]], d.qvel[self.va[j]]
phi, w = self._phi_from_joint(j, q, qd)
sv = self.servo_h if j[0] == "h" else self.servo
drive = 0.0 if getattr(self, "limp", False) else sv.drive(self.phi_target(j), phi)
if j[0] == "h":
d.ctrl[self.act[j]] = -drive / (p.crank_r * math.cos(phi)) # slide force through the crank
else:
d.ctrl[self.act[j]] = drive
tau = drive - (0.0 if getattr(self, "limp", False) else sv.b * w) # net motor torque
taus[j] = tau
ist = (p.i_stall_h if (j[0] == "h" and p.i_stall_h is not None) else p.i_stall)
I += min(ist, p.i_idle + (ist - p.i_idle) * abs(tau) / sv.ts + p.i_run * abs(w) / p.w0)
mujoco.mj_step(self.m, d)
return taus, I
def run(self, seconds):
n = max(1, int(round(seconds / self.m.opt.timestep)))
for _ in range(n):
taus, I = self.step()
if int(round(self.d.time / self.m.opt.timestep)) % 20 == 0: # log @100 Hz
self._log(taus, I)
# ---------- TARS-AI move_legs() replay ----------
def move_legs(self, lh=None, rh=None, ll=None, rl=None, speed=1.0, easing=0.6, overhead=0.002):
tgt = {}
if lh: tgt["hL"] = pct_to_count(lh, HEIGHT_UP, HEIGHT_DOWN)
if rh: tgt["hR"] = pct_to_count(rh, HEIGHT_UP, HEIGHT_DOWN)
if ll: tgt["sL"] = pct_to_count(ll, LEG_FWD, LEG_BACK)
if rl: tgt["sR"] = pct_to_count(rl, LEG_FWD, LEG_BACK)
for j in self.count: # after continuous moves (CPG/s-curve) counts are fractional
self.count[j] = int(round(self.count[j]))
tgt = {j: c for j, c in tgt.items() if c != self.count[j]}
if not tgt:
return
dist = {j: abs(c - self.count[j]) for j, c in tgt.items()}
maxd = max(dist.values())
steps = 0
base = 0.02 * (1 - speed)
while any(self.count[j] != c for j, c in tgt.items()):
for j, c in tgt.items():
if self.count[j] != c:
self.count[j] += 1 if c > self.count[j] else -1
steps += 1
prog = min(steps / maxd, 1.0)
e = 2 * prog * prog if prog < 0.5 else 1 - 2 * (1 - prog) ** 2
mult = 1 + easing * (1 - 4 * (e - 0.5) ** 2)
self.run(base * mult + overhead)
self.run(0.05)
def set_angles(self, **deg_or_m):
"""Direct targets for custom routines: sL/sR in degrees of leg angle, hL/hR in metres of leg lift."""
for j, v in deg_or_m.items():
if j[0] == "s":
self.count[j] = LEG_NEUTRAL + v / DEG_PER_COUNT
else:
phi = math.asin(float(np.clip(-v / self.p.crank_r, -0.99, 0.99)))
self.count[j] = HEIGHT_NEUTRAL + math.degrees(phi) / DEG_PER_COUNT
def ramp(self, seconds, **targets):
start = dict(self.count)
self.set_angles(**targets)
end = dict(self.count)
n = max(1, int(seconds / 0.02))
for k in range(1, n + 1):
a = k / n
for j in end:
self.count[j] = start[j] + (end[j] - start[j]) * a
self.run(0.02)
# ---------- metrics ----------
def com(self):
return self.d.subtree_com[self.torso_id].copy()
def _load_contacts(self, min_force=0.05):
"""Floor contact points that actually carry load (normal force > min_force N)."""
pts, f6 = [], np.zeros(6)
for i in range(self.d.ncon):
c = self.d.contact[i]
if c.geom1 == self.floor or c.geom2 == self.floor:
mujoco.mj_contactForce(self.m, self.d, i, f6)
if f6[0] > min_force:
pts.append(c.pos[:2].copy())
return pts
def support_margin(self):
"""Signed distance (m) from the COM ground projection to the edge of the support polygon (+ = inside).
Support polygon = load-bearing contacts accumulated over the last 50 ms (filters touch-down flicker)."""
self._hist = (getattr(self, "_hist", []) + [self._load_contacts()])[-5:]
pts = [q for frame in self._hist for q in frame]
if len(pts) < 1:
return float("nan")
P = np.array(pts)
c = self.com()[:2]
hull = _convex_hull(P)
if len(hull) < 3:
return -_dist_to_polyline(c, hull)
inside = True
dmin = 1e9
for a, b in zip(hull, np.roll(hull, -1, axis=0)):
e = b - a
n = np.array([e[1], -e[0]]) / (np.linalg.norm(e) + 1e-12)
s = np.dot(c - a, n)
dmin = min(dmin, abs(s))
if s > 0: # hull is CCW -> outward normal = (e_y, -e_x)
inside = False
return dmin if inside else -_dist_to_polygon(c, hull)
def tilt_deg(self):
up = self.d.sensor("up").data
return math.degrees(math.acos(float(np.clip(up[2], -1, 1))))
def _log(self, taus, I):
L, d = self.log, self.d
L["t"].append(d.time)
L["x"].append(d.qpos[0]); L["y"].append(d.qpos[1]); L["z"].append(d.qpos[2])
w, x, y, z = d.qpos[3:7]
L["yaw"].append(math.degrees(math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))))
L["up"].append(self.tilt_deg())
L["margin"].append(self.support_margin())
L["I_total"].append(I)
for j in self.JOINTS:
L["tau_" + j].append(taus[j])
if self.record and len(L["t"]) % 4 == 0:
self._frame()
def torque_stats(self):
out = {}
for j in self.JOINTS:
a = np.abs(np.array(self.log["tau_" + j])) / (self.p.tau_stall_h if j[0] == "h" else self.p.tau_stall) * 100
out[j] = dict(peak=round(float(a.max()), 1), p95=round(float(np.percentile(a, 95)), 1),
rms=round(float(np.sqrt((a ** 2).mean())), 1))
return out
def current_stats(self):
I = np.array(self.log["I_total"])
k = 5 # 50 ms moving window ~ what a bulk capacitor sees
Iw = np.convolve(I, np.ones(k) / k, mode="valid") if len(I) > k else I
return dict(mean_A=round(float(I.mean()), 2), peak50ms_A=round(float(Iw.max()), 2))
# ---------- rendering ----------
def _frame(self):
if self._r is None:
self._r = mujoco.Renderer(self.m, 480, 640)
self._cam = mujoco.MjvCamera()
self._cam.distance = self.cam_cfg.get("distance", 0.85)
self._cam.elevation = self.cam_cfg.get("elevation", -15)
self._cam.azimuth = self.cam_cfg.get("azimuth", 130)
self._cam.lookat[:] = [self.d.qpos[0], self.d.qpos[1], 0.07]
self._r.update_scene(self.d, self._cam)
self.frames.append(self._r.render().copy())
def _convex_hull(P):
P = np.unique(np.round(P, 5), axis=0)
if len(P) < 3:
return P
P = P[np.lexsort((P[:, 1], P[:, 0]))]
def cross(o, a, b): return (a[0]-o[0])*(b[1]-o[1]) - (a[1]-o[1])*(b[0]-o[0])
lower, upper = [], []
for q in P:
while len(lower) >= 2 and cross(lower[-2], lower[-1], q) <= 0: lower.pop()
lower.append(q)
for q in P[::-1]:
while len(upper) >= 2 and cross(upper[-2], upper[-1], q) <= 0: upper.pop()
upper.append(q)
return np.array(lower[:-1] + upper[:-1])
def _dist_to_seg(c, a, b):
ab = b - a
t = np.clip(np.dot(c - a, ab) / (np.dot(ab, ab) + 1e-12), 0, 1)
return float(np.linalg.norm(c - (a + t * ab)))
def _dist_to_polyline(c, P):
if len(P) == 1:
return float(np.linalg.norm(c - P[0]))
return min(_dist_to_seg(c, a, b) for a, b in zip(P[:-1], P[1:]))
def _dist_to_polygon(c, P):
return min(_dist_to_seg(c, a, b) for a, b in zip(P, np.roll(P, -1, axis=0)))