177 lines
7.2 KiB
Python
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
|