feat: add Microduck UNO Q deployment and hotspot skill control

This commit is contained in:
2026-11-19 13:49:08 +08:00
parent 2b0c395508
commit 164a397726
99 changed files with 12488 additions and 4 deletions
+1
View File
@@ -0,0 +1 @@
"""Core configuration, observation pipeline, ONNX runner, and control loop."""
+176
View File
@@ -0,0 +1,176 @@
from __future__ import annotations
import json
import math
from pathlib import Path
from typing import Any
import numpy as np
TWO_PI = 2.0 * math.pi
def validate_robot_manifest(manifest: dict[str, Any]) -> dict[str, Any]:
if not isinstance(manifest, dict):
raise ValueError("robot_manifest must be a JSON object")
if manifest.get("obs_size") != 61:
raise ValueError(f"Unsupported obs_size {manifest.get('obs_size')}; expected 61")
if manifest.get("action_size") != 14:
raise ValueError(f"Unsupported action_size {manifest.get('action_size')}; expected 14")
if manifest.get("command_size") != 13:
raise ValueError(f"Unsupported command_size {manifest.get('command_size')}; expected 13")
joint_names = manifest.get("joint_names")
default_pose = manifest.get("default_pose")
if not isinstance(joint_names, list) or len(joint_names) != 14:
raise ValueError("robot_manifest.joint_names must contain 14 joints")
if not isinstance(default_pose, list) or len(default_pose) != 14:
raise ValueError("robot_manifest.default_pose must contain 14 floats")
if not all(isinstance(v, (int, float)) and math.isfinite(float(v)) for v in default_pose):
raise ValueError("robot_manifest.default_pose contains non-finite value")
skills = manifest.get("skills")
if not isinstance(skills, dict):
raise ValueError("robot_manifest.skills must be an object")
for required in ("walk", "stand"):
if required not in skills or not isinstance(skills[required], dict) or not skills[required].get("path"):
raise ValueError(f"robot_manifest.skills missing required '{required}' policy")
return manifest
def load_robot_manifest(path: str | Path) -> dict[str, Any]:
with Path(path).open("r", encoding="utf-8") as f:
data = json.load(f)
return validate_robot_manifest(data)
def validate_hardware_profile(profile: dict[str, Any], joint_names: list[str] | None = None) -> dict[str, Any]:
if not isinstance(profile, dict):
raise ValueError("hardware_profile must be a JSON object")
servo = profile.get("servo")
if not isinstance(servo, dict):
raise ValueError("hardware_profile.servo missing")
ticks_per_rev = int(servo.get("ticks_per_rev", 0))
if ticks_per_rev <= 0:
raise ValueError("hardware_profile.servo.ticks_per_rev must be > 0")
imu = profile.get("imu")
if not isinstance(imu, dict):
raise ValueError("hardware_profile.imu missing")
axis_map = np.asarray(imu.get("axis_map", []), dtype=np.float64)
if axis_map.shape != (3, 3) or not np.all(np.isfinite(axis_map)):
raise ValueError("hardware_profile.imu.axis_map must be a 3x3 finite matrix")
joints = profile.get("joints")
if not isinstance(joints, dict):
raise ValueError("hardware_profile.joints missing")
expected = joint_names or list(joints.keys())
if len(expected) != 14:
raise ValueError("hardware_profile.joints must configure all 14 joints")
seen_ids: set[int] = set()
for name in expected:
if name not in joints:
raise ValueError(f"hardware_profile.joints missing '{name}'")
cfg = joints[name]
jid = int(cfg["id"])
if not (0 <= jid <= 253) or jid in seen_ids:
raise ValueError(f"Invalid or duplicate servo id {jid} for joint {name}")
seen_ids.add(jid)
if int(cfg["sign"]) not in (-1, 1):
raise ValueError(f"Joint {name} sign must be -1 or 1")
if float(cfg["min_rad"]) > float(cfg["max_rad"]):
raise ValueError(f"Joint {name} min_rad > max_rad")
return profile
def load_hardware_profile(path: str | Path, joint_names: list[str] | None = None) -> dict[str, Any]:
with Path(path).open("r", encoding="utf-8") as f:
data = json.load(f)
return validate_hardware_profile(data, joint_names=joint_names)
def save_hardware_profile(path: str | Path, profile: dict[str, Any], joint_names: list[str] | None = None) -> dict[str, Any]:
validated = validate_hardware_profile(profile, joint_names=joint_names)
target = Path(path)
target.parent.mkdir(parents=True, exist_ok=True)
tmp = target.with_suffix(".json.tmp")
with tmp.open("w", encoding="utf-8") as f:
json.dump(validated, f, indent=2, ensure_ascii=False)
f.write("\n")
tmp.replace(target)
return validated
def rad_to_ticks(q_rad: np.ndarray | list[float], joint_names: list[str], profile: dict[str, Any]) -> list[int]:
servo = profile["servo"]
ticks_per_rev = int(servo["ticks_per_rev"])
neutral = int(servo["neutral_tick"])
rad_per_tick = TWO_PI / ticks_per_rev
joints_cfg = profile["joints"]
max_tick = ticks_per_rev - 1
out: list[int] = []
for idx, name in enumerate(joint_names):
cfg = joints_cfg[name]
angle = float(q_rad[idx])
if not math.isfinite(angle):
raise ValueError(f"Non-finite angle for joint {name}")
angle = max(float(cfg["min_rad"]), min(float(cfg["max_rad"]), angle))
tick = round(neutral + int(cfg["zero_offset_ticks"]) + int(cfg["sign"]) * (angle / rad_per_tick))
tick = max(0, min(max_tick, tick))
out.append(int(tick))
return out
def ticks_to_rad(ticks: np.ndarray | list[int], joint_names: list[str], profile: dict[str, Any]) -> np.ndarray:
servo = profile["servo"]
ticks_per_rev = int(servo["ticks_per_rev"])
neutral = int(servo["neutral_tick"])
rad_per_tick = TWO_PI / ticks_per_rev
joints_cfg = profile["joints"]
out = np.zeros(len(joint_names), dtype=np.float64)
for idx, name in enumerate(joint_names):
cfg = joints_cfg[name]
zero = neutral + int(cfg["zero_offset_ticks"])
out[idx] = int(cfg["sign"]) * (float(ticks[idx]) - zero) * rad_per_tick
return out
def raw_vel_to_rad_s(raw_vel: list[int], joint_names: list[str], profile: dict[str, Any]) -> np.ndarray:
"""Decode Feetech signed-magnitude speed ticks (bit 15 = negative) to rad/s."""
servo = profile["servo"]
scale = float(servo.get("vel_scale_rad_s_per_unit", TWO_PI / int(servo["ticks_per_rev"])))
joints_cfg = profile["joints"]
out = np.zeros(len(joint_names), dtype=np.float64)
for idx, name in enumerate(joint_names):
raw = int(raw_vel[idx])
mag = raw & 0x7FFF
signed = -mag if (raw & 0x8000) else mag
out[idx] = int(joints_cfg[name]["sign"]) * signed * scale
return out
def apply_imu_calibration(
gyro_raw: np.ndarray | list[float],
grav_raw: np.ndarray | list[float],
profile: dict[str, Any],
) -> tuple[np.ndarray, np.ndarray]:
imu = profile["imu"]
axis_map = np.asarray(imu["axis_map"], dtype=np.float64)
gyro_bias = np.asarray(imu.get("gyro_bias_rad_s", [0.0, 0.0, 0.0]), dtype=np.float64)
g_in = np.asarray(gyro_raw, dtype=np.float64)
p_in = np.asarray(grav_raw, dtype=np.float64)
if g_in.shape != (3,) or p_in.shape != (3,):
raise ValueError("IMU vectors must be 3D")
if not (np.all(np.isfinite(g_in)) and np.all(np.isfinite(p_in))):
raise ValueError("IMU readings contain non-finite values")
gyro = axis_map @ (g_in - gyro_bias)
grav = axis_map @ p_in
norm = float(np.linalg.norm(grav))
if norm > 1e-6:
grav = grav / norm
return gyro, grav
+380
View File
@@ -0,0 +1,380 @@
from __future__ import annotations
import threading
import time
from pathlib import Path
from typing import Any
import numpy as np
from hal.base import HardwareBridge, HardwareFrame
from .config import (
apply_imu_calibration,
rad_to_ticks,
raw_vel_to_rad_s,
save_hardware_profile,
ticks_to_rad,
)
from .observations import build_observation, decode_action
from .policy_runner import PolicySet
from .state_machine import LifecycleState, MicroduckStateMachine
class MicroduckController:
"""50Hz real-time control loop coordinating HAL, 61D observations, ONNX policies, and safety."""
def __init__(
self,
base_dir: str | Path,
manifest: dict[str, Any],
profile: dict[str, Any],
hal: HardwareBridge,
policies: PolicySet,
custom_script: dict[str, Any] | None = None,
profile_path: str | Path | None = None,
) -> None:
self.base_dir = Path(base_dir)
self.profile_path = Path(profile_path) if profile_path else self.base_dir / "config" / "hardware_profile.json"
self.manifest = manifest
self.profile = profile
self.hal = hal
self.policies = policies
self.joint_names: list[str] = list(manifest["joint_names"])
self.default_pose = np.asarray(manifest["default_pose"], dtype=np.float32)
self.ctrl_dt = float(manifest.get("sim", {}).get("ctrl_dt", 0.02))
self.state_machine = MicroduckStateMachine(manifest, custom_script=custom_script)
self._lock = threading.RLock()
self._running = False
self._thread: threading.Thread | None = None
self.last_action = np.zeros(14, dtype=np.float32)
self._last_q_rad: np.ndarray | None = None
self._vel_lpf = np.zeros(14, dtype=np.float64)
self._last_remote_input_s: float = 0.0
self._last_frame: HardwareFrame | None = None
self._last_gyro = np.zeros(3, dtype=np.float64)
self._last_grav = np.array([0.0, 0.0, -1.0], dtype=np.float64)
self._last_q = self.default_pose.astype(np.float64).copy()
self._last_targets = self.default_pose.astype(np.float64).copy()
self._ctrl_hz = 0.0
self._last_error: str | None = None
self._calib_target_ticks: list[int] | None = None
self._calib_torque_on = False
def connect(self) -> None:
with self._lock:
self.hal.connect()
self.hal.configure(self.joint_names, self.profile)
self._last_frame = self.hal.sync_step(
enable_torque=False,
target_ticks=rad_to_ticks(self.default_pose, self.joint_names, self.profile),
)
self._last_gyro, self._last_grav = apply_imu_calibration(
self._last_frame.gyro_raw, self._last_frame.proj_gravity_raw, self.profile
)
self._last_q = ticks_to_rad(self._last_frame.ticks, self.joint_names, self.profile)
def start(self) -> None:
if self._running:
return
self.connect()
self._running = True
self._thread = threading.Thread(target=self._loop, name="microduck-50hz", daemon=True)
self._thread.start()
def stop(self) -> None:
self._running = False
if self._thread is not None:
self._thread.join(timeout=2.0)
self._thread = None
with self._lock:
try:
self.state_machine.emergency_stop()
self.hal.set_torque(False)
finally:
self.hal.close()
def _loop(self) -> None:
next_t = time.monotonic()
hz_t0 = next_t
ticks = 0
while self._running:
now = time.monotonic()
try:
self.step_once(dt=self.ctrl_dt, now_s=now)
self._last_error = None
except Exception as exc:
self._last_error = str(exc)
with self._lock:
self.state_machine.emergency_stop()
try:
self.hal.set_torque(False)
except Exception:
pass
ticks += 1
if now - hz_t0 >= 0.5:
self._ctrl_hz = ticks / max(now - hz_t0, 1e-6)
ticks = 0
hz_t0 = now
next_t += self.ctrl_dt
sleep_s = next_t - time.monotonic()
if sleep_s > 0:
time.sleep(sleep_s)
else:
next_t = time.monotonic()
def set_remote_input(
self,
norm_x: float,
norm_wz: float,
speed_scale: float | None = None,
now_s: float | None = None,
) -> None:
with self._lock:
scale = (
float(speed_scale)
if speed_scale is not None
else float(self.profile.get("safety", {}).get("cmd_vel_scale", 0.5))
)
self.state_machine.set_velocity_cmd(norm_x, norm_wz, speed_scale=scale)
self._last_remote_input_s = time.monotonic() if now_s is None else float(now_s)
def set_head_target(self, neck_pitch: float, head_pitch: float, head_yaw: float, head_roll: float) -> None:
with self._lock:
self.state_machine.set_head_target(neck_pitch, head_pitch, head_yaw, head_roll)
def set_auto_stand_on_fall(self, enabled: bool) -> None:
with self._lock:
self.profile.setdefault("safety", {})["auto_stand_on_fall"] = bool(enabled)
save_hardware_profile(self.profile_path, self.profile, self.joint_names)
def set_speed_scale(self, scale: float) -> None:
with self._lock:
clamped = max(0.1, min(1.0, float(scale)))
self.profile.setdefault("safety", {})["cmd_vel_scale"] = clamped
save_hardware_profile(self.profile_path, self.profile, self.joint_names)
def request_stand(self) -> None:
with self._lock:
if "walk" not in self.policies.policies:
raise RuntimeError("No control policies loaded yet — deploy a bundle from the website first")
self._calib_torque_on = False
self._calib_target_ticks = None
soft_start_s = float(self.profile.get("safety", {}).get("soft_start_s", 1.5))
self.state_machine.request_stand(self._last_q, soft_start_s=soft_start_s)
def emergency_stop(self) -> None:
with self._lock:
self._calib_torque_on = False
self._calib_target_ticks = None
self.state_machine.emergency_stop()
self.last_action.fill(0)
self.hal.set_torque(False)
def trigger_action(self, action: str) -> bool:
with self._lock:
return self.state_machine.trigger_action(action)
def reload_runtime(
self,
manifest: dict[str, Any],
policies: PolicySet,
profile: dict[str, Any] | None = None,
custom_script: dict[str, Any] | None = None,
) -> None:
with self._lock:
self.state_machine.emergency_stop()
self.hal.set_torque(False)
self.manifest = manifest
self.policies = policies
self.joint_names = list(manifest["joint_names"])
self.default_pose = np.asarray(manifest["default_pose"], dtype=np.float32)
self.ctrl_dt = float(manifest.get("sim", {}).get("ctrl_dt", 0.02))
self.state_machine.update_manifest(manifest, custom_script=custom_script)
if profile is not None:
self.profile = profile
self.hal.configure(self.joint_names, self.profile)
self.last_action.fill(0)
self._last_error = None
def update_profile(self, profile: dict[str, Any]) -> dict[str, Any]:
with self._lock:
self.profile = save_hardware_profile(self.profile_path, profile, self.joint_names)
self.hal.configure(self.joint_names, self.profile)
return self.profile
def capture_joint_zero(self, joint_name: str) -> dict[str, Any]:
with self._lock:
if joint_name not in self.joint_names:
raise ValueError(f"Unknown joint '{joint_name}'")
idx = self.joint_names.index(joint_name)
if self._last_frame is None:
raise RuntimeError("No hardware frame captured yet")
neutral = int(self.profile["servo"]["neutral_tick"])
current_tick = int(self._last_frame.ticks[idx])
self.profile["joints"][joint_name]["zero_offset_ticks"] = current_tick - neutral
return self.update_profile(self.profile)
def test_single_joint(self, joint_name: str, target_rad: float) -> list[int]:
with self._lock:
if joint_name not in self.joint_names:
raise ValueError(f"Unknown joint '{joint_name}'")
if self.state_machine.lifecycle not in (LifecycleState.IDLE, LifecycleState.CALIBRATION):
raise RuntimeError("Emergency stop first before testing individual joints")
self.state_machine.enter_calibration()
idx = self.joint_names.index(joint_name)
pose = self._last_q.copy()
pose[idx] = float(target_rad)
ticks = rad_to_ticks(pose, self.joint_names, self.profile)
self._calib_target_ticks = ticks
self._calib_torque_on = True
return ticks
def exit_calibration(self) -> None:
with self._lock:
self._calib_torque_on = False
self._calib_target_ticks = None
self.state_machine.exit_calibration()
self.hal.set_torque(False)
def calibrate_imu_zero(self, samples: int = 200) -> dict[str, Any]:
with self._lock:
bias = self.hal.calibrate_imu_raw(samples=samples)
self.profile["imu"]["gyro_bias_rad_s"] = [float(b) for b in bias]
self.update_profile(self.profile)
return self.profile["imu"]
def ping_servos(self, max_id: int = 20) -> list[int]:
with self._lock:
return self.hal.ping_servos(max_id=max_id)
def _compute_joint_vel(self, q_rad: np.ndarray, raw_speeds: list[int], dt: float) -> np.ndarray:
servo_cfg = self.profile["servo"]
if servo_cfg.get("vel_source") == "servo":
return raw_vel_to_rad_s(raw_speeds, self.joint_names, self.profile)
if self._last_q_rad is None:
self._last_q_rad = q_rad.copy()
self._vel_lpf.fill(0.0)
return self._vel_lpf.copy()
fd = (q_rad - self._last_q_rad) / max(dt, 1e-4)
self._last_q_rad = q_rad.copy()
alpha = float(servo_cfg.get("vel_lpf_alpha", 0.4))
self._vel_lpf = (1.0 - alpha) * self._vel_lpf + alpha * fd
return self._vel_lpf.copy()
def step_once(self, dt: float | None = None, now_s: float | None = None) -> dict[str, Any]:
with self._lock:
dt_s = self.ctrl_dt if dt is None else float(dt)
now = time.monotonic() if now_s is None else float(now_s)
# Enforce mobile WS heartbeat timeout while walking
ws_timeout_s = float(self.profile.get("safety", {}).get("ws_timeout_ms", 500)) / 1000.0
if self._last_remote_input_s > 0.0 and (now - self._last_remote_input_s) > ws_timeout_s:
self.state_machine.set_velocity_cmd(0.0, 0.0)
servo_cfg = self.profile["servo"]
speed_limit = int(servo_cfg.get("speed_limit", 0))
acc = int(servo_cfg.get("acc", 50))
# Determine command to send for THIS cycle from current state + previous sensor state
auto_stand = bool(self.profile.get("safety", {}).get("auto_stand_on_fall", False))
if self.state_machine.lifecycle == LifecycleState.CALIBRATION:
enable_torque = self._calib_torque_on
cmd_ticks = (
list(self._calib_target_ticks)
if self._calib_target_ticks is not None
else rad_to_ticks(self._last_q, self.joint_names, self.profile)
)
decision = self.state_machine.step(dt_s, float(self._last_grav[2]), auto_stand)
else:
decision = self.state_machine.step(dt_s, float(self._last_grav[2]), auto_stand)
if decision.zero_last_action:
self.last_action.fill(0)
if decision.lifecycle == LifecycleState.SOFT_START:
q_interp = (
1.0 - decision.soft_start_alpha
) * self.state_machine.soft_start_q0 + decision.soft_start_alpha * self.default_pose
self._last_targets = q_interp
cmd_ticks = rad_to_ticks(q_interp, self.joint_names, self.profile)
enable_torque = True
elif decision.lifecycle == LifecycleState.ACTIVE and decision.policy_key is not None:
policy = self.policies.policies.get(decision.policy_key) or self.policies.policies["walk"]
q_vel = self._vel_lpf
obs = build_observation(
self._last_gyro,
self._last_grav,
self._last_q,
q_vel,
self.last_action,
decision.command_13,
self.default_pose,
)
raw_act = policy.infer(obs)
self.last_action = raw_act
targets = decode_action(raw_act, self.default_pose, policy.action_scale)
self._last_targets = targets.astype(np.float64)
cmd_ticks = rad_to_ticks(targets, self.joint_names, self.profile)
enable_torque = True
elif decision.lifecycle == LifecycleState.ACTIVE and decision.recovery_state == "fallen":
cmd_ticks = rad_to_ticks(self._last_targets, self.joint_names, self.profile)
enable_torque = True
else:
cmd_ticks = rad_to_ticks(self._last_q, self.joint_names, self.profile)
enable_torque = False
frame = self.hal.sync_step(
enable_torque=enable_torque,
target_ticks=cmd_ticks,
speed_limit=speed_limit,
acc=acc,
)
self._last_frame = frame
self._last_gyro, self._last_grav = apply_imu_calibration(
frame.gyro_raw, frame.proj_gravity_raw, self.profile
)
self._last_q = ticks_to_rad(frame.ticks, self.joint_names, self.profile)
self._compute_joint_vel(self._last_q, frame.raw_speeds, dt_s)
return self._build_status_unlocked(decision, enable_torque)
def _build_status_unlocked(self, decision: Any | None = None, torque_enabled: bool | None = None) -> dict[str, Any]:
frame = self._last_frame
life = self.state_machine.lifecycle.value
active_policy = decision.policy_key if decision else None
if torque_enabled is None:
torque_enabled = life in (LifecycleState.SOFT_START.value, LifecycleState.ACTIVE.value) or self._calib_torque_on
return {
"hal": self.hal.name,
"policy_backend": self.policies.backend,
"lifecycle": life,
"mode_label": decision.mode_label if decision else self.state_machine._mode_label(),
"skill_mode": self.state_machine.skill_mode,
"active_policy": active_policy,
"recovery_state": self.state_machine.recovery_state,
"loco": self.state_machine.loco,
"rollers_enabled": self.state_machine.rollers_enabled,
"head_mode": self.state_machine.head_mode,
"torque_enabled": bool(torque_enabled),
"auto_stand_on_fall": bool(self.profile.get("safety", {}).get("auto_stand_on_fall", False)),
"speed_scale": float(self.profile.get("safety", {}).get("cmd_vel_scale", 0.5)),
"ctrl_hz": round(self._ctrl_hz, 1),
"voltage_v": round(frame.voltage_v, 2) if frame else 0.0,
"max_temp_c": round(frame.max_temp_c, 1) if frame else 0.0,
"imu_ok": frame.imu_ok if frame else False,
"servo_err_mask": frame.servo_err_mask if frame else 0,
"gyro": [round(float(x), 4) for x in self._last_gyro],
"projected_gravity": [round(float(x), 4) for x in self._last_grav],
"ticks": list(frame.ticks) if frame else [2048] * 14,
"joint_pos_rad": [round(float(x), 4) for x in self._last_q],
"loaded_skills": sorted(self.policies.policies.keys()),
"custom_move": self.manifest.get("custom_move"),
"error": self._last_error,
}
def get_status(self) -> dict[str, Any]:
with self._lock:
return self._build_status_unlocked()
+252
View File
@@ -0,0 +1,252 @@
from __future__ import annotations
import io
import json
import shutil
import stat
import tempfile
import threading
import time
import zipfile
from pathlib import Path, PurePosixPath
from typing import Any
from .config import save_hardware_profile, validate_hardware_profile, validate_robot_manifest
from .controller import MicroduckController
from .policy_runner import PolicySet
MAX_ZIP_FILES = 512
MAX_ZIP_UNCOMPRESSED_BYTES = 128 * 1024 * 1024
def _safe_rel_path(member_name: str) -> PurePosixPath:
if not member_name or "\\" in member_name:
raise ValueError(f"Unsafe zip member path: {member_name!r}")
if member_name.startswith("/") or ":" in member_name:
raise ValueError(f"Absolute or drive path forbidden in zip: {member_name!r}")
parts = PurePosixPath(member_name).parts
if not parts or any(part in ("..", ".", "") for part in parts):
raise ValueError(f"Path traversal forbidden in zip: {member_name!r}")
return PurePosixPath(*parts)
class DeployService:
"""Atomic, validated hot-reload deployment of Microduck manifest + ONNX policies."""
def __init__(
self,
base_dir: str | Path,
controller: MicroduckController,
policy_backend: str = "auto",
) -> None:
self.base_dir = Path(base_dir)
self.controller = controller
self.policy_backend = policy_backend
self._lock = threading.Lock()
self.last_deploy: dict[str, Any] | None = self._read_last_deploy()
def _read_last_deploy(self) -> dict[str, Any] | None:
meta_path = self.base_dir / "config" / ".last_deploy.json"
if not meta_path.is_file():
return None
try:
with meta_path.open("r", encoding="utf-8") as f:
return json.load(f)
except Exception:
return None
def _extract_bundle_to_stage(self, raw_bytes: bytes, stage_dir: Path) -> None:
if not raw_bytes:
raise ValueError("Empty deployment bundle")
try:
zf = zipfile.ZipFile(io.BytesIO(raw_bytes), "r")
except zipfile.BadZipFile as exc:
raise ValueError("Deployment payload is not a valid ZIP archive") from exc
with zf:
infos = zf.infolist()
if len(infos) > MAX_ZIP_FILES:
raise ValueError(f"Too many files in deployment zip ({len(infos)} > {MAX_ZIP_FILES})")
total_size = 0
entries: list[tuple[zipfile.ZipInfo, PurePosixPath]] = []
for info in infos:
if info.is_dir():
if info.filename.rstrip("/"):
_safe_rel_path(info.filename.rstrip("/"))
continue
mode = (info.external_attr >> 16) & 0o170000
if mode == stat.S_IFLNK:
raise ValueError(f"Symlinks are forbidden in deployment zip: {info.filename!r}")
total_size += max(0, int(info.file_size))
if total_size > MAX_ZIP_UNCOMPRESSED_BYTES:
raise ValueError("Deployment zip exceeds maximum uncompressed size (128 MB)")
entries.append((info, _safe_rel_path(info.filename)))
if not entries:
raise ValueError("Deployment zip contains no files")
top_dirs = {p.parts[0] for _, p in entries if len(p.parts) > 1}
has_root_manifest = any(
str(p) in ("config/robot_manifest.json", "robot_manifest.json") for _, p in entries
)
strip_prefix: str | None = None
if not has_root_manifest and len(top_dirs) == 1:
only_top = next(iter(top_dirs))
if any(
str(p) in (f"{only_top}/config/robot_manifest.json", f"{only_top}/robot_manifest.json")
for _, p in entries
):
strip_prefix = only_top
for info, rel in entries:
if strip_prefix and rel.parts[0] == strip_prefix:
if len(rel.parts) == 1:
continue
rel = PurePosixPath(*rel.parts[1:])
dest = (stage_dir / Path(*rel.parts)).resolve()
if not dest.is_relative_to(stage_dir.resolve()):
raise ValueError(f"Escaping path rejected: {info.filename!r}")
dest.parent.mkdir(parents=True, exist_ok=True)
with zf.open(info, "r") as src, dest.open("wb") as dst:
shutil.copyfileobj(src, dst)
def apply_zip_bytes(self, raw_bytes: bytes, overwrite_profile: bool = False) -> dict[str, Any]:
with self._lock, tempfile.TemporaryDirectory(prefix="microduck_deploy_") as tmp:
stage = Path(tmp) / "stage"
backup = Path(tmp) / "backup"
stage.mkdir(parents=True, exist_ok=True)
backup.mkdir(parents=True, exist_ok=True)
self._extract_bundle_to_stage(raw_bytes, stage)
manifest_src = stage / "config" / "robot_manifest.json"
if not manifest_src.is_file():
alt = stage / "robot_manifest.json"
if alt.is_file():
manifest_src = alt
else:
raise ValueError("Deployment zip missing config/robot_manifest.json")
try:
manifest = validate_robot_manifest(json.loads(manifest_src.read_text(encoding="utf-8")))
except json.JSONDecodeError as exc:
raise ValueError(f"Invalid robot_manifest.json: {exc}") from exc
# Normalize into stage/config & verify every referenced ONNX exists & is non-empty
(stage / "config").mkdir(parents=True, exist_ok=True)
(stage / "config" / "robot_manifest.json").write_text(
json.dumps(manifest, indent=2) + "\n", encoding="utf-8"
)
stage_policies: dict[str, Path] = {}
for key, spec in manifest["skills"].items():
rel = spec.get("path")
if not rel:
continue
rel_posix = _safe_rel_path(str(rel))
pol_file = (stage / Path(*rel_posix.parts)).resolve()
if not pol_file.is_relative_to(stage.resolve()) or not pol_file.is_file():
raise ValueError(f"Deployment zip missing policy '{key}' at '{rel}'")
if pol_file.stat().st_size == 0:
raise ValueError(f"Policy '{key}' at '{rel}' is empty")
stage_policies[key] = pol_file
# Optional custom move script
custom_script: dict[str, Any] | None = None
script_src = stage / "config" / "custom_move_script.json"
if not script_src.is_file():
alt_script = stage / "custom_move_script.json"
if alt_script.is_file():
script_src = alt_script
if script_src.is_file():
try:
custom_script = json.loads(script_src.read_text(encoding="utf-8"))
except json.JSONDecodeError as exc:
raise ValueError(f"Invalid custom_move_script.json: {exc}") from exc
if not isinstance(custom_script, dict) or not isinstance(custom_script.get("keyframes"), list):
raise ValueError("custom_move_script.json must contain a 'keyframes' list")
elif isinstance(manifest.get("custom_move"), dict) and manifest["custom_move"].get("slot") == "script":
raise ValueError("Manifest references custom_move script, but config/custom_move_script.json is missing")
# Optional hardware profile
target_profile_path = self.base_dir / "config" / "hardware_profile.json"
incoming_profile: dict[str, Any] | None = None
profile_src = stage / "config" / "hardware_profile.json"
if not profile_src.is_file():
alt_prof = stage / "hardware_profile.json"
if alt_prof.is_file():
profile_src = alt_prof
if (overwrite_profile or not target_profile_path.is_file()) and profile_src.is_file():
try:
incoming_profile = validate_hardware_profile(
json.loads(profile_src.read_text(encoding="utf-8")),
joint_names=manifest["joint_names"],
)
except json.JSONDecodeError as exc:
raise ValueError(f"Invalid hardware_profile.json: {exc}") from exc
# Pre-validate ONNX graph & metadata inside stage before touching live files
PolicySet.load_from_manifest(stage, manifest, backend=self.policy_backend)
# Backup existing config + policies for atomic rollback
live_config = self.base_dir / "config"
live_policies = self.base_dir / "policies"
live_config.mkdir(parents=True, exist_ok=True)
live_policies.mkdir(parents=True, exist_ok=True)
for sub in ("config", "policies"):
src_dir = self.base_dir / sub
if src_dir.is_dir():
shutil.copytree(src_dir, backup / sub, dirs_exist_ok=True)
try:
shutil.copy2(stage / "config" / "robot_manifest.json", live_config / "robot_manifest.json")
target_script = live_config / "custom_move_script.json"
if custom_script is not None:
target_script.write_text(json.dumps(custom_script, indent=2) + "\n", encoding="utf-8")
elif target_script.is_file():
target_script.unlink()
for _, src_pol in stage_policies.items():
rel_to_stage = src_pol.relative_to(stage)
dst_pol = self.base_dir / rel_to_stage
dst_pol.parent.mkdir(parents=True, exist_ok=True)
shutil.copy2(src_pol, dst_pol)
if incoming_profile is not None:
save_hardware_profile(target_profile_path, incoming_profile, joint_names=manifest["joint_names"])
live_policies_set = PolicySet.load_from_manifest(
self.base_dir, manifest, backend=self.policy_backend
)
self.controller.reload_runtime(
manifest=manifest,
policies=live_policies_set,
profile=incoming_profile,
custom_script=custom_script,
)
except Exception:
# Roll back live config + policies on reload failure
for sub in ("config", "policies"):
bak_dir = backup / sub
live_dir = self.base_dir / sub
if bak_dir.is_dir():
shutil.copytree(bak_dir, live_dir, dirs_exist_ok=True)
raise
deploy_meta = {
"ok": True,
"deployed_at_s": round(time.time(), 3),
"robot_id": manifest.get("robot_id", "microduck"),
"loaded_skills": sorted(live_policies_set.policies.keys()),
"profile_overwritten": incoming_profile is not None,
"custom_move": manifest.get("custom_move"),
}
(live_config / ".last_deploy.json").write_text(
json.dumps(deploy_meta, indent=2) + "\n", encoding="utf-8"
)
self.last_deploy = deploy_meta
return deploy_meta
+85
View File
@@ -0,0 +1,85 @@
from __future__ import annotations
from typing import Any
import numpy as np
def build_observation(
gyro: np.ndarray,
projected_gravity: np.ndarray,
joint_pos_rad: np.ndarray,
joint_vel_rad_s: np.ndarray,
last_action: np.ndarray,
command_13: np.ndarray,
default_pose: np.ndarray,
) -> np.ndarray:
"""Assemble the exact 61D float32 observation expected by Microduck ONNX policies.
Layout:
[0:3] base_ang_vel (gyro, rad/s)
[3:6] projected_gravity (trunk_base frame, unit vector pointing down: [0,0,-1] upright)
[6:20] joint_pos_rel (joint_pos_rad - default_pose)
[20:34] joint_vel (rad/s)
[34:48] last_action (previous raw actor output)
[48:61] command (13D: [vx, vy, wz, head(4), body(6)])
"""
obs = np.empty(61, dtype=np.float32)
obs[0:3] = np.asarray(gyro, dtype=np.float32)
obs[3:6] = np.asarray(projected_gravity, dtype=np.float32)
obs[6:20] = np.asarray(joint_pos_rad, dtype=np.float32) - np.asarray(default_pose, dtype=np.float32)
obs[20:34] = np.asarray(joint_vel_rad_s, dtype=np.float32)
obs[34:48] = np.asarray(last_action, dtype=np.float32)
obs[48:61] = np.asarray(command_13, dtype=np.float32)
if not np.all(np.isfinite(obs)):
raise ValueError("Observation vector contains non-finite values")
return obs
def decode_action(
raw_action: np.ndarray,
default_pose: np.ndarray,
action_scale: float = 1.0,
) -> np.ndarray:
act = np.asarray(raw_action, dtype=np.float32)
if act.shape != (14,) or not np.all(np.isfinite(act)):
raise ValueError("raw_action must be a finite 14D vector")
return np.asarray(default_pose, dtype=np.float32) + np.float32(action_scale) * act
def script_command_at(script: dict[str, Any], t_s: float, cmd_size: int = 13) -> np.ndarray:
"""Linearly interpolate a custom move_script.json timeline at time t_s."""
keyframes = script.get("keyframes", [])
out = np.zeros(cmd_size, dtype=np.float32)
if not keyframes:
return out
duration_s = float(script.get("durationS") or script.get("duration_s") or 0.0)
loop = script.get("loop", True) is not False
tt = float(t_s)
if loop and duration_s > 0.0:
tt = ((tt % duration_s) + duration_s) % duration_s
def _pad(raw_cmd: list[float]) -> np.ndarray:
arr = np.zeros(cmd_size, dtype=np.float32)
n = min(cmd_size, len(raw_cmd))
if n > 0:
arr[:n] = np.asarray(raw_cmd[:n], dtype=np.float32)
return arr
if tt <= float(keyframes[0]["t"]):
return _pad(keyframes[0]["cmd"])
for i in range(len(keyframes) - 1):
a = keyframes[i]
b = keyframes[i + 1]
ta = float(a["t"])
tb = float(b["t"])
if ta <= tt <= tb:
u = (tt - ta) / max(tb - ta, 1e-6)
ca = _pad(a["cmd"])
cb = _pad(b["cmd"])
return (1.0 - u) * ca + u * cb
return _pad(keyframes[-1]["cmd"])
+123
View File
@@ -0,0 +1,123 @@
from __future__ import annotations
from dataclasses import dataclass
from pathlib import Path
from typing import Any
import numpy as np
@dataclass
class LoadedPolicy:
key: str
action_scale: float
backend: str
_session: Any | None = None
_input_name: str = "obs"
_output_name: str = "actions"
def infer(self, obs_61: np.ndarray) -> np.ndarray:
if obs_61.shape != (61,):
raise ValueError(f"Expected 61D observation, got {obs_61.shape}")
if self.backend == "dummy":
# Deterministic zero-action dummy policy for PC dry-run without onnxruntime.
return np.zeros(14, dtype=np.float32)
inp = np.asarray(obs_61, dtype=np.float32).reshape(1, 61)
out = self._session.run([self._output_name], {self._input_name: inp})[0]
act = np.asarray(out, dtype=np.float32).reshape(14)
if not np.all(np.isfinite(act)):
raise ValueError(f"Policy '{self.key}' returned non-finite action")
return act
class PolicySet:
def __init__(self, policies: dict[str, LoadedPolicy], backend: str) -> None:
self.policies = policies
self.backend = backend
@classmethod
def resolve_backend(cls, requested: str) -> str:
if requested in ("onnx", "dummy"):
return requested
if requested != "auto":
raise ValueError(f"Unknown policy backend: {requested}")
try:
import onnxruntime # noqa: F401
return "onnx"
except ImportError as exc:
raise RuntimeError(
"onnxruntime is not installed. Run 'bash scripts/install_deps.sh' on UNO Q, "
"or pass '--policy-backend dummy' for pre-hardware UI/protocol dry-run."
) from exc
@classmethod
def load_from_manifest(
cls,
base_dir: str | Path,
manifest: dict[str, Any],
backend: str = "auto",
) -> "PolicySet":
resolved_backend = cls.resolve_backend(backend)
ort = None
if resolved_backend == "onnx":
try:
import onnxruntime as ort_mod
ort = ort_mod
except ImportError as exc:
raise RuntimeError("onnxruntime is required for backend='onnx'") from exc
root = Path(base_dir)
default_scale = float(manifest.get("action_scale", 1.0))
skills = manifest.get("skills", {})
loaded: dict[str, LoadedPolicy] = {}
for key, spec in skills.items():
rel_path = spec.get("path")
if not rel_path:
continue
scale = float(spec.get("action_scale", default_scale))
if resolved_backend == "dummy":
loaded[key] = LoadedPolicy(key=key, action_scale=scale, backend="dummy")
continue
model_path = (root / rel_path).resolve()
if not model_path.is_file():
raise FileNotFoundError(f"Missing ONNX policy for '{key}': {model_path}")
sess_opts = ort.SessionOptions()
sess_opts.intra_op_num_threads = 2
sess = ort.InferenceSession(
str(model_path),
sess_options=sess_opts,
providers=["CPUExecutionProvider"],
)
inps = sess.get_inputs()
outs = sess.get_outputs()
if len(inps) != 1 or len(outs) != 1:
raise ValueError(f"Policy '{key}' must have 1 input / 1 output")
# Smoke validation mirroring custom-policy.js
smoke_obs = np.zeros((1, 61), dtype=np.float32)
smoke_out = sess.run([outs[0].name], {inps[0].name: smoke_obs})[0]
if tuple(np.asarray(smoke_out).shape) != (1, 14):
raise ValueError(
f"Policy '{key}' output shape {np.asarray(smoke_out).shape} != (1, 14)"
)
if not np.all(np.isfinite(smoke_out)):
raise ValueError(f"Policy '{key}' smoke output is non-finite")
loaded[key] = LoadedPolicy(
key=key,
action_scale=scale,
backend="onnx",
_session=sess,
_input_name=inps[0].name,
_output_name=outs[0].name,
)
if "walk" not in loaded or "stand" not in loaded:
raise ValueError("PolicySet must load at least 'walk' and 'stand'")
return cls(policies=loaded, backend=resolved_backend)
+561
View File
@@ -0,0 +1,561 @@
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,
)