Files
VibemakerSandbox/hardware/unoq/python/core/config.py
T

177 lines
7.2 KiB
Python

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