562 lines
22 KiB
Python
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,
|
|
)
|