Files
VibemakerSandbox/hardware/unoq/python/core/state_machine.py
T

562 lines
22 KiB
Python

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