"""Johnny 6's firmware: every baked skill as a COMMAND. The operator's architecture, stated twice and then demanded: "the manual joystick should send the robot commands to act on our automated learnt steps" and "are we not just using that same baked firmware in the free roam?" Until this file, no - free roam was a hand transcription of the laws that kept missing organs (the spin's CoM hold, recover's foot shift, the learned leg speed), and every omission was a fall the exams had already priced away. That is the same disease controller.py was built to kill, one layer up. Structure: - Each skill is a LAW CLASS holding its baked gains and one tick() of pure control logic, registered under a command name. The law tick is EXTRACTED from the stage's exam episode and the episode now calls it - so the exam, the demo and free roam execute the same bytes, and parity is proved by re-scoring every bake (recorded in the commit that introduced this file). - Firmware is the dispatcher: it loads the bakes, owns the shared base layers (bench-interpolated wheel gains, the lean trim, and recover's reflex), maps operator input to commands, and runs the handover glue between laws - the one part that is firmware-only, which is why it lives here where it can be tested headless, not in a web server. Growing the firmware = write one law class with @command, give its exam episode the same tick, and the remote gets the command for free. Laws take plain numbers in and give actuator targets out - no MuJoCo, no files - so this module is the thing that gets transcribed to the real robot's MCU. """ import json import math from pathlib import Path import numpy as np import controller as C COMMANDS = {} def command(name): def reg(cls): cls.name = name COMMANDS[name] = cls return cls return reg # ------------------------------------------------------------ shared layers # the three benches and their stances - the interpolation's x-axis BALANCE_STANCES = [("balance", 460.0), ("balance_mid", 400.0), ("balance_crouch", 340.0)] def interp_gains(pts, stance_mm): """Wheel gains at a height, interpolated between the balance bakes. Piecewise LINEAR through the baked points, clamped flat at the ends - a quadratic through three points can overshoot between them, and an overshot clamp or station is a law nobody examined.""" if not pts: raise RuntimeError("no balance bench is baked") h = float(stance_mm) if h <= pts[0][0]: return pts[0][1] if h >= pts[-1][0]: return pts[-1][1] for (h0, g0v), (h1, g1v) in zip(pts, pts[1:]): if h0 <= h <= h1: f = (h - h0) / max(1e-9, h1 - h0) return (1.0 - f) * g0v + f * g1v return pts[-1][1] # ------------------------------------------------------------------- laws # # Every law: __init__ takes its baked gains and fixed context, tick() # takes this tick's sensed numbers and returns what to actuate. All # clamps, signs, filters and ramps live INSIDE the law - the caller # supplies measurements and intentions, never safety. @command("spin") class SpinLaw: """The baked spin skill - on-the-spot rotation at any gate height. Verbatim from spin_episode (SCORE_VERSION 93, baked 5.79): rate ramp (a stepped rate kicks the elliptical topple), yaw-rate servo with its clamp, CoM hold walking the feet under the measured mass, the bank answering roll with direction-aware feedforward, cent trim against the omega-squared pitch shift, and the skater rise. Serves both command shapes: a held rate (the stick / throttle) and a turn-to-heading (indexed quarters, mission verbs).""" STAGE = "spin" def __init__(self, gains, stance_mm, wheel_gains, brace, dt): (self.leg_speed, k_yaw, t_max, k_head, spin_rate, self.com_hold, self.bank_k, self.bank_d, self.cent_k, self.bank_ff, self.skater) = (list(gains) + [0.0] * 11)[:11] self.k_yaw = abs(k_yaw) self.t_max = min(0.5, max(0.02, abs(t_max))) self.k_head = abs(k_head) self.spin_rate = min(4.0, max(0.2, abs(spin_rate))) self.kp, self.kd, self.station, self.clamp = [ float(v) for v in wheel_gains] self.stance_mm = float(stance_mm) self.trim = math.radians(C.lean_trim_deg(self.stance_mm)) self.leg_rate = C.leg_rate(self.leg_speed) self.brx = brace self.dt = dt self.fdx = 0.0 self.wz_ramp = 0.0 def tick(self, lean, lrate, speed, gz, rollr, rrate, com_off, hold_wz=None, remaining=0.0): """One control tick. Returns (leg_want, leg_rate, u, t, landed).""" if hold_wz is not None: # the SUSTAINED spin: a constant held rate - RAMPED in, # because stepping the rate in one tick kicks a translation # the rotation smears into an ellipse landed = False self.wz_ramp = min(abs(hold_wz), self.wz_ramp + 3.0 * self.dt) wz_cmd = math.copysign(self.wz_ramp, hold_wz) else: # turn-to-heading: P on remaining angle, capped at the # learned safe rate; a landed quarter is just balance with # a gentle 0.3 rad/s heading trim landed = (abs(remaining) < math.radians(6.0) and abs(gz) < 0.4) wz_cap = 0.3 if landed else self.spin_rate wz_cmd = float(np.clip(self.k_head * remaining, -wz_cap, wz_cap)) # spin about the MEASURED centre of mass: feet walk the axle # under it in realtime, offset projected on the robot's OWN # fore-aft axis self.fdx += (self.dt / 0.08) * (self.com_hold * com_off - self.fdx) # the bank: inside leg shortens, outside extends - feedback on # roll plus the direction-aware feedforward; asleep at standstill dh = float(np.clip(self.bank_k * rollr + self.bank_d * rrate + self.bank_ff * gz, -0.05, 0.05)) dh *= min(1.0, abs(gz) / 0.3) # the skater: the working height rises with spin rate sk_mm = min(460.0, self.stance_mm + 100.0 * abs(self.skater) * abs(gz)) dxc = float(np.clip(self.fdx, -0.14, 0.14)) hl, kl = C.stance_to_leg( max(0.2, min(0.462, sk_mm / 1000.0 + dh)), dxc) hr, kr = C.stance_to_leg( max(0.2, min(0.462, sk_mm / 1000.0 - dh)), dxc) # NO odometry in the wheels (stillness doctrine) ref = (C.lean_reference(self.station, speed, 0.0, self.clamp) + self.brx.update(speed, 0.0, self.dt)) # cent trim: the omega-squared equilibrium shift, fed forward u = C.wheel_command(self.kp, self.kd, lean, lrate, ref, self.trim + float(np.clip( self.cent_k * gz * gz, -0.25, 0.25))) t = float(np.clip(-self.k_yaw * (wz_cmd - gz), -self.t_max, self.t_max)) t = float(np.clip(t, -(1.0 - abs(u)), 1.0 - abs(u))) return (np.array([hl, kl, hr, kr]), self.leg_rate, u, t, landed) @command("drive") class DriveLaw: """The baked cruise skill - hold a commanded road speed. Verbatim from cruise_episode (baked 7.09): the speed loop's own filter before the lean target (raw speed feedback is the drive limit cycle), the acceleration budget on the command, odometry of the SPEED ERROR so asking for speed does not wind the station term against you, and the shove brace fed its own throttle so it cannot brace against the operator.""" STAGE = "cruise" def __init__(self, gains, brace, dt): (self.kp, self.kd, self.station, self.clamp, v_tau, accel) = (list(gains) + [0.10, 1.2])[:6] self.filt = C.SpeedFilter(abs(v_tau)) self.accel = max(0.3, abs(accel)) self.brx = brace self.dt = dt self.v_cmd = 0.0 self.oerr = 0.0 def tick(self, want_v, lean, lrate, speed): """One control tick. Returns (u, v_cmd).""" self.v_cmd += float(np.clip(want_v - self.v_cmd, -self.accel * self.dt, self.accel * self.dt)) v = self.filt.update(speed, self.dt) self.oerr += (v - self.v_cmd) * self.dt ref = (C.lean_reference(self.station, v - self.v_cmd, self.oerr, self.clamp) + self.brx.update(speed, self.v_cmd, self.dt)) u = C.wheel_command(self.kp, self.kd, lean, lrate, ref) return u, self.v_cmd class GovernedDrive: """INTERIM drive - bench wheels + cruise's governance, until cruise is relearned on live legs. The baked cruise law was learned on its exam's wheels-only body: the legs there are rigid. On the real robot's live position-servo legs it runs away at EVERY speed (measured: pitch grows to the law's own clamp, wheels saturate, 2.4 m/s faceplant from a 0.4 command). Until the cruise exam runs with live legs (planned with the rock meter, SCORE_VERSION 94) the firmware drives on the balance bench's wheel law at the stand - with trim - governed by cruise's two learned slots: the speed filter and the acceleration budget. This composition measured 1.0 m/s with steering, released clean, before the refactor.""" def __init__(self, cruise_g, wheel_g, trim, brace, dt): g = (list(cruise_g or []) + [12.0, 2.0, 1.0, 0.08, 0.10, 1.2])[:6] self.kp, self.kd, self.station, self.clamp = [ float(v) for v in wheel_g] self.trim = float(trim) self.v_tau = max(0.03, abs(g[4])) self.accel = max(0.3, abs(g[5])) self.brx = brace self.dt = dt self.v_f = 0.0 self.v_cmd = 0.0 def tick(self, want_v, lean, lrate, speed): self.v_cmd += float(np.clip(want_v - self.v_cmd, -self.accel * self.dt, self.accel * self.dt)) self.v_f += (self.dt / self.v_tau) * (speed - self.v_f) ref = (C.lean_reference(self.station, self.v_f - self.v_cmd, 0.0, self.clamp) + self.brx.update(speed, self.v_cmd, self.dt)) u = C.wheel_command(self.kp, self.kd, lean, lrate, ref, self.trim) return u, self.v_cmd @command("stance") class StanceLaw: """Stand at a height and catch whatever arrives. Verbatim from recover_episode (the standing law every exam rides on): feet slide TOWARD the fall, putting the contact patch back under the mass faster than the wheels can drive it there, and the shove brace leans into a detected push. Wheel gains and trim are supplied by the caller so the same law serves recover's exam (its own baked slots at 460) and the firmware's stance command (bench-interpolated gains at any gate height).""" STAGE = "recover" def __init__(self, wheel_gains, trim, shift_k, shift_max, leg_speed, brace, stance_mm, dt): self.kp, self.kd, self.station, self.clamp = [ float(v) for v in wheel_gains] self.trim = float(trim) self.shift_k = float(shift_k) self.shift_max = float(shift_max) self.leg_rate = C.leg_rate(leg_speed) self.brx = brace self.stance_mm = float(stance_mm) self.dt = dt def tick(self, lean, lrate, speed, odo, v_cmd=0.0): """One control tick. Returns (leg_want, leg_rate, u).""" dx = C.foot_shift(self.shift_k, lean, self.shift_max) hp, kn = C.stance_to_leg(self.stance_mm / 1000.0, dx) b = self.brx.update(speed, v_cmd, self.dt) ref = C.lean_reference(self.station, speed, odo, self.clamp) + b u = C.wheel_command(self.kp, self.kd, lean, lrate, ref, self.trim) return np.array([hp, kn, hp, kn]), self.leg_rate, u @command("wake") class WakeLaw: """Stand up from the collapsed heap - the baked rise choreography. Phases: lay out flat, launch pose, then the rise drive BY HEIGHT (gating on roller contact cut the drive at the rock-over and dropped it every time) until the stand pose lands. A wake that has not stood inside its window re-seats and tries again - the rise skill is honestly imperfect (bake 6.36/12) and the real machine would do exactly this.""" STAGE = "rise" LAY = (48.0, 26.0) # the operator's lay-out LAUNCH = (53.0, -141.0) # the operator's launch pose TIMEOUT = 14.0 # measured wakes finish in 8-12s TRIES = 3 def __init__(self, gains, dt): g = (list(gains or [1.7796, 0.0258, -0.6327, 24.06]) + [0.0] * 4)[:4] self.g = [float(v) for v in g] self.hand_m = max(0.200, min(0.462, abs(self.g[3]) / 1000.0)) self.dt = dt self.phase = 0 self.t = 0.0 self.tries = 0 self.gave_up = False sth, stk = C.stance_to_leg(0.455) self.stand = np.array([sth, stk] * 2) self.lay = np.array([math.radians(self.LAY[0]), math.radians(self.LAY[1])] * 2) self.launch = np.array([math.radians(self.LAUNCH[0]), math.radians(self.LAUNCH[1])] * 2) def tick(self, hip_z, cur_legs): """One control tick. Returns (leg_want, leg_rate, drive, done, reseat): drive is the open-loop wheel command during the ascent (None = balance owns the wheels), done means STOOD, reseat asks the caller to put the robot back in the heap for another try (sim: place_sitting; real robot: it is already down there).""" self.t += self.dt if self.t > self.TIMEOUT: self.tries += 1 self.t = 0.0 self.phase = 0 if self.tries >= self.TRIES: self.gave_up = True return cur_legs, math.radians(60.0), None, False, False return cur_legs, math.radians(60.0), None, False, True if self.gave_up: return cur_legs, math.radians(60.0), None, False, False if self.phase == 0: # lay out, gently - momentum here is a failed launch there if float(np.max(np.abs(cur_legs - self.lay))) \ < math.radians(6.0): self.phase = 1 return self.lay, math.radians(35.0), None, False, False if self.phase == 1: if float(np.max(np.abs(cur_legs - self.launch))) \ < math.radians(8.0): self.phase = 2 return self.launch, math.radians(150.0), None, False, False # phase 2, the ascent: rise drive by HEIGHT until the handover, # then the stand pose while balance takes the wheels drive = None if hip_z < self.hand_m: drive = float(np.clip( self.g[0] + self.g[1] * (hip_z - 0.165) / 0.29, -1.0, 1.0)) want = self.stand rate = math.radians(150.0 * min(1.5, max(0.1, abs(self.g[2])))) done = (hip_z * 1000.0 > 300.0 and float(np.max(np.abs(cur_legs - self.stand))) < math.radians(8.0)) return want, rate, drive, done, False # -------------------------------------------------------------- dispatcher class Firmware: """The robot's brain between the remote and the laws. Owns the bakes, the base layers, and the mode glue: which law has the legs and wheels right now, and how command changes hand over. The glue rules were each paid for with a measured fall: - the spin law engages only for on-the-spot turns (driving turns are differential - the CoM hold cancels the drive lean and diverges on long legs), and once engaged it LATCHES until the stick releases (the speed estimate wobbles past any entry gate mid-spin, and dropping the legs mid-spin fell a 350 crouch nearly every time); - every law exit hands the legs to a neutral stance hold at that height (freezing "where they are" kept the CoM hold's foot offset baked in - equilibrium off the trim lean, wheels accelerating forever: the mission-end shoot-off); - the stance hold's odometry rebases on entry: hold HERE, never home to where the odometer happened to start. """ def __init__(self, dt, runs_dir): self.dt = dt self.runs = Path(runs_dir) self.bpts = [] for nm, h in BALANCE_STANCES: g = self.bake(nm) if g: self.bpts.append((h, np.array(g[:4], float))) self.bpts.sort() self.rec = self.bake("recover") # idle | wake | stance | spin | drive | manual self.mode = "idle" self.law = None self.stance_mm = None self.manual_legs = None self.want_v = 0.0 self.want_wz = 0.0 self.odo = 0.0 def bake(self, stage): try: return json.load( open(self.runs / f"{stage}_gains.json"))["gains"] except Exception: return None def brace(self): """Recover's reflex as a base layer under every law.""" if self.rec and len(self.rec) >= 10: return C.ShoveBrace(self.rec[8], self.rec[9]) return C.ShoveBrace(0.0, 0.3) # ---- commands (the remote's vocabulary) def cmd_wake(self): self.law = WakeLaw(self.bake("rise"), self.dt) self.mode = "wake" def cmd_stance(self, mm): mm = float(np.clip(mm, 330.0, 462.0)) changed = (self.stance_mm is None or abs(mm - self.stance_mm) > 0.5) self.stance_mm = mm # only a CHANGED height rebuilds the hold - the remote streams # the fader every tick, and rebuilding per tick would reset the # brace and re-zero the odometer forever if self.mode == "stance" and changed: self._stand(mm) def cmd_manual(self, legs_rad): """Direct joint sculpting: legs verbatim, wheels balancing. None releases back to a stance hold at the measured height.""" if legs_rad is None: if self.mode == "manual": self.mode = "manual_exit" # resolved in tick (needs hip_z) return self.manual_legs = np.array([float(v) for v in legs_rad]) if self.mode != "wake": self.mode = "manual" self.law = None def cmd_halt(self): self.want_v = 0.0 self.want_wz = 0.0 self._stand(self.stance_mm or 455.0) def stick(self, v, wz): """The semi-automated stick: raw operator axes in, LAW choice out. Forward = the drive law; twist at standstill = the spin law; release = a stance hold where you are. SHAPED for a real 2-axis stick (operator's diagnosis: the demo slider was one pure axis, the joystick is not): a full-left pull always carries vertical leakage, and 12% of wobble was measured flipping the firmware through four modes mid-spin. Deadzone per axis, then axis dominance - the axis you clearly mean wins, the leakage is discarded - and while a spin is latched the vertical axis is dead entirely until the twist releases: mode changes are deliberate acts, not thumb drift.""" v = float(np.clip(v, -1.5, 1.5)) # rate ceiling 1.0: the measured release-clean envelope - 1.2 # sustained lurches 55 deg on release (unexamined regime); # 1.0 and 0.8 release clean both directions (0.6 oddly falls - # noted for the exam). The relearn may raise this. wz = float(np.clip(wz, -1.0, 1.0)) av, aw = abs(v) / 1.5, abs(wz) / 1.0 if av < 0.10: v = 0.0 if aw < 0.10: wz = 0.0 if self.mode == "spin" and abs(wz) > 0.0: v = 0.0 # the latch owns the stick elif aw >= 1.5 * av: v = 0.0 # clearly a twist elif av >= 1.5 * aw: wz = 0.0 # clearly a throttle self.want_v = v self.want_wz = wz # ---- the glue def _stand(self, mm): mm = float(np.clip(mm if mm else 455.0, 330.0, 462.0)) self.law = StanceLaw( interp_gains(self.bpts, mm), math.radians(C.lean_trim_deg(mm)), self.rec[6] if self.rec and len(self.rec) >= 8 else 0.0, self.rec[7] if self.rec and len(self.rec) >= 8 else 0.1, self.rec[5] if self.rec and len(self.rec) >= 8 else 1.0, self.brace(), mm, self.dt) self.mode = "stance" self.odo = 0.0 # hold HERE def _spin(self, mm): g = self.bake("spin") if g is None: return False mm = float(np.clip(mm if mm else 455.0, 340.0, 460.0)) self.law = SpinLaw(g, mm, interp_gains(self.bpts, mm), self.brace(), self.dt) self.mode = "spin" self.spin_settle = 0.0 self.hold_cmd = 0.0 self.spin_rel = None return True def _bench_u(self, hip_z, lean, lrate, speed): """Bench-interpolated balance at the MEASURED height - the wheel law under manual sculpting and the wake's ascent.""" mm = float(np.clip(1000.0 * hip_z, 340.0, 462.0)) wg = interp_gains(self.bpts, mm) ref = C.lean_reference(float(wg[2]), speed, 0.0, float(wg[3])) return C.wheel_command(float(wg[0]), float(wg[1]), lean, lrate, ref, math.radians(C.lean_trim_deg(mm))) def _drive(self): self.law = GovernedDrive(self.bake("cruise"), interp_gains(self.bpts, 455.0), math.radians(C.lean_trim_deg(455.0)), self.brace(), self.dt) # DRIVE carries its exam's conditions: cruise was learned at # the stand pose, where its trimless wheel law is honest (trim # ~0.2 deg). Driving from a 350 crouch needs ~7 deg the law # does not know - so the legs rise to the stand for the drive # and return to the operator's gate on release. hp, kn = C.stance_to_leg(0.455) self.drive_legs = np.array([hp, kn, hp, kn]) self.mode = "drive" def tick(self, sense): """One firmware tick. sense: dict with lean, lrate, speed, gz, rollr, rrate, com_off, hip_z, cur_legs. Returns dict with leg_want, leg_rate, u, t, wake_drive, reseat, mode.""" lean = sense["lean"] lrate = sense["lrate"] speed = sense["speed"] self.odo += speed * self.dt out = {"u": 0.0, "t": 0.0, "wake_drive": None, "reseat": False, "leg_want": None, "leg_rate": None} if self.mode == "wake": want, rate, drive, done, reseat = self.law.tick( sense["hip_z"], sense["cur_legs"]) out.update(leg_want=want, leg_rate=rate, wake_drive=drive, reseat=reseat) if done: # the stand hands over to a stance hold at the # operator's chosen gate (or the stand height) self._stand(self.stance_mm or 455.0) elif self.law.gave_up: self.mode = "idle" self.law = None if self.mode == "wake": # balance owns the wheels above the handover height if drive is None: out["u"] = self._bench_u(sense["hip_z"], lean, lrate, speed) out["mode"] = self.mode return out if self.mode == "manual": out.update(leg_want=self.manual_legs, leg_rate=math.radians(240.0), u=self._bench_u(sense["hip_z"], lean, lrate, speed)) out["mode"] = self.mode return out if self.mode == "manual_exit": self._stand(float(np.clip(1000.0 * sense["hip_z"], 330.0, 462.0))) # mode arbitration from the stick (never during a wake, never # on a heap - idle means the robot has not stood yet and the # stick is dead until WAKE) spinning = (self.mode == "spin") want_spin = (abs(self.want_wz) > 0.02 and abs(self.want_v) <= 0.05) want_drive = abs(self.want_v) > 0.05 if self.mode == "idle": want_spin = want_drive = False if want_drive and self.mode == "stance": # COMMANDS SEQUENCE THROUGH THE ROBOT'S OWN STATE (the # operator's doctrine): a throttle during a spin does # NOTHING until the spin has come to rest in a stance # hold - the robot finishes what it is doing before it # takes the next order. And RISE BEFORE YOU DRIVE: cruise # was learned at the stand, so a crouched drive-off first # stands up (trim-correct), then hands to the drive. if sense["hip_z"] * 1000.0 < 430.0: if self.law.stance_mm < 454.0: self._stand(455.0) else: self._drive() elif want_spin and self.mode == "stance": # entry gate: on-the-spot only (the latch is the mode # itself - once spinning, only the stick releases it) if abs(speed) < 0.3: self._spin(self.stance_mm) # spin release is choreographed INSIDE the spin branch below, # using the exam's own landed shape - switching laws with any # residual motion drops the robot (measured: every handover # variant tried, prompt or patient, fell on some direction) elif self.mode == "drive" and not want_drive \ and abs(self.law.v_cmd) < 0.05 and abs(speed) < 0.15: # drive release: the law winds its own command down; the # stance hold takes over only once genuinely slow self._stand(self.stance_mm) if self.mode == "spin": # the rate command RAMPS BOTH WAYS. The law's own ramp # only ramps up (v92: a stepped rate kicks a translation # the rotation smears into an ellipse) - and a stepped # RELEASE kicks the same ellipse backwards: measured 1.25 # m/s of translation the station-less spin law can never # kill. The demo never showed it because its spring-return # slider IS a down-ramp; the firmware provides the spring. target = self.want_wz if abs(self.want_wz) > 0.02 else 0.0 self.hold_cmd += float(np.clip(target - self.hold_cmd, -3.0 * self.dt, 3.0 * self.dt)) gz = sense["gz"] if abs(target) > 0.02 or (self.spin_rel is None and abs(self.hold_cmd) > 0.05): # commanded (or still winding down): held-rate mode self.spin_rel = None want, rate, u, t, _ = self.law.tick( lean, lrate, speed, gz, sense["rollr"], sense["rrate"], sense["com_off"], hold_wz=self.hold_cmd if abs(self.hold_cmd) > 1e-6 else 1e-6) else: # RELEASED: hold rate ZERO - exactly the demo's shape, # the one the operator has verified for a week. A # heading-hold variant was tried here and measured # WORSE: its servo chases a drifting gyro-integrated # reference, and the chase sustained a slow pirouette # (0.7 rad/s) the yaw brake then fought forever. The # law at zero rate is the exam's own settle state. if self.spin_rel is None: self.spin_rel = 0.0 self.spin_settle = 0.0 want, rate, u, t, _ = self.law.tick( lean, lrate, speed, gz, sense["rollr"], sense["rrate"], sense["com_off"], hold_wz=1e-6) # landed = the ROTATION is dead, judged by the gyro # alone (the exam's own quiet criterion) if abs(gz) < 0.25: self.spin_settle += self.dt else: self.spin_settle = 0.0 # LANDED - and it STAYS HERE, like the demo. The # landed spin law is the strongest stander the robot # owns (CoM hold live, feet under the mass); the # stance law's recover reflex is baked near zero - the # operator watched the handover go "to a weaker stance # that cannot cope". The legs change law only when the # NEXT command needs different machinery - throttle, # or a moved gate. A command also forces the swap at # merely QUIET-ISH (a residual 0.3-0.5 rad/s scrub # pirouette at tall stances never passes the strict # settle, and it must not leave the robot deaf). _wants_out = (abs(self.want_v) > 0.05 or (self.stance_mm is not None and abs(self.stance_mm - self.law.stance_mm) > 0.5)) if self.spin_settle >= 0.4: out["mode"] = "hold" if _wants_out and (self.spin_settle >= 0.4 or abs(gz) < 0.6): self._stand(self.stance_mm or self.law.stance_mm) out.update(leg_want=want, leg_rate=rate, u=u, t=t) elif self.mode == "drive": u, _ = self.law.tick(self.want_v, lean, lrate, speed) # driving turns are a wheels-only differential, gentle t = float(np.clip(-0.35 * self.want_wz, -0.25, 0.25)) t = float(np.clip(t, -(1.0 - abs(u)), 1.0 - abs(u))) out.update(u=u, t=t, leg_want=self.drive_legs, leg_rate=math.radians(240.0)) elif self.mode == "stance": want, rate, u = self.law.tick(lean, lrate, speed, self.odo) out.update(leg_want=want, leg_rate=rate, u=u) if "mode" not in out or out.get("mode") is None \ or out["mode"] == self.mode: out["mode"] = self.mode return out