from __future__ import annotations import math from dataclasses import dataclass from enum import Enum from typing import Any import numpy as np from .observations import script_command_at class LifecycleState(str, Enum): IDLE = "IDLE" SOFT_START = "SOFT_START" ACTIVE = "ACTIVE" CALIBRATION = "CALIBRATION" FALL_SAFE = "FALL_SAFE" @dataclass class StepDecision: lifecycle: LifecycleState skill_mode: str policy_key: str | None torque_enabled: bool soft_start_alpha: float command_13: np.ndarray zero_last_action: bool mode_label: str recovery_state: str | None loco: str head_mode: bool class MicroduckStateMachine: """Hardware-safe 50Hz state machine mirroring app/src/robots/microduck/runtime.js.""" def __init__(self, manifest: dict[str, Any], custom_script: dict[str, Any] | None = None) -> None: self.manifest = manifest self.custom_script = custom_script self._load_params(manifest) self.lifecycle: LifecycleState = LifecycleState.IDLE self.loco: str = "legs" # "legs" | "rollers" self.skill_mode: str = "walk" # "walk"|"sitstand"|"roll"|"kickL"|"kickR"|"crouch"|"groundpick" self.sit_flag: int = 0 self.head_mode: bool = False self.vel_cmd = np.zeros(3, dtype=np.float32) # [vx, 0, wz] self.head_target = np.zeros(4, dtype=np.float32) self.head_smooth = np.zeros(4, dtype=np.float32) self.soft_start_q0 = np.zeros(14, dtype=np.float64) self.soft_start_elapsed = 0.0 self.soft_start_duration = 1.5 self.sit_delay_rem: float | None = None self.stand_delay_rem: float | None = None self.kick_steps: int | None = None self.post_kick_lock: int = 0 self.crouch_phase: float | None = None self.pick_phase: float | None = None self.roll_steps: int | None = None self.roll_tipped: bool = False self.script_t: float | None = None self.fall_debounce: int = 0 self.recovery_state: str | None = None # None | "fallen" | "recovering" self.recovery_steps: int = 0 self.recovery_upright_steps: int = 0 def _load_params(self, manifest: dict[str, Any]) -> None: safety = manifest.get("safety", {}) head = manifest.get("head", {}) legacy = manifest.get("legacy", {}) variants = manifest.get("variants", {}) self.tilt_z = float(safety.get("tilt_z", -0.5)) self.upright_z = float(safety.get("upright_z", -0.85)) self.fall_debounce_steps = int(safety.get("fall_debounce_steps", 10)) self.fall_settle_steps = int(safety.get("fall_settle_steps", 15)) self.recover_upright_steps_req = int(safety.get("recover_upright_steps", 50)) self.recover_giveup_steps = int(safety.get("recover_giveup_steps", 300)) self.kick_total_steps = int(safety.get("kick_steps", 25)) self.post_kick_lock_total = int(safety.get("post_kick_lock_steps", 20)) self.roll_max_steps = int(safety.get("roll_max_steps", 150)) self.sit_delay_s = float(safety.get("sit_delay_s", 0.8)) self.stand_delay_s = float(safety.get("stand_delay_s", 2.0)) self.head_max = float(head.get("max_rad", 2.5)) self.head_alpha = float(head.get("alpha", 0.2)) self.head_signs = np.asarray(head.get("signs", [1, -1, 1, -1]), dtype=np.float32) self.crouch_period_s = float(legacy.get("crouch_period", 5.0)) self.crouch_end_phase = float(legacy.get("crouch_end", 0.7)) self.groundpick_period_s = float(legacy.get("groundpick_period", 4.0)) self.groundpick_end_phase = float(legacy.get("groundpick_end", 0.7)) self.legs_limits = list(variants.get("legs", {}).get("limits", [0.25, -0.2, 1.0])) self.rollers_limits = list(variants.get("rollers", {}).get("limits", [0.6, -0.5, 0.3])) self.rollers_enabled = bool(variants.get("rollers", {}).get("enabled", False)) custom_move = manifest.get("custom_move") self.custom_slot = custom_move.get("slot") if isinstance(custom_move, dict) else None roll_skill = manifest.get("skills", {}).get("roll", {}) ctrl_dt = float(manifest.get("sim", {}).get("ctrl_dt", 0.02)) if self.custom_slot == "trick" and roll_skill.get("duration_s"): self.roll_max_steps = max(10, round(float(roll_skill["duration_s"]) / ctrl_dt)) def update_manifest(self, manifest: dict[str, Any], custom_script: dict[str, Any] | None = None) -> None: self.manifest = manifest self.custom_script = custom_script self._load_params(manifest) if not self.rollers_enabled and self.loco == "rollers": self.loco = "legs" def _clear_skill_runs(self) -> None: self.sit_delay_rem = None self.stand_delay_rem = None self.kick_steps = None self.post_kick_lock = 0 self.crouch_phase = None self.pick_phase = None self.roll_steps = None self.roll_tipped = False self.script_t = None self.fall_debounce = 0 self.recovery_state = None self.recovery_steps = 0 self.recovery_upright_steps = 0 def emergency_stop(self) -> None: self._clear_skill_runs() self.skill_mode = "walk" self.sit_flag = 0 self.head_mode = False self.vel_cmd.fill(0) self.head_target.fill(0) self.head_smooth.fill(0) self.lifecycle = LifecycleState.IDLE def enter_calibration(self) -> None: self.emergency_stop() self.lifecycle = LifecycleState.CALIBRATION def exit_calibration(self) -> None: if self.lifecycle == LifecycleState.CALIBRATION: self.lifecycle = LifecycleState.IDLE def request_stand(self, current_q_rad: np.ndarray, soft_start_s: float = 1.5) -> None: self._clear_skill_runs() self.skill_mode = "walk" self.sit_flag = 0 self.head_mode = False self.vel_cmd.fill(0) self.head_target.fill(0) self.head_smooth.fill(0) self.soft_start_q0 = np.asarray(current_q_rad, dtype=np.float64).copy() self.soft_start_elapsed = 0.0 self.soft_start_duration = max(0.02, float(soft_start_s)) self.lifecycle = LifecycleState.SOFT_START def set_velocity_cmd(self, norm_x: float, norm_wz: float, speed_scale: float = 1.0) -> None: lim_f, lim_b, lim_a = self.rollers_limits if self.loco == "rollers" else self.legs_limits nx = max(-1.0, min(1.0, float(norm_x))) nwz = max(-1.0, min(1.0, float(norm_wz))) scale = max(0.05, min(1.0, float(speed_scale))) vx = (nx * lim_f if nx >= 0.0 else nx * (-lim_b)) * scale wz = nwz * lim_a * scale self.vel_cmd[0] = vx self.vel_cmd[1] = 0.0 self.vel_cmd[2] = wz def set_head_target(self, neck_pitch: float, head_pitch: float, head_yaw: float, head_roll: float) -> None: raw = np.array([neck_pitch, head_pitch, head_yaw, head_roll], dtype=np.float32) raw = np.clip(raw, -1.0, 1.0) self.head_target = self.head_signs * raw * self.head_max def _is_one_shot_busy(self) -> bool: return ( (self.skill_mode == "roll" and self.roll_steps is not None) or (self.skill_mode in ("kickL", "kickR") and self.kick_steps is not None) or (self.skill_mode == "crouch" and self.crouch_phase is not None) or (self.skill_mode == "groundpick" and self.pick_phase is not None) ) def trigger_action(self, action: str) -> bool: if self.lifecycle != LifecycleState.ACTIVE or self.recovery_state is not None: return False if action == "headToggle": if self.head_mode: self.head_mode = False return True if ( self.skill_mode in ("walk", "sitstand") and self.post_kick_lock == 0 and self.stand_delay_rem is None ): self.head_mode = True return True return False if action == "locoToggle": if not self.rollers_enabled: return False if self._is_one_shot_busy() or self.stand_delay_rem is not None: return False self._clear_skill_runs() self.skill_mode = "walk" self.sit_flag = 0 self.head_mode = False self.loco = "rollers" if self.loco == "legs" else "legs" return True if action == "sitToggle": if self.loco != "legs": return self.trigger_action("crouch") if self.custom_slot == "trick": return self.trigger_action("roll") if self.custom_slot == "script" and self.custom_script is not None: return self.trigger_action("scriptToggle") if self._is_one_shot_busy(): return False self.head_mode = False if self.skill_mode == "sitstand" and self.sit_flag == 1: self.sit_flag = 0 self.sit_delay_rem = None self.stand_delay_rem = self.stand_delay_s return True if self.skill_mode == "walk" and self.stand_delay_rem is None: self.skill_mode = "sitstand" self.sit_flag = 0 self.sit_delay_rem = self.sit_delay_s return True return False if action == "scriptToggle": if self.custom_script is None or self.loco != "legs": return False if self.script_t is not None: self.script_t = None return True if self.skill_mode == "walk" and self.stand_delay_rem is None: self.head_mode = False self.script_t = 0.0 return True return False if action in ("kickL", "kickR"): if self.loco != "legs" or self.skill_mode != "walk" or self.stand_delay_rem is not None: return False self.head_mode = False self.sit_delay_rem = None self.skill_mode = action self.sit_flag = 0 self.kick_steps = 0 return True if action == "groundPick": if self.loco != "legs" or self.skill_mode != "walk" or self.stand_delay_rem is not None: return False self.head_mode = False self.sit_delay_rem = None self.skill_mode = "groundpick" self.sit_flag = 0 self.pick_phase = 0.0 return True if action == "roll": if self.loco == "rollers": return self.trigger_action("crouch") if self.skill_mode != "walk" or self.stand_delay_rem is not None: return False self.head_mode = False self.sit_delay_rem = None self.skill_mode = "roll" self.sit_flag = 0 self.roll_steps = 0 self.roll_tipped = False return True if action == "crouch": if self.loco != "rollers" or self.skill_mode != "walk": return False self.head_mode = False self.skill_mode = "crouch" self.crouch_phase = 0.0 return True return False def _active_policy_key(self) -> str | None: if self.recovery_state == "fallen": return None if self.recovery_state == "recovering": return "stand" if self.loco == "rollers" and self.skill_mode == "walk": return "drive" return self.skill_mode def _mode_label(self) -> str: if self.lifecycle == LifecycleState.IDLE: return "Idle (Torque Off)" if self.lifecycle == LifecycleState.CALIBRATION: return "Calibration" if self.lifecycle == LifecycleState.FALL_SAFE: return "Fall Safe" if self.lifecycle == LifecycleState.SOFT_START: return "Soft Start" if self.recovery_state: return "Recovery" if self.skill_mode == "roll": return "Trick" if self.custom_slot == "trick" else "Roll" if self.script_t is not None: return "Script" if self.skill_mode == "crouch": return "Crouch" if self.skill_mode == "groundpick": return "Pick" if self.skill_mode in ("kickL", "kickR"): return "Kick" if self.head_mode: return "Head" if self.skill_mode == "sitstand" and self.sit_flag == 1: return "Sit" return "Drive" if self.loco == "rollers" else "Run" def _build_command_13(self) -> np.ndarray: cmd = np.zeros(13, dtype=np.float32) if self.skill_mode == "sitstand": cmd[0] = float(self.sit_flag) elif self.skill_mode == "crouch" and self.crouch_phase is not None: a = 2.0 * math.pi * self.crouch_phase cmd[0] = math.cos(a) cmd[1] = math.sin(a) elif self.skill_mode == "groundpick" and self.pick_phase is not None: a = 2.0 * math.pi * self.pick_phase cmd[0] = math.cos(a) cmd[1] = math.sin(a) elif not ( self.head_mode or self.skill_mode == "roll" or self.skill_mode in ("kickL", "kickR") or self.post_kick_lock > 0 or self.recovery_state is not None ): cmd[0:3] = self.vel_cmd for h in range(4): self.head_smooth[h] += self.head_alpha * (self.head_target[h] - self.head_smooth[h]) gp_zero = self.skill_mode == "groundpick" or self.recovery_state is not None if not gp_zero: cmd[3:7] = self.head_smooth if ( self.script_t is not None and self.skill_mode == "walk" and self.recovery_state is None and self.custom_script is not None ): cmd = script_command_at(self.custom_script, self.script_t, 13) return cmd def step( self, dt: float, proj_grav_z: float, auto_stand_on_fall: bool = False, ) -> StepDecision: zero_last_action = False if self.lifecycle in (LifecycleState.IDLE, LifecycleState.CALIBRATION, LifecycleState.FALL_SAFE): return StepDecision( lifecycle=self.lifecycle, skill_mode=self.skill_mode, policy_key=None, torque_enabled=False, soft_start_alpha=0.0, command_13=np.zeros(13, dtype=np.float32), zero_last_action=False, mode_label=self._mode_label(), recovery_state=self.recovery_state, loco=self.loco, head_mode=self.head_mode, ) if self.lifecycle == LifecycleState.SOFT_START: self.soft_start_elapsed += dt u = min(1.0, self.soft_start_elapsed / self.soft_start_duration) alpha = 0.5 * (1.0 - math.cos(math.pi * u)) if u < 1.0 - 1e-6: return StepDecision( lifecycle=self.lifecycle, skill_mode="soft_start", policy_key=None, torque_enabled=True, soft_start_alpha=alpha, command_13=np.zeros(13, dtype=np.float32), zero_last_action=True, mode_label=self._mode_label(), recovery_state=None, loco=self.loco, head_mode=False, ) self.lifecycle = LifecycleState.ACTIVE self.skill_mode = "walk" zero_last_action = True # Active lifecycle: advance timers before fall/recovery and command build if self.sit_delay_rem is not None: self.sit_delay_rem -= dt if self.sit_delay_rem <= 0.0: self.sit_delay_rem = None if self.skill_mode == "sitstand": self.sit_flag = 1 if self.stand_delay_rem is not None: self.stand_delay_rem -= dt if self.stand_delay_rem <= 0.0: self.stand_delay_rem = None self.skill_mode = "walk" zero_last_action = True # Fall detection & recovery state machine fallen_now = (not math.isfinite(proj_grav_z)) or (proj_grav_z > self.tilt_z) if self.recovery_state == "fallen": self.recovery_steps += 1 if self.recovery_steps >= self.fall_settle_steps: self.recovery_state = "recovering" self.recovery_steps = 0 self.recovery_upright_steps = 0 zero_last_action = True elif self.recovery_state == "recovering": self.recovery_steps += 1 if proj_grav_z < self.upright_z: self.recovery_upright_steps += 1 else: self.recovery_upright_steps = 0 if self.recovery_upright_steps >= self.recover_upright_steps_req: self.recovery_state = None self.recovery_steps = 0 self.recovery_upright_steps = 0 self.skill_mode = "walk" zero_last_action = True elif self.recovery_steps >= self.recover_giveup_steps: self._clear_skill_runs() self.lifecycle = LifecycleState.FALL_SAFE return StepDecision( lifecycle=self.lifecycle, skill_mode="walk", policy_key=None, torque_enabled=False, soft_start_alpha=0.0, command_13=np.zeros(13, dtype=np.float32), zero_last_action=True, mode_label=self._mode_label(), recovery_state=None, loco=self.loco, head_mode=False, ) elif fallen_now: recoverable = ( auto_stand_on_fall and self.loco == "legs" and self.skill_mode == "walk" and self.post_kick_lock == 0 and self.stand_delay_rem is None ) self.fall_debounce += 1 if self.fall_debounce >= self.fall_debounce_steps: self.fall_debounce = 0 self.head_mode = False if recoverable: self.recovery_state = "fallen" self.recovery_steps = 0 else: self._clear_skill_runs() self.lifecycle = LifecycleState.FALL_SAFE return StepDecision( lifecycle=self.lifecycle, skill_mode="walk", policy_key=None, torque_enabled=False, soft_start_alpha=0.0, command_13=np.zeros(13, dtype=np.float32), zero_last_action=True, mode_label=self._mode_label(), recovery_state=None, loco=self.loco, head_mode=False, ) else: self.fall_debounce = 0 policy_key = self._active_policy_key() command_13 = self._build_command_13() # Advance step-counted skills AFTER choosing this tick's policy/command if self.post_kick_lock > 0 and self.skill_mode == "walk": self.post_kick_lock -= 1 if self.skill_mode in ("kickL", "kickR") and self.kick_steps is not None: self.kick_steps += 1 if self.kick_steps >= self.kick_total_steps: self.kick_steps = None self.skill_mode = "walk" self.post_kick_lock = self.post_kick_lock_total if self.skill_mode == "crouch" and self.crouch_phase is not None: self.crouch_phase += dt / self.crouch_period_s if self.crouch_phase >= self.crouch_end_phase: self.crouch_phase = None self.skill_mode = "walk" if self.skill_mode == "groundpick" and self.pick_phase is not None: self.pick_phase += dt / self.groundpick_period_s if self.pick_phase >= self.groundpick_end_phase: self.pick_phase = None self.skill_mode = "walk" if self.skill_mode == "roll" and self.roll_steps is not None: self.roll_steps += 1 if proj_grav_z > -0.3: self.roll_tipped = True upright = proj_grav_z < self.upright_z done = self.roll_tipped and upright and self.roll_steps >= 40 expired = self.roll_steps >= self.roll_max_steps if done or expired: self.roll_steps = None self.roll_tipped = False self.skill_mode = "walk" zero_last_action = True if not upright: self.lifecycle = LifecycleState.FALL_SAFE if self.script_t is not None: self.script_t += dt if self.custom_script is None: self.script_t = None else: dur = float(self.custom_script.get("durationS") or self.custom_script.get("duration_s") or 0.0) loop = self.custom_script.get("loop", True) is not False if not loop and self.script_t >= dur: self.script_t = None return StepDecision( lifecycle=self.lifecycle, skill_mode=self.skill_mode, policy_key=policy_key, torque_enabled=self.lifecycle == LifecycleState.ACTIVE, soft_start_alpha=1.0, command_13=command_13, zero_last_action=zero_last_action, mode_label=self._mode_label(), recovery_state=self.recovery_state, loco=self.loco, head_mode=self.head_mode, )