feat: add Microduck UNO Q deployment and hotspot skill control
This commit is contained in:
@@ -0,0 +1 @@
|
||||
"""Core configuration, observation pipeline, ONNX runner, and control loop."""
|
||||
@@ -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
|
||||
@@ -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()
|
||||
@@ -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
|
||||
@@ -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"])
|
||||
@@ -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)
|
||||
@@ -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,
|
||||
)
|
||||
Reference in New Issue
Block a user