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"])