feat: add Microduck UNO Q deployment and hotspot skill control
This commit is contained in:
@@ -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
|
||||
Reference in New Issue
Block a user