86 lines
2.9 KiB
Python
86 lines
2.9 KiB
Python
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"])
|