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