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