diff --git a/README.md b/README.md index 9d7aa369..e7468283 100644 --- a/README.md +++ b/README.md @@ -191,6 +191,8 @@ Each operation can be driven from the web control panel or the CLI: - [`tracker.*`](https://docs.almond.bot/cli/tracker) — Mantis tracker setup: bridge, identify, pair, install, and base-station / Ultimate checks - [`tune.pid`](https://docs.almond.bot/cli/tune-pid) - [`tune.friction`](https://docs.almond.bot/cli/tune-friction) +- [`tune.breakaway`](https://docs.almond.bot/cli/tune-breakaway) +- [`tune.a4`](https://docs.almond.bot/cli/tune-a4) - [`tune.gravity`](https://docs.almond.bot/cli/tune-gravity) - [`tune.factory`](https://docs.almond.bot/cli/tune-factory) - [`calibration.pull`](https://docs.almond.bot/cli/tune-factory#calibration-pull) diff --git a/almond_axol/cli/__init__.py b/almond_axol/cli/__init__.py index b53cfdc2..a9adb898 100644 --- a/almond_axol/cli/__init__.py +++ b/almond_axol/cli/__init__.py @@ -38,7 +38,8 @@ from .motor import set_config as motor_set_config from .tune import factory as tune_factory from .tune import filter as tune_filter -from .tune import friction, pid, repeatability +from .tune import a4 as tune_a4 +from .tune import breakaway, friction, pid, repeatability from .tune import gravity as tune_gravity from .tune import motion as tune_motion from .zed import driver as zed_driver @@ -165,6 +166,8 @@ def build_parser() -> argparse.ArgumentParser: jetson_setup.add_parser(subparsers) pid.add_parser(subparsers) friction.add_parser(subparsers) + breakaway.add_parser(subparsers) + tune_a4.add_parser(subparsers) tune_gravity.add_parser(subparsers) tune_factory.add_parser(subparsers) calibration_cmd.add_parser(subparsers) diff --git a/almond_axol/cli/can/setup.py b/almond_axol/cli/can/setup.py index ec47ccae..7dd6a386 100644 --- a/almond_axol/cli/can/setup.py +++ b/almond_axol/cli/can/setup.py @@ -81,6 +81,7 @@ CAN_LEFT, CAN_MANTIS_LEFT, CAN_MANTIS_RIGHT, + CAN_RESET_SCRIPT, CAN_RIGHT, Joint, ) @@ -1400,10 +1401,10 @@ def _identify_dual_adapter(serial: str, *, reset: bool = False) -> DualHubIdenti # a healthy first pass, but if either half is down recover the adapter # pair together before probing. Pair-wide ordering matters for this # dual-channel firmware: both channels go down before either comes up. - bring_up_interfaces( - ifaces, - force_cycle=reset or not all(iface_up(iface) for iface in ifaces), - ) + if reset or not all(iface_up(iface) for iface in ifaces): + _recover_hub_pair(ifaces) + else: + bring_up_interfaces(ifaces, force_cycle=False) except RuntimeError: return "silent" @@ -1429,7 +1430,7 @@ def _identify_dual_adapter(serial: str, *, reset: bool = False) -> DualHubIdenti " No identity response; recovering both CAN channels and retrying..." ) try: - bring_up_interfaces(ifaces, force_cycle=True) + _recover_hub_pair(ifaces) except RuntimeError: return "silent" return "silent" @@ -1940,6 +1941,109 @@ def _write_cron_script(profile: _Profile = _AXOL_PROFILE) -> None: print(" Done.") +def _reset_script_text() -> str: + """The arm hub's USB reset: clears what a link flap cannot, then brings up. + + ``gs_can_close()`` kills the host's in-flight URBs and sends the adapter a + mode reset, but the hub firmware keeps the frames it had already accepted + (up to ``GS_MAX_TX_URBS``, 10 per channel) and transmits them on the next + open: the kernel logs one "Unexpected unused echo id" per replayed frame. + Behind a stalled bus those are the session's last position commands, so a + flap only defers the jump to whatever brings the bus back. A USB port + reset re-enumerates the hub; the bring-up script then configures the + fresh interfaces. Only the arm hub is reset — the single-channel wheel + and chest adapters keep the plain flap the bring-up script gives them. + """ + left, right = _AXOL_PROFILE.left, _AXOL_PROFILE.right + return ( + f"#!/bin/bash\n" + f"# USB-reset the Almond Axol arm hub, then bring the CAN interfaces up.\n" + f"#\n" + f"# The hub firmware keeps frames it already accepted through a link\n" + f"# down/up and transmits them on the next open, so the purge after a\n" + f"# stalled bus resets the device instead of flapping its channels.\n" + f"# Written by `axol can.setup`; run by the realtime core's purge and by\n" + f"# can.setup's recovery (granted by `axol provision`).\n" + f"set -euo pipefail\n\n" + f'if [ "${{{_GLOBAL_LOCK_ENV}:-}}" != "1" ]; then\n' + f' exec 8<"{_GLOBAL_LOCK_FILE}"\n' + f" flock 8\n" + f"fi\n\n" + f"HUB=\n" + f"for IFACE in {left} {right}; do\n" + f' if [ -e "/sys/class/net/${{IFACE}}/device" ]; then\n' + f' HUB=$(readlink -f "/sys/class/net/${{IFACE}}/device/..")\n' + f" break\n" + f" fi\n" + f"done\n\n" + f'if [ -n "${{HUB}}" ] && [ -f "${{HUB}}/busnum" ]; then\n' + f' BUS=$(cat "${{HUB}}/busnum")\n' + f' DEV=$(cat "${{HUB}}/devnum")\n' + f" if command -v usbreset >/dev/null 2>&1 && " + f'usbreset "$(printf "%03d/%03d" "${{BUS}}" "${{DEV}}")"; then\n' + f' echo "arm hub ${{HUB##*/}}: USB reset"\n' + f" else\n" + f" # No usbreset (or it failed): deauthorize and reauthorize, which\n" + f" # unbinds gs_usb and re-enumerates the hub the same way.\n" + f' echo 0 > "${{HUB}}/authorized"\n' + f" sleep 0.5\n" + f' echo 1 > "${{HUB}}/authorized"\n' + f' echo "arm hub ${{HUB##*/}}: reauthorized"\n' + f" fi\n" + f" # gs_usb rebinds inside the reset; udev renames the new netdevs.\n" + f" for _ in $(seq 1 50); do\n" + f" if ip link show {left} >/dev/null 2>&1 " + f"&& ip link show {right} >/dev/null 2>&1; then\n" + f" break\n" + f" fi\n" + f" sleep 0.1\n" + f" done\n" + f"else\n" + f' echo "{left}/{right} not present — no hub to reset"\n' + f"fi\n\n" + f"# fd 8 (the global lock) survives the exec; the flag stops the bring-up\n" + f"# script from reopening it and deadlocking on itself.\n" + f'exec env {_GLOBAL_LOCK_ENV}=1 bash "{_AXOL_PROFILE.cron_script}"\n' + ) + + +def _write_reset_script() -> None: + """Install :func:`_reset_script_text` at :data:`CAN_RESET_SCRIPT`.""" + print(f"Writing CAN adapter reset script to {CAN_RESET_SCRIPT}...") + _ensure_root_lock_file(_GLOBAL_LOCK_FILE) + _install_privileged_script(CAN_RESET_SCRIPT, _reset_script_text()) + print(" Done.") + + +def _root_script_command(script: Path) -> list[str]: + """``bash script``, flagged when this thread already holds the global lock. + + The generated scripts take the global lock themselves; reopening it while + this thread owns it would deadlock. The unflagged form is the one + ``axol provision`` grants for non-interactive callers. + """ + command = ["bash", str(script)] + if int(getattr(_LOCK_LOCAL, "depth", 0)): + command = ["env", f"{_GLOBAL_LOCK_ENV}=1", *command] + return command + + +def _recover_hub_pair(channels: list[str]) -> None: + """Recover a silent dual hub: USB reset where installed, else a pair cycle. + + Every recovery cycle of the arm hub is also a moment its firmware can + release frames queued behind an earlier stall (seen 2026-09-22: the right + arm drove to the stalled session's last pose during ``can.setup``). The + reset script clears them first; other hubs, and hosts that have not been + set up with it yet, keep the paired down/up cycle. + """ + if CAN_RESET_SCRIPT.exists() and set(channels) == {_CAN_L, _CAN_R}: + print(" Arm hub: USB reset (drops frames the adapter still holds)...") + run_root(_root_script_command(CAN_RESET_SCRIPT), check=True) + return + bring_up_interfaces(channels, force_cycle=True) + + def _read_root_crontab() -> str: """Read root's crontab, distinguishing an empty table from inspection errors.""" result = run_root(["env", "LC_ALL=C", "crontab", "-l"]) @@ -2226,7 +2330,7 @@ def _bring_up_can_locked(profile: _Profile = _AXOL_PROFILE) -> None: run_root(script_command, check=True) for attempt in range(3): if attempt: - bring_up_interfaces([profile.left, profile.right], force_cycle=True) + _recover_hub_pair([profile.left, profile.right]) left, right = rx_alive_per_arm(profile) if left and right: print(f" Done — motors responding on both {noun}s.") @@ -2367,6 +2471,15 @@ def _flap_for_purge(channels: list[str]) -> None: ``can0``, a host that has never run ``can.setup`` — falls back to :func:`bring_up_interfaces`, which configures each channel explicitly. """ + if ( + CAN_RESET_SCRIPT.exists() + and set(channels) <= _SCRIPT_MANAGED_CHANNELS + and set(channels) & {_CAN_L, _CAN_R} + ): + # Frames on the arm hub may also be parked inside the adapter, where + # only a USB reset reaches them; the script then runs the bring-up. + run_root(["bash", str(CAN_RESET_SCRIPT)], check=True) + return if CAN_BRINGUP_SCRIPT.exists() and set(channels) <= _SCRIPT_MANAGED_CHANNELS: # The script takes its own locks, so this deliberately does not hold # the global setup lock: a caller that did would deadlock it. @@ -2459,6 +2572,7 @@ def _apply_setup( _validate_adapter_assignments(hub_serial, wheels_serial, chest_serial) _write_udev_rules(hub_serial, wheels_serial, chest_serial) _write_cron_script() + _write_reset_script() _write_hotplug_unit() _reload_udev() _rename_interfaces(hub_serial, wheels_serial, chest_serial) diff --git a/almond_axol/cli/tune/a4.py b/almond_axol/cli/tune/a4.py new file mode 100644 index 00000000..ad799f9f --- /dev/null +++ b/almond_axol/cli/tune/a4.py @@ -0,0 +1,1628 @@ +""" +axol tune.a4 + +Tune a MyActuator joint's **firmware position loop** (0xA4 absolute position +closed-loop) — the loop the realtime core drives when a joint's +``wire_mode`` is ``a4``. Streams a sine or constant-speed triangle target at +``--rate`` Hz with the firmware gains, speed cap and planner acceleration you +choose, reads the fine 0.01° position (0x92) every cycle, scores tracking and +smoothness, and saves the run for the diagnostics dashboard. + +Planner acceleration: ``0`` is the documented direct-tracking mode, and the +protocol maximum ``60000`` makes the planner finish each 200 Hz step inside +the tick — on the X6-P20 elbow the latter tracked a 3 deg/s triangle to +0.02° RMS with 4 ms lag against 0.23° / 74 ms for direct tracking. Values in +between re-plan every target and the joint barely moves. The value is +written *before* the mode-switch reset: on the elbow's 2025070202 firmware +a 0 written into a running position loop is silently ignored (the joint +holds and executes nothing), while the same 0 applied through the reset +works; non-zero values apply live on every firmware seen. + +Why a separate tool: ``tune.pid`` tunes the MIT impedance frame, whose gains +live in the host. Under 0xA4 the whole controller is the motor's own +position PI → speed PI → current loop, so the knobs are the firmware gains +(``position_kp/ki/kd``, ``speed_kp/ki``, ``current_kp/ki``), the 0xA4 speed +cap, and the position planner's acceleration (0 = direct PI tracking of the +stream; anything else re-plans every target and will not follow a stream). + +Safety: + +* Gains are written to **RAM** (0x31) unless ``--persist`` is given, and the + pre-run values are written back when the run ends (``--keep`` skips that + so a winner stays). The tuning flows reset a motor when they switch its + control mode, which also reloads ROM gains — so the gains are written + *after* the mode switch and homing, right before the wave. +* A buzz guard watches the fine position for high-frequency motion and the + reply current; past ``--buzz-abort`` degrees of >10 Hz content or + ``--iq-abort`` amps it restores the previous gains at once, holds, and + ends the run. Size the current limit for the pose: a loaded X8 shoulder + draws ~10 A holding gravity alone at -55°. Shoulder_1 at speed_kp 0.1 (3× stock) vibrated immediately + on 2026-09-18; start every sweep from the stock values in small steps. +* The joint under test holds position stiffly in this mode and will push + back against contact up to motor torque. Keep the workspace clear. + +Examples: + axol tune.a4 --r --joint shoulder_1 --center -35 --amp 10 --mode triangle --speed 3 + axol tune.a4 --r --joint shoulder_1 --mode sine --freq 0.3 --speed-kp 0.05 --speed-ki 0.0005 + axol tune.a4 --r --joint shoulder_1 --accel 0 --position-kp 0.02 --save-run --label "pkp 0.02" +""" + +from __future__ import annotations + +import argparse +import asyncio +import math +import struct +import time +from collections import deque +from typing import Any + +import numpy as np + +from ...constants import ARM_JOINTS, Joint +from ...motor import CanBus, ControlMode, Motor, MotorError +from ...motor.damiao import DamiaoMotor +from ...motor.myactuator import _MA_FW_V44_VERSION, _MA_PID_IDX, MyActuatorMotor +from ...robot.axol import arm_limits +from ...robot.config import position_wire_mode +from ...tuning import ( + JointFrameMotor, + joint_frame_motors, + log_to_series, + ramp_stages, + safe_limits, + save_run, + sine_metrics, + sweep_safety, +) +from ...tuning.runner import LiveStream, report_achieved_rate +from ...tuning.wrist_imu import WristImu, format_imu +from ..motor import add_side_and_channel_arguments, resolve_channel +from .friction import _home_all, _ramp_verified, _safe_torque_off, rest_target + +_MA_POS_CONTROL = 0xA4 +_MA_TF_CONTROL = 0x73 # V4.4: position control with torque feedforward +_MA_MULTI_TURN_ANGLE = 0x92 +_MA_STATUS2 = 0x9C # temperature, iq (0.01 A), speed (dps), angle +_MA_READ_ACCEL = 0x42 +_MA_WRITE_ACCEL = 0x43 +_MA_READ_GAIN = 0x30 +_MA_WRITE_GAIN_RAM = 0x31 +_MA_WRITE_GAIN_ROM = 0x32 +_FLASH_SETTLE_S = 0.3 + +#: Planner acceleration (dps/s, the protocol maximum) at which the firmware +#: completes each 200 Hz step's plan inside the tick, so a re-planning +#: position loop follows the stream instead of stalling on it. On the right +#: elbow (X6-P20) this tracked a 3 deg/s triangle to 0.02° RMS with 4 ms lag +#: — better than direct tracking (accel 0: 0.23°, 74 ms) — while 5000 dps/s +#: never finished a plan before the next target and the joint barely moved. +_ACCEL_STEP_FOLLOW = 60000 + +# Damiao (the wrists): the position-velocity mode is the same three-loop +# cascade with the gains in RAM registers (0x55 writes take effect at once, +# 0xAA stores them) — KP_APR / KI_APR position, KP_ASR / KI_ASR velocity — +# and its own trapezoidal profiler whose ACC / DEC registers (rad/s², DEC +# negative) shape every streamed target. Each 0x100+ID command (p_des, +# v_des cap, both float32 LE) is answered with the MIT feedback frame, so +# position comes back at 16 bits over ±PMAX (0.022° at 12.5 rad), no paired +# read needed. Register 0x50 (p_m) is the float position for held joints. +_DM_GAIN_REGS = {"speed_kp": 25, "speed_ki": 26, "position_kp": 27, "position_ki": 28} +_DM_REG_ACC = 4 +_DM_REG_DEC = 5 +_DM_REG_PM = 80 +_DM_REPLY_TIMEOUT_S = 0.02 + +GAIN_NAMES: tuple[str, ...] = tuple(_MA_PID_IDX) + +#: A joint held on its own firmware position loop while another joint runs +#: the wave should not move; past this (rad, ~1°) it let go or sagged. +_HOLD_DRIFT_TOL = math.radians(1.0) + +#: Position error (rad) past which the wave is abandoned — the loop is not +#: following at all (planner in profiled mode, or a runaway). +_ERR_ABORT = math.radians(20.0) +#: Window (s) the buzz guard evaluates high-frequency motion over. +_BUZZ_WINDOW_S = 0.1 + + +# --------------------------------------------------------------------------- +# Pure pieces (unit-tested) +# --------------------------------------------------------------------------- + + +def waveform( + mode: str, + center: float, + amp: float, + duration: float, + rate: float, + *, + freq: float = 0.5, + speed: float = 0.05, +) -> list[tuple[float, float, float]]: + """``(t, target, v_cmd)`` samples of a sine (``freq`` Hz) or a constant-speed + triangle (``speed`` rad/s, ``amp`` half-travel) about ``center`` (rad). + + Both start at ``center`` with zero velocity so the first command is the + hold pose the joint was ramped to. The triangle is the stick-slip probe: + the whole pass runs at one speed, so creep behaviour is not confined to + the sine's turnarounds. + """ + n = max(1, math.ceil(duration * rate)) + out: list[tuple[float, float, float]] = [] + if mode == "sine": + w = 2.0 * math.pi * freed(freq) + for k in range(n): + t = k / rate + out.append((t, center + amp * math.sin(w * t), amp * w * math.cos(w * t))) + return out + if mode != "triangle": + raise ValueError(f"unknown mode {mode!r}") + v = abs(speed) + if v <= 0.0 or amp <= 0.0: + return [(k / rate, center, 0.0) for k in range(n)] + # Triangle: centre → +amp → −amp → +amp …, each leg at constant speed. + for k in range(n): + t = k / rate + s = v * t # distance travelled along the zig-zag + # Fold onto a 4·amp period: 0..amp up, amp..3amp down, 3amp..4amp up. + phase = math.fmod(s, 4.0 * amp) + if phase < amp: + q, vs = center + phase, +v + elif phase < 3.0 * amp: + q, vs = center + amp - (phase - amp), -v + else: + q, vs = center - amp + (phase - 3.0 * amp), +v + out.append((t, q, vs)) + return out + + +def freed(freq: float) -> float: + """Positive frequency (a zero or negative request becomes a hold).""" + return max(freq, 0.0) + + +class BuzzGuard: + """Abort detector: high-frequency position motion or excess current. + + Feed one ``(position rad, iq A)`` sample per cycle. High-frequency motion + is the RMS of the position about its mean over the last + :data:`_BUZZ_WINDOW_S`; a commanded wave contributes little to that at + creep speeds, a limit cycle or resonance a lot. + """ + + def __init__(self, rate: float, buzz_rad: float, iq_abort: float) -> None: + self._n = max(4, int(round(_BUZZ_WINDOW_S * rate))) + self._pos: deque[float] = deque(maxlen=self._n) + self.buzz_rad = buzz_rad + self.iq_abort = iq_abort + self.peak_buzz = 0.0 + self.peak_iq = 0.0 + + def feed(self, pos: float, iq: float) -> str | None: + self._pos.append(pos) + self.peak_iq = max(self.peak_iq, abs(iq)) + if self.iq_abort > 0.0 and abs(iq) > self.iq_abort: + return f"current {iq:+.2f} A past the {self.iq_abort:g} A limit" + if len(self._pos) < self._n: + return None + arr = np.asarray(self._pos) + # Remove the wave itself (a straight line over 0.1 s) before scoring. + x = np.arange(self._n) + coef = np.polyfit(x, arr, 1) + resid = arr - np.polyval(coef, x) + buzz = float(np.sqrt(np.mean(resid * resid))) + self.peak_buzz = max(self.peak_buzz, buzz) + if self.buzz_rad > 0.0 and buzz > self.buzz_rad: + return f"{math.degrees(buzz):.2f}° of high-frequency motion (limit {math.degrees(self.buzz_rad):.2f}°)" + return None + + +def a4_metrics(log: list[dict], rate: float) -> dict[str, Any]: + """Score a firmware-loop run: the sine scorecard plus creep smoothness.""" + m: dict[str, Any] = sine_metrics(log) + if len(log) < 20: + return m + t = np.array([r["t"] for r in log]) + target = np.array([r["target"] for r in log]) + actual = np.array([r["actual"] for r in log]) + v_cmd = np.array([r["v_cmd"] for r in log]) + iq = np.array([r["iq"] for r in log]) + dt = float(np.median(np.diff(t))) if len(t) > 1 else 1.0 / rate + err = actual - target + # Lag: how far behind the command the joint runs, from the error while + # moving (error against the direction of travel over the speed). Works + # for a ramp or triangle, where a correlation shift would be swallowed + # by an offset, and for a sine, where the sign flips each half-cycle. + moving_now = np.abs(v_cmd) > 0.02 + if moving_now.any(): + behind = float(np.mean(-err[moving_now] * np.sign(v_cmd[moving_now]))) + m["lag_ms"] = behind / float(np.mean(np.abs(v_cmd[moving_now]))) * 1000.0 + else: + m["lag_ms"] = math.nan + # 1-4 Hz band of the error (the stick-slip band). + e = err - err.mean() + n = len(e) + F = np.abs(np.fft.rfft(e * np.hanning(n))) ** 2 + f = np.fft.rfftfreq(n, dt) + tot = float(F.sum()) + m["band_1_4"] = ( + float(math.sqrt(F[(f >= 1) & (f <= 4)].sum() / tot) * e.std()) + if tot > 0 + else 0.0 + ) + # Velocity ripple relative to the command, and stuck windows, while the + # command is actually moving. + win = max(2, int(round(0.05 / dt))) + kern = np.ones(win) / win + v_meas = np.convolve(np.gradient(actual, t), kern, mode="same") + moving = np.abs(v_cmd) > 0.02 + v_ref = float(np.mean(np.abs(v_cmd[moving]))) if moving.any() else 0.0 + m["v_ripple"] = ( + float(np.std((v_meas - v_cmd)[moving]) / v_ref) if v_ref > 0 else math.nan + ) + nwin = len(actual) // win + if nwin and moving.any(): + blocks = actual[: nwin * win].reshape(nwin, win) + mv_blocks = moving[: nwin * win].reshape(nwin, win).all(axis=1) + travel = np.abs(blocks[:, -1] - blocks[:, 0]) + m["stuck_frac"] = ( + float(np.mean(travel[mv_blocks] < math.radians(0.03))) + if mv_blocks.any() + else math.nan + ) + else: + m["stuck_frac"] = math.nan + # >10 Hz position content: buzz/resonance. + m["buzz"] = float(math.sqrt(F[f >= 10].sum() / tot) * e.std()) if tot > 0 else 0.0 + m["iq_rms"] = float(np.sqrt(np.nanmean(iq * iq))) + m["iq_max"] = float(np.nanmax(np.abs(iq))) + # Current *variation* — what the operator feels. The mean current is the + # gravity hold and says nothing about smoothness; its spread does, and + # the 3-8 Hz band of it is the position loop's own mode (~5 Hz on the + # X8-P20 shoulders), the shudder a 12 deg/s triangle's reversals kick up + # (1.9 A at position_kp 0.7 against 0.2 A at 3 deg/s) that neither the + # >10 Hz position "buzz" nor the >20 Hz current band track. + iq_c = iq - np.nanmean(iq) + m["iq_sd"] = float(np.nanstd(iq)) + Fi = np.abs(np.fft.rfft(np.nan_to_num(iq_c) * np.hanning(n))) ** 2 + tot_i = float(Fi.sum()) + m["iq_mode"] = ( + float(math.sqrt(Fi[(f >= 3) & (f <= 8)].sum() / tot_i) * m["iq_sd"]) + if tot_i > 0 + else 0.0 + ) + return m + + +# --------------------------------------------------------------------------- +# Motor access +# --------------------------------------------------------------------------- + + +def parse_pose( + specs: list[str] | None, joint: Joint, is_left: bool +) -> dict[Joint, float]: + """``--pose JOINT=DEG`` flags → joint-frame hold targets (rad), validated. + + Same rules as ``tune.pid``: known arm joint, not the test joint, inside + the arm's limits, shoulder_2 outboard only (the base is inboard), and + wrist_2's inboard half only with the elbow bent ≥ 30°. + + Why a pose here at all: a firmware position loop that is well damped + with the arm hanging can go underdamped with the arm extended — the + reflected inertia about shoulder_2 is several times larger with + shoulder_1 raised and the elbow bent — and right shoulder_2 was seen + oscillating on exactly that hold during a shoulder_3 sweep + (2026-09-22). Tune the worst-case pose, not just the rest pose. + """ + side = "left" if is_left else "right" + pose: dict[Joint, float] = {} + for spec in specs or []: + name, _, deg = spec.partition("=") + try: + pj = Joint(name) + except ValueError: + raise SystemExit(f"--pose: unknown joint {name!r}") from None + if pj == joint: + raise SystemExit(f"--pose: {name} is the test joint") + if pj not in ARM_JOINTS: + raise SystemExit(f"--pose: {name} is not an arm joint") + try: + rad = math.radians(float(deg)) + except ValueError: + raise SystemExit( + f"--pose: bad angle in {spec!r} (want JOINT=DEG)" + ) from None + lo, hi = arm_limits(pj, is_left) + if not (lo <= rad <= hi): + raise SystemExit( + f"--pose: {name}={deg}° is outside " + f"[{math.degrees(lo):.0f}, {math.degrees(hi):.0f}]° for the {side} arm" + ) + pose[pj] = rad + s2 = pose.get(Joint.SHOULDER_2) + s2_out = -1.0 if is_left else 1.0 + if s2 is not None and s2 * s2_out < 0: + raise SystemExit( + f"--pose: shoulder_2 must stay outboard " + f"({'negative' if s2_out < 0 else 'positive'} on the {side} arm) — " + "the robot base is inboard" + ) + w2 = pose.get(Joint.WRIST_2) + w2_out = 1.0 if is_left else -1.0 + if w2 is not None and w2 * w2_out < 0: + elbow_pose = pose.get(Joint.ELBOW) + if elbow_pose is None or abs(elbow_pose) < math.radians(30.0): + raise SystemExit( + "--pose: wrist_2 posed in its inboard half meets the base with the " + "elbow straight — pose the elbow bent too, e.g. --pose elbow=75" + ) + return pose + + +def parse_held_gains( + specs: list[str] | None, joint: Joint, is_left: bool +) -> dict[Joint, dict[str, float]]: + """``--held-gain [SIDE.]JOINT.GAIN=VALUE`` flags → RAM gains per held joint. + + A ring every held joint shares is fed by the held joints' own firmware + loops (see :func:`ring_power`), which ``--position-kp`` etc. cannot + reach: those set the test joint only. The side may be omitted but must + match the run's arm when given; the joint must be another arm joint, and + the gain one its motor's loop has (a Damiao wrist: position/speed kp/ki + only). Checked before any motor is enabled. + """ + side = "left" if is_left else "right" + known = set(GAIN_NAMES) + out: dict[Joint, dict[str, float]] = {} + for spec in specs or []: + name, eq, value = spec.partition("=") + parts = name.split(".") + if not eq or len(parts) not in (2, 3): + raise SystemExit( + f"--held-gain: {spec!r} is not [SIDE.]JOINT.GAIN=VALUE " + "(e.g. shoulder_2.position_kp=0.5)" + ) + if len(parts) == 3: + if parts[0] != side: + raise SystemExit( + f"--held-gain: {spec!r} names the {parts[0]} arm; this run is {side}" + ) + parts = parts[1:] + jname, gain = parts + try: + hj = Joint(jname) + except ValueError: + raise SystemExit(f"--held-gain: unknown joint {jname!r}") from None + if hj not in ARM_JOINTS: + raise SystemExit(f"--held-gain: {jname} is not an arm joint") + if hj == joint: + raise SystemExit( + f"--held-gain: {jname} is the test joint — set its gains with " + f"--{gain.replace('_', '-')}" + ) + if gain not in known: + raise SystemExit( + f"--held-gain: unknown gain {gain!r} (one of {', '.join(GAIN_NAMES)})" + ) + if position_wire_mode(hj) == "pv" and gain not in _DM_GAIN_REGS: + raise SystemExit( + f"--held-gain: {jname} is a Damiao motor: its loop has " + f"{', '.join(_DM_GAIN_REGS)} only (not {gain})" + ) + try: + out.setdefault(hj, {})[gain] = float(value) + except ValueError: + raise SystemExit(f"--held-gain: bad value in {spec!r}") from None + return out + + +def held_summary( + held_log: dict[str, list[tuple[float, float]]], holds: dict[str, float] +) -> dict[str, dict[str, float]]: + """Score each held joint's motion during the wave. + + Per joint (degrees): ``drift`` = mean position minus its hold, ``p2p`` + = peak-to-peak excursion, ``std`` = standard deviation, ``hz`` = + dominant frequency of that motion (NaN below ~1 s of samples). A held + joint sits on its own firmware position loop: std above a few + hundredths of a degree with a clear ``hz`` is the loop oscillating in + that pose; a large ``drift`` with little ``std`` is a joint that let go. + """ + out: dict[str, dict[str, float]] = {} + for name, samples in held_log.items(): + if len(samples) < 4: + continue + t = np.array([a for a, _ in samples]) + q = np.degrees(np.array([b for _, b in samples])) + hold = math.degrees(holds.get(name, 0.0)) + row = { + "drift": float(q.mean() - hold), + "p2p": float(np.ptp(q)), + "std": float(q.std()), + "hz": math.nan, + } + if t[-1] - t[0] > 1.0: + fs = (len(t) - 1) / (t[-1] - t[0]) + tu = np.arange(t[0], t[-1], 1.0 / fs) + x = np.interp(tu, t, q) + x = x - x.mean() + F = np.abs(np.fft.rfft(x * np.hanning(len(x)))) + f = np.fft.rfftfreq(len(x), 1.0 / fs) + m = f >= 0.5 + if m.any() and F[m].max() > 0: + row["hz"] = float(f[m][int(np.argmax(F[m]))]) + out[name] = row + return out + + +#: Half-width (Hz) of the band around the ring frequency that +#: :func:`ring_power` keeps: wide enough for a ring that wanders a few tenths +#: of a hertz over the run, narrow enough to leave the wave itself (the +#: triangle's fundamental is well under 1 Hz) and gravity's DC out. +_RING_HALF_BAND_HZ = 0.75 + + +def _band(t: np.ndarray, x: np.ndarray, lo: float, hi: float, fs: float) -> np.ndarray: + """``x`` resampled onto a uniform ``fs`` grid and band-passed to [lo, hi].""" + tu = np.arange(t[0], t[-1], 1.0 / fs) + xu = np.interp(tu, t, x) + X = np.fft.rfft(xu - xu.mean()) + f = np.fft.rfftfreq(len(xu), 1.0 / fs) + X[(f < lo) | (f > hi)] = 0.0 + return np.fft.irfft(X, len(xu)) + + +def ring_power( + dyn: dict[str, list[tuple[float, float, float]]], ring_hz: float +) -> dict[str, dict[str, float]]: + """Which joint feeds a shared ring: each joint's power at ``ring_hz``. + + ``dyn`` maps a joint to ``[(t, velocity rad/s, torque Nm), ...]``. Both + signals are band-passed to ``ring_hz`` ± :data:`_RING_HALF_BAND_HZ` and + ``power_w`` is the mean of their product — the mechanical power that + joint's motor puts into the ring. In a ring every joint shares, the one + whose loop drives it (torque in phase with velocity) shows positive + power; the joints being shaken by it only absorb (negative). ``cos_phi`` + is that phase as a correlation (+1 pure drive, -1 pure damping, ~0 a + spring), ``vel_amp`` / ``tau_amp`` the ring's amplitude in each signal. + Joints with under ~2 s of samples, or sampled too slowly to resolve the + band, are left out. + """ + lo, hi = ring_hz - _RING_HALF_BAND_HZ, ring_hz + _RING_HALF_BAND_HZ + out: dict[str, dict[str, float]] = {} + for name, samples in dyn.items(): + if len(samples) < 16: + continue + t = np.array([a for a, _, _ in samples]) + if t[-1] - t[0] < 2.0: + continue + fs = (len(t) - 1) / (t[-1] - t[0]) + if hi >= fs / 2: + continue + w = _band(t, np.array([b for _, b, _ in samples]), lo, hi, fs) + tau = _band(t, np.array([c for _, _, c in samples]), lo, hi, fs) + sw, st = float(w.std()), float(tau.std()) + power = float(np.mean(w * tau)) + out[name] = { + "power_w": power, + "cos_phi": power / (sw * st) if sw > 0 and st > 0 else math.nan, + "vel_amp": math.sqrt(2.0) * sw, + "tau_amp": math.sqrt(2.0) * st, + } + return out + + +def ring_hz(held_scores: dict[str, dict[str, float]]) -> float | None: + """The shared ring's frequency: the most-moving oscillating held joint's.""" + ringing = [ + r for r in held_scores.values() if r["std"] > 0.05 and r["hz"] == r["hz"] + ] + if not ringing: + return None + return max(ringing, key=lambda r: r["std"])["hz"] + + +def held_series( + held_log: dict[str, list[tuple[float, float]]], + held_dyn: dict[str, list[tuple[float, float, float]]], +) -> dict[str, np.ndarray]: + """The held joints' raw samples as run series, one key set per joint. + + ``held__pos_t`` / ``_pos`` (rad, joint frame) and ``_dyn_t`` / + ``_vel`` (rad/s) / ``_tau`` (Nm). Each joint has its own time base — the + reads are round-robin — so these are not aligned with the wave's ``t``. + """ + out: dict[str, np.ndarray] = {} + for name, samples in held_log.items(): + if samples: + out[f"held_{name}_pos_t"] = np.array([a for a, _ in samples]) + out[f"held_{name}_pos"] = np.array([b for _, b in samples]) + for name, samples in held_dyn.items(): + if samples: + out[f"held_{name}_dyn_t"] = np.array([a for a, _, _ in samples]) + out[f"held_{name}_vel"] = np.array([b for _, b, _ in samples]) + out[f"held_{name}_tau"] = np.array([c for _, _, c in samples]) + return out + + +def _a4_frame(position_rad: float, cap_dps: float) -> bytes: + cap = int(max(0.0, min(65535.0, round(cap_dps)))) + return ( + bytes([_MA_POS_CONTROL, 0x00]) + + struct.pack(" bytes: + """0x73 (protocol V4.4): the 0xA4 frame with an int8 torque feedforward in + 1% of rated current in byte 1.""" + ff = int(max(-128, min(127, round(ff_pct)))) + return bytes([_MA_TF_CONTROL, ff & 0xFF]) + _a4_frame(position_rad, cap_dps)[2:] + + +#: The ``--tf-probe`` square wave: feedforward 0, +P, 0, -P % for this long each. +TF_PROBE_HALF_S = 0.25 + + +def tf_probe_ff(t: float, pct: float) -> float: + """The probe's feedforward (% rated current) at ``t`` seconds in.""" + return (0.0, pct, 0.0, -pct)[int(t / TF_PROBE_HALF_S) % 4] + + +def tf_step_estimate(samples: list[tuple[float, float, float]]) -> dict[str, float]: + """Amps of q-axis current per 1% of rated current, from a probe's + ``(t, ff_pct, iq_A)`` samples. + + At each feedforward switch the firmware adds the new current at once + (its current loop runs at kHz), while the position and speed PIs answer + only once the joint has moved: the reply to the first frame carrying the + new feedforward (sent back within a fraction of a millisecond) against + the mean of the few before the switch, over the jump in percent, is the + current one percent buys. Later samples are already unwinding under the + loops, so only that first one is used. The median over every switch + rejects the odd step a control-loop transient spoiled. + + Returns ``{"amps_per_pct", "spread", "edges"}`` — the spread is the + interquartile range across switches; ``amps_per_pct`` is NaN with no + usable switch. + """ + ratios = [] + for i in range(6, len(samples)): + d_ff = samples[i][1] - samples[i - 1][1] + if d_ff == 0 or samples[i - 1][1] != samples[i - 6][1]: + continue + before = float(np.mean([s[2] for s in samples[i - 5 : i]])) + after = samples[i][2] + ratios.append((after - before) / d_ff) + if not ratios: + return {"amps_per_pct": math.nan, "spread": math.nan, "edges": 0} + q1, med, q3 = np.percentile(ratios, [25, 50, 75]) + return {"amps_per_pct": float(med), "spread": float(q3 - q1), "edges": len(ratios)} + + +async def _tf_probe( + driver: MyActuatorMotor, + motor: JointFrameMotor, + pose: float, + cap_dps: float, + rate: float, + pct: float, + seconds: float = 8.0, +) -> list[tuple[float, float, float]]: + """Hold ``pose`` on 0x73 with the ``tf_probe_ff`` square wave; returns + ``(t, ff_pct, iq_A)`` per reply.""" + target = pose - motor.frame_offset + period = 1.0 / rate + out: list[tuple[float, float, float]] = [] + t0 = time.perf_counter() + deadline = t0 + for k in range(int(seconds * rate)): + deadline += period + ff = tf_probe_ff(k * period, pct) + resp = await driver._request(_tf_frame(target, cap_dps, ff)) + iq, _speed = _decode_a4_reply(resp) + out.append((time.perf_counter() - t0, ff, iq)) + await asyncio.sleep(max(0.0, deadline - time.perf_counter())) + return out + + +def speed_cap( + v_cmd_rad_s: float, cap_dps: float, track: float, floor_dps: float +) -> float: + """The 0xA4 speed cap (deg/s) for one streamed sample. + + ``track <= 0``: the fixed ``cap_dps``. Otherwise the cap follows the + commanded speed — ``track × |v_cmd|``, floored at ``floor_dps`` so a + stationary or reversing target can still be corrected, and never above + ``cap_dps``. + + Why: with the planner at its maximum (60000 dps/s) and a fixed 60 dps + cap, each 200 Hz target is a 0.015° step at 3 deg/s that the planner + covers in ~0.5 ms at the cap and then idles for the remaining 4.5 ms — + the joint moves in bursts at twenty times the commanded speed with a + 5 % duty cycle. On the right elbow that was 0.019° RMS tracking with + four times the current spread of direct tracking (1.28 A vs 0.33 A, + 0.76 A above 20 Hz, 68–82 Hz velocity content). A cap of ~1.1–1.2× the + commanded speed lets the planner run continuously at about that speed + and arrive just before the next target instead. + """ + if track <= 0.0: + return cap_dps + want = track * abs(math.degrees(v_cmd_rad_s)) + return min(cap_dps, max(floor_dps, want)) + + +def _decode_a4_reply(resp: bytes) -> tuple[float, float]: + """(iq A, speed rad/s) from a 0xA4 reply.""" + iq = struct.unpack_from(" dict[str, float]: + out: dict[str, float] = {} + if isinstance(driver, DamiaoMotor): + for name, rid in _DM_GAIN_REGS.items(): + out[name] = float(await driver._read_register(rid)) + return out + for name, index in _MA_PID_IDX.items(): + resp = await driver._request(bytes([_MA_READ_GAIN, index, 0, 0, 0, 0, 0, 0])) + out[name] = float(struct.unpack_from(" None: + if isinstance(driver, DamiaoMotor): + # RAM registers take effect immediately; 0xAA stores them all. + for name, value in gains.items(): + await driver._write_register(_DM_GAIN_REGS[name], float(value)) + await asyncio.sleep(0.02) + if persist: + await driver._store_parameters() + await asyncio.sleep(_FLASH_SETTLE_S) + return + cmd = _MA_WRITE_GAIN_ROM if persist else _MA_WRITE_GAIN_RAM + for name, value in gains.items(): + await driver._request( + bytes([cmd, _MA_PID_IDX[name], 0, 0]) + struct.pack(" bytes: + """The Damiao position-velocity command: ``(p_des rad, v_des rad/s)`` LE.""" + return struct.pack(" tuple[float, float]: + return ( + float(await driver._read_register(_DM_REG_ACC)), + float(await driver._read_register(_DM_REG_DEC)), + ) + + +async def _dm_write_ramps( + driver: DamiaoMotor, acc: float, dec: float, persist: bool +) -> tuple[float, float]: + await driver._write_register(_DM_REG_ACC, float(acc)) + await asyncio.sleep(0.02) + await driver._write_register(_DM_REG_DEC, float(dec)) + await asyncio.sleep(0.02) + if persist: + await driver._store_parameters() + await asyncio.sleep(_FLASH_SETTLE_S) + return await _dm_read_ramps(driver) + + +async def _read_accel(driver: MyActuatorMotor) -> tuple[int, int]: + out = [] + for kind in (0x00, 0x01): + resp = await driver._request(bytes([_MA_READ_ACCEL, kind, 0, 0, 0, 0, 0, 0])) + out.append(int(struct.unpack_from(" tuple[int, int]: + for kind, value in ((0x00, acc), (0x01, dec)): + await driver._request( + bytes([_MA_WRITE_ACCEL, kind, 0, 0]) + struct.pack(" tuple[ + list[dict], + str | None, + dict[str, list[tuple[float, float]]], + dict[str, list[tuple[float, float, float]]], +]: + """Stream the wave; returns the log, the abort reason (if any), and the + held joints' samples, one read of one held joint per tick after the + wave's own round trips. Turns alternate between a position read (0x92 / + Damiao p_m) into ``{joint: [(t, position_rad), ...]}`` and a velocity + + torque read (0x9C / Damiao 0xCC feedback) into ``{joint: [(t, vel_rad_s, + torque_nm), ...]}``, so each held joint gets each at ``rate / (2 * + len(held))`` Hz — ~33 Hz on a full arm at 400 Hz, ample for the few-Hz + rings the held table and :func:`ring_power` look for.""" + offset = motor.frame_offset + period = 1.0 / rate + log: list[dict] = [] + held_items = [ + (j.value, jm.motor._driver, jm.frame_offset) + for j, jm in (held or {}).items() + if isinstance(jm.motor._driver, (MyActuatorMotor, DamiaoMotor)) + ] + held_log: dict[str, list[tuple[float, float]]] = {n: [] for n, _, _ in held_items} + held_dyn: dict[str, list[tuple[float, float, float]]] = { + n: [] for n, _, _ in held_items + } + is_dm = isinstance(driver, DamiaoMotor) + loop = asyncio.get_running_loop() + t0 = time.perf_counter() + deadline = t0 + for k, (_t_nominal, target, v_cmd) in enumerate(samples): + deadline += period + cap = speed_cap(v_cmd, cap_dps, cap_track, cap_floor_dps) + # The commanded target, led along the wave's velocity (--lead-ms); the + # log keeps the true target, so the run scores against the wave. + sent = target + v_cmd * lead_s + if is_dm: + # One 0x100 command, one feedback frame back: position (16-bit), + # velocity and torque. Torque fills the ``iq`` channel, in Nm. + fut = loop.create_future() + driver._feedback_waiters.append(fut) + await driver._raw_send( + dm_frame(sent - offset, cap), 0x100 + driver._motor_id + ) + try: + fb = await asyncio.wait_for(fut, _DM_REPLY_TIMEOUT_S) + except asyncio.TimeoutError: + fb = driver._feedback + if fb is None: + raise MotorError(f"Damiao motor {driver._motor_id:#04x}: no feedback") + pos, iq, speed = fb.position + offset, fb.torque, fb.velocity + else: + resp = await driver._request(_a4_frame(sent - offset, cap)) + iq, speed = _decode_a4_reply(resp) + fine = await driver._request( + bytes([_MA_MULTI_TURN_ANGLE, 0, 0, 0, 0, 0, 0, 0]) + ) + pos = ( + struct.unpack_from("= len(held_items): + # The dynamics turn: velocity and torque from one reply, + # so the two are simultaneous for the power product. + if isinstance(hdrv, DamiaoMotor): + fb = await hdrv._request_feedback( + timeout=_DM_REPLY_TIMEOUT_S, attempts=1 + ) + held_dyn[name].append((now, fb.velocity, fb.torque)) + else: + st = await hdrv._request( + bytes([_MA_STATUS2, 0, 0, 0, 0, 0, 0, 0]) + ) + # Own names: iq/speed are the driven joint's, read + # below by the log and the buzz guard. + h_iq = struct.unpack_from(" _ERR_ABORT: + reason = f"tracking error {math.degrees(pos - target):+.1f}° — the loop is not following" + if reason is not None: + return log, reason, held_log, held_dyn + await asyncio.sleep(max(0.0, deadline - time.perf_counter())) + return log, None, held_log, held_dyn + + +async def _hold( + driver: MyActuatorMotor | DamiaoMotor, + motor: JointFrameMotor, + pose: float, + cap_dps: float, + seconds: float, +) -> None: + period = 0.01 + end = time.perf_counter() + seconds + while time.perf_counter() < end: + if isinstance(driver, DamiaoMotor): + await driver._raw_send( + dm_frame(pose - motor.frame_offset, cap_dps), 0x100 + driver._motor_id + ) + else: + await driver._request(_a4_frame(pose - motor.frame_offset, cap_dps)) + await asyncio.sleep(period) + + +async def _report_tf_probe( + driver: MyActuatorMotor | DamiaoMotor, + motor: JointFrameMotor, + joint: Joint, + pose: float, + args: argparse.Namespace, +) -> None: + """Run ``--tf-probe`` at the held pose and print the rated current it + implies (the ``firmware.tf_rated_current_a`` to configure).""" + if not isinstance(driver, MyActuatorMotor): + print(f" ! --tf-probe: {joint.value} is not a MyActuator motor") + return + version = driver._fw_version or await driver._read_firmware_version() + if version < _MA_FW_V44_VERSION: + print( + f" ! --tf-probe: firmware {version} predates 0x73 (protocol V4.4, " + f"VersionDate {_MA_FW_V44_VERSION} or later) — this joint stays on 0xA4" + ) + return + pct = float(args.tf_probe) + print( + f" 0x73 probe: holding {math.degrees(pose):+.1f}° with feedforward " + f"0 / +{pct:g} / 0 / -{pct:g} % rated current, {TF_PROBE_HALF_S:g} s each ..." + ) + samples = await _tf_probe(driver, motor, pose, args.cap, args.rate, pct) + await _hold(driver, motor, pose, args.cap, 0.3) + est = tf_step_estimate(samples) + if not est["edges"] or not math.isfinite(est["amps_per_pct"]): + print(" ! no usable feedforward switch in the probe — nothing to estimate") + return + if est["amps_per_pct"] <= 0.0: + print( + f" ! +1% feedforward moved iq by {est['amps_per_pct']:+.4f} A — not the " + "positive step 0x73 is documented to give. Do not enable the core's 0x73 " + "feedforward on this joint until that is understood (a sign flip would " + "double the gravity load instead of carrying it)." + ) + return + kt = float(driver._kt) + rated = 100.0 * est["amps_per_pct"] + print(f"\n{'─' * 66}") + print( + f" iq per 1% feedforward: {est['amps_per_pct']:.4f} A " + f"(IQR {est['spread']:.4f} over {est['edges']} switches)" + ) + print( + f" → rated current ≈ {rated:.2f} A; with kt {kt:g} Nm/A that is " + f"{kt * est['amps_per_pct']:.4f} Nm per %" + ) + print( + f" configure: firmware.tf_rated_current_a = {rated:.2f} (or its datasheet " + f"value) — e.g. tune.motion --gain {joint.value}.firmware.tf_rated_current_a=" + f"{rated:.2f}" + ) + print(f"{'─' * 66}") + + +# --------------------------------------------------------------------------- +# CLI +# --------------------------------------------------------------------------- + + +def add_parser(subparsers: argparse._SubParsersAction) -> None: # type: ignore[type-arg] + """Register the ``tune.a4`` subcommand.""" + p = subparsers.add_parser( + "tune.a4", + help="Tune a MyActuator joint's firmware position loop (0xA4) with a sine or triangle.", + formatter_class=argparse.RawDescriptionHelpFormatter, + description=__doc__, + ) + add_side_and_channel_arguments(p) + p.add_argument( + "--joint", + required=True, + choices=[j.value for j in ARM_JOINTS], + help="Joint to drive (MyActuator joints: shoulder_1 … wrist_1)", + ) + p.add_argument( + "--mode", + choices=["sine", "triangle"], + default="triangle", + help="Wave shape: sine (--freq) or constant-speed triangle (--speed) (default: triangle)", + ) + p.add_argument( + "--center", + type=float, + default=None, + help="Centre, joint-frame degrees (default: joint midpoint of the safe range)", + ) + p.add_argument( + "--amp", type=float, default=10.0, help="Half-travel, degrees (default: 10)" + ) + p.add_argument( + "--freq", type=float, default=0.3, help="[sine] frequency, Hz (default: 0.3)" + ) + p.add_argument( + "--speed", + type=float, + default=3.0, + help="[triangle] pass speed, deg/s (default: 3)", + ) + p.add_argument( + "--duration", type=float, default=12.0, help="Seconds of wave (default: 12)" + ) + p.add_argument( + "--rate", + type=float, + default=400.0, + help="Command rate, Hz (default: 400 — the realtime core's a4 stream rate; 200 Hz put an audible target staircase on the X8-P20 shoulder at 12 deg/s that 400 removed)", + ) + p.add_argument( + "--cap", type=float, default=60.0, help="0xA4 speed cap, deg/s (default: 60)" + ) + p.add_argument( + "--dm-acc", + type=float, + default=None, + help="Damiao wrists only: the position-velocity profiler's ACC (and -DEC), rad/s², " + "written to the registers for the run and restored afterwards unless --keep " + "(--persist stores them). The wrists were found at 2 rad/s² (~115 deg/s²), far " + "too slow to follow a streamed target.", + ) + p.add_argument( + "--pose", + action="append", + default=None, + metavar="JOINT=DEG", + help="Hold another joint at this joint-frame angle (degrees) during the run, " + "e.g. --pose shoulder_1=-90 --pose elbow=-75 (repeatable; overrides the sweep's " + "own clearance pose for that joint). A firmware loop that is well damped with " + "the arm hanging can oscillate with it extended — right shoulder_2 did, held " + "during a shoulder_3 sweep — so tune the worst-case pose too. Posed joints " + "return to rest afterwards.", + ) + p.add_argument( + "--held-gain", + action="append", + default=None, + metavar="[SIDE.]JOINT.GAIN=VALUE", + help="Set a *held* joint's firmware loop gain for the run, in RAM, " + "restored afterwards (unless --keep), e.g. --held-gain " + "shoulder_2.position_kp=0.5 --held-gain wrist_2.position_kp=200 " + "(repeatable). The held joints' loops are what feed a ring they all " + "share — the power table names them — and the per-gain flags only reach " + "the test joint. Written after the mode switch, like the test joint's " + "(the reset reloads ROM). Damiao wrists take position/speed kp/ki only", + ) + p.add_argument( + "--cap-track", + type=float, + default=0.0, + help="Make the per-command speed cap follow the wave: cap = this × |commanded " + "speed| (floored at --cap-floor, never above --cap). 0 (default) = fixed --cap. " + f"With --accel {_ACCEL_STEP_FOLLOW} a fixed cap lets the planner burst through " + "each 200 Hz step at the cap and idle the rest of the tick (4x the current " + "spread on the elbow); 1.1-1.2 keeps it moving continuously at about the " + "commanded speed.", + ) + p.add_argument( + "--cap-floor", + type=float, + default=1.0, + help="Lowest cap --cap-track may set, deg/s, so a stationary or reversing " + "target can still be corrected (default: 1)", + ) + p.add_argument( + "--lead-ms", + type=float, + default=0.0, + help="Send each command's target this far ahead along the commanded velocity " + "(target + v_cmd × lead); the run is still scored against the true wave. With " + f"--accel {_ACCEL_STEP_FOLLOW} the planner plans to *stop* at each target, so at " + "a fixed command rate it cannot average more than ~accel × tick / 4 (≈31 deg/s at " + "480 Hz) whatever the cap; a target a few ms ahead is never reached within the " + "tick, so it cruises at the cap instead. 0 (default) = no lead.", + ) + p.add_argument( + "--no-imu", + action="store_true", + help="Do not record the wrist camera's IMU (recorded by default: the run " + "gets an 'imu' shake score — 1-15 Hz displacement p2p in mm at the gripper).", + ) + p.add_argument( + "--tf-probe", + type=float, + default=None, + metavar="PCT", + help="Instead of the wave: hold the joint at --center on 0x73 (protocol " + "V4.4 position control with torque feedforward) and step the feedforward " + f"0 / +PCT / 0 / -PCT %% of rated current ({TF_PROBE_HALF_S:g} s each, 8 s) — " + "the q-axis current jump at each step, before the loop reacts, measures " + "the current 1%% buys, i.e. the motor's rated current: the " + "firmware.tf_rated_current_a the realtime core scales its 0x73 " + "feedforward with. 5 is a gentle ~1 Nm on a shoulder. MyActuator V4.4 " + "firmware only; run it with --accel 0.", + ) + p.add_argument( + "--accel", + type=int, + default=None, + help="Position-planner acceleration (dps/s) for the run, written to ROM before the " + "mode-switch reset and restored afterwards unless --keep; default: leave the stored " + f"value. 0 = direct PI tracking of the stream; {_ACCEL_STEP_FOLLOW} (the protocol " + "maximum) = the planner completes each 200 Hz step within the tick, which tracked the " + "X6-P20 elbow better than 0 (0.02° vs 0.23° RMS). Anything in between re-plans every " + "target and will not follow the wave.", + ) + for name in GAIN_NAMES: + p.add_argument( + f"--{name.replace('_', '-')}", + dest=name, + type=float, + default=None, + help=f"Firmware {name} for this run (default: leave as is)", + ) + p.add_argument( + "--persist", + action="store_true", + help="Write gains to ROM (0x32) instead of RAM (0x31)", + ) + p.add_argument( + "--keep", + action="store_true", + help="Leave the run's gains and planner acceleration in the motor afterwards", + ) + p.add_argument( + "--buzz-abort", + type=float, + default=0.3, + help="Abort past this much >10 Hz position motion, degrees RMS over 0.1 s (default: 0.3; 0 off)", + ) + p.add_argument( + "--iq-abort", + type=float, + default=30.0, + help="Abort past this reply current, amps (default: 30; 0 off). A loaded X8 " + "shoulder draws ~10 A just holding gravity at -55°, so keep this well above " + "the pose's static current", + ) + p.add_argument( + "--save-run", + action="store_true", + help="Persist the run for the diagnostics dashboard", + ) + p.add_argument("--label", default=None, help="Free-form note stored on the run") + p.add_argument( + "--group", default=None, help="Shared id linking the runs of one sweep" + ) + p.set_defaults(func=run) + + +def run(args: argparse.Namespace) -> None: + asyncio.run(_run(args)) + + +async def _run(args: argparse.Namespace) -> None: + joint = Joint(args.joint) + is_left = args.l + side = "left" if is_left else "right" + lo, hi = safe_limits(joint, is_left) + amp = math.radians(args.amp) + center = math.radians(args.center) if args.center is not None else (lo + hi) / 2.0 + margin = math.radians(2.0) + if not (lo + margin <= center - amp and center + amp <= hi - margin): + raise SystemExit( + f"{joint.value}: {math.degrees(center - amp):+.1f}..{math.degrees(center + amp):+.1f}° " + f"is outside the safe range [{math.degrees(lo) + 2:.1f}, {math.degrees(hi) - 2:.1f}]°" + ) + requested = { + n: getattr(args, n) for n in GAIN_NAMES if getattr(args, n) is not None + } + held_gains = parse_held_gains(args.held_gain, joint, is_left) + if not 0.0 <= args.lead_ms <= 50.0: + raise SystemExit("--lead-ms must be within 0..50") + samples = waveform( + args.mode, + center, + amp, + args.duration, + args.rate, + freq=args.freq, + speed=math.radians(args.speed), + ) + # A led target runs ahead of the wave by up to v_max × lead; keep that + # inside the 2° range margin checked above. + lead_reach = max(abs(v) for _, _, v in samples) * args.lead_ms / 1e3 + if lead_reach > math.radians(1.0): + raise SystemExit( + f"--lead-ms {args.lead_ms:g} leads the target up to " + f"{math.degrees(lead_reach):.2f}° past the wave (max 1°): lower the lead " + "or the speed" + ) + print(f"\ntune.a4 — {side} {joint.value}: firmware position loop (0xA4)") + print( + f" {args.mode} about {math.degrees(center):+.1f}° ±{args.amp:g}°, " + + (f"{args.freq:g} Hz" if args.mode == "sine" else f"{args.speed:g} deg/s") + + f", {args.duration:g} s at {args.rate:g} Hz, speed cap {args.cap:g} dps" + + ( + f" tracking {args.cap_track:g}× commanded speed (floor {args.cap_floor:g}" + ", planner permitting)" + if args.cap_track > 0 + else "" + ) + + (f", targets led {args.lead_ms:g} ms" if args.lead_ms else "") + ) + + channel = resolve_channel(args) + # The wrist camera's IMU: opened before the motors, stopped after them. + imu = WristImu([side], enabled=not args.no_imu and args.tf_probe is None) + imu.start() + stream_origin: float | None = None + async with CanBus(channel) as bus: + raw = {j: Motor(bus, j) for j in ARM_JOINTS} + await asyncio.gather(*[m.enable() for m in raw.values()]) + driver = raw[joint]._driver + if not isinstance(driver, (MyActuatorMotor, DamiaoMotor)): + raise SystemExit(f"{joint.value} has no firmware position loop to tune") + is_dm = isinstance(driver, DamiaoMotor) + if is_dm: + unsupported = sorted(set(requested) - set(_DM_GAIN_REGS)) + if unsupported: + raise SystemExit( + f"{joint.value} is a Damiao motor: its loop has " + f"{', '.join(_DM_GAIN_REGS)} only (not {', '.join(unsupported)})" + ) + if args.accel is not None: + raise SystemExit( + f"{joint.value} is a Damiao motor: its profiler is ACC/DEC in rad/s² — " + "use --dm-acc, not --accel" + ) + elif args.dm_acc is not None: + raise SystemExit( + f"--dm-acc is for the Damiao wrists; {joint.value} takes --accel" + ) + before_gains: dict[str, float] | None = None + before_accel: tuple[int, int] | None = None + before_ramps: tuple[float, float] | None = None + log: list[dict] = [] + reason: str | None = None + used_gains: dict[str, float] = {} + accel_used: tuple[int, int] | None = None + ramps_used: tuple[float, float] | None = None + held_scores: dict[str, dict[str, float]] = {} + held_log: dict[str, list[tuple[float, float]]] = {} + held_dyn: dict[str, list[tuple[float, float, float]]] = {} + ring_at: float | None = None + ring_scores: dict[str, dict[str, float]] = {} + # Held joints' gains as found (restored at the end) and as run. + held_before: dict[Joint, dict[str, float]] = {} + held_used: dict[str, dict[str, float]] = {} + current_label = "torque" if is_dm else "current" + current_unit = "Nm" if is_dm else "A" + + # Planner acceleration goes in *before* the mode switch below: that + # switch is a 0x76 reset, and the reset is what makes a planner value + # of 0 take effect. On the X6-P20 elbow (firmware 2025070202) a 0 + # written into a running position loop is ignored — the joint held + # its target and executed nothing for a whole run (2026-09-21) while + # the same 0 stored before the reset gave the documented direct + # tracking. Non-zero values do apply live on that firmware (5000 → + # 60000 took effect mid-session); the X8-P20 shoulders (2026042402) + # apply 0 live as well. Writing first is right for every one of them. + cap_track = args.cap_track + if is_dm: + # Damiao: the profiler is always on (ACC in (0, fmax), DEC < 0), + # registers in RAM, no reset needed for them to take effect. A + # stored 2 rad/s² (the wrists as found) is ~115 deg/s² — far too + # slow to follow a streamed target; the sweep says what does. + stored_ramps = await _dm_read_ramps(driver) + print( + f" profiler ACC/DEC stored: {stored_ramps[0]:g}/{stored_ramps[1]:g} rad/s²" + ) + if args.dm_acc is not None and (stored_ramps[0], -stored_ramps[1]) != ( + args.dm_acc, + args.dm_acc, + ): + before_ramps = stored_ramps + ramps_used = await _dm_write_ramps( + driver, args.dm_acc, -abs(args.dm_acc), args.persist + ) + print( + f" profiler ACC/DEC {stored_ramps[0]:g}/{stored_ramps[1]:g} → " + f"{ramps_used[0]:g}/{ramps_used[1]:g} rad/s²" + ) + else: + ramps_used = stored_ramps + else: + stored_accel = await _read_accel(driver) + print( + f" planner accel/decel stored: {stored_accel[0]}/{stored_accel[1]} dps/s" + ) + if args.accel is not None and stored_accel != (args.accel, args.accel): + before_accel = stored_accel + accel_used = await _write_accel(driver, args.accel, args.accel) + print( + f" planner accel/decel {stored_accel[0]}/{stored_accel[1]} → {accel_used[0]}/{accel_used[1]} dps/s" + ) + else: + accel_used = stored_accel + if accel_used[0] not in (0, _ACCEL_STEP_FOLLOW): + print( + f" ! planner acceleration is {accel_used[0]} dps/s: the firmware re-plans " + "every streamed target and will not follow the wave — pass --accel 0 " + f"(direct PI tracking) or --accel {_ACCEL_STEP_FOLLOW} (planner completes " + "each step within the tick)" + ) + + if accel_used is not None and accel_used[0] == 0 and cap_track > 0: + # Under direct tracking the cap is a hard limit on the PI output: + # pinned near the commanded speed the loop can never catch up + # (right elbow, pKp 0.5, cap-track 1.1: 1.8° RMS, 480 ms lag). + # The knob exists for the planner's per-tick bursts, which + # direct tracking does not have. + print( + f" ! --cap-track {cap_track:g} ignored: the planner is at 0 (direct " + "PI tracking), where the cap would only throttle the loop" + ) + cap_track = 0.0 + + motors = await joint_frame_motors(raw, is_left) + await asyncio.gather( + *[ + m.set_control_mode(ControlMode.POSITION_VELOCITY) + for m in motors.values() + ] + ) + motor = motors[joint] + try: + print(" Homing all joints to rest ...") + await _home_all(motors) + other_targets, _lo, _hi, notes = sweep_safety(joint, is_left) + for note in notes: + print(f" {note}") + pose = parse_pose(args.pose, joint, is_left) + if pose: + other_targets.update(pose) + print( + " Posing " + + ", ".join( + f"{j.value} at {math.degrees(q):+.0f}°" for j, q in pose.items() + ) + ) + for stage in ramp_stages(other_targets): + await _ramp_verified(motors, stage) + print(f" Ramping {joint.value} to {math.degrees(center):+.1f}° ...") + await _ramp_verified(motors, {joint: center}) + await asyncio.sleep(0.3) + + # Gains are written here, after the mode switch and homing: RAM + # gains (0x31) do not survive the reset those perform. + stock = await _read_gains(driver) + used_gains = {**stock, **requested} + if requested: + before_gains = stock + await _write_gains(driver, requested, args.persist) + used_gains = await _read_gains(driver) + for n in requested: + flag = ( + "" + if abs(used_gains[n] - requested[n]) + <= 1e-6 * max(1.0, abs(requested[n])) + else " (! not accepted)" + ) + print(f" {n:12s} {stock[n]:.6g} → {used_gains[n]:.6g}{flag}") + else: + print(" gains: " + ", ".join(f"{n}={v:.6g}" for n, v in stock.items())) + for hj, hgains in held_gains.items(): + hdrv = raw[hj]._driver + found = await _read_gains(hdrv) + held_before[hj] = {n: found[n] for n in hgains} + await _write_gains(hdrv, hgains, False) + got = await _read_gains(hdrv) + held_used[hj.value] = {n: got[n] for n in hgains} + for n, want in hgains.items(): + flag = ( + "" + if abs(got[n] - want) <= 1e-6 * max(1.0, abs(want)) + else " (! not accepted)" + ) + print( + f" held {hj.value}.{n:12s} {found[n]:.6g} → {got[n]:.6g}{flag}" + ) + + if args.tf_probe is not None: + await _report_tf_probe(driver, motor, joint, center, args) + return + guard = BuzzGuard(args.rate, math.radians(args.buzz_abort), args.iq_abort) + live = LiveStream("sine", joint) + print(" Running ...") + stream_origin = time.perf_counter() + log, reason, held_log, held_dyn = await _stream( + motor, + driver, + samples, + args.cap, + args.rate, + guard, + live, + cap_track=cap_track, + cap_floor_dps=args.cap_floor, + lead_s=args.lead_ms / 1e3, + held={j: jm for j, jm in motors.items() if j != joint}, + ) + live.flush() + held_scores = held_summary( + held_log, + { + j.value: other_targets.get( + j, rest_target(j, getattr(jm, "_is_left", None)) + ) + for j, jm in motors.items() + }, + ) + if held_scores: + print( + " held joints during the wave (drift / p2p / std / dominant Hz):" + ) + for name, r in held_scores.items(): + flag = ( + " <-- oscillating" + if r["std"] > 0.05 and r["hz"] == r["hz"] + else "" + ) + print( + f" {name:12s} {r['drift']:+6.2f}° / {r['p2p']:5.2f}° / {r['std']:5.3f}° / " + + (f"{r['hz']:4.1f} Hz" if r["hz"] == r["hz"] else " — ") + + flag + ) + ring_at = ring_hz(held_scores) if held_scores else None + if ring_at is not None: + # The joint under test is in the ring too: its own echo + # carries speed and current (Damiao: torque) every tick. + kt = 1.0 if is_dm else float(driver._kt) + dyn = { + f"{joint.value} (driven)": [ + (r["t"], r["speed"], r["iq"] * kt) for r in log + ], + **held_dyn, + } + ring_scores = ring_power(dyn, ring_at) + if ring_scores: + print( + f" power into the {ring_at:.1f} Hz ring (W; + feeds it, " + "- absorbs it) / phase corr / velocity ° s⁻¹ / torque Nm:" + ) + ranked = sorted( + ring_scores.items(), key=lambda kv: -kv[1]["power_w"] + ) + for i, (name, r) in enumerate(ranked): + mark = ( + " <-- feeds the ring" + if i == 0 and r["power_w"] > 0 + else "" + ) + print( + f" {name:20s} {r['power_w']:+8.4f} / {r['cos_phi']:+5.2f} / " + f"{math.degrees(r['vel_amp']):6.2f} / {r['tau_amp']:6.3f}{mark}" + ) + # The other joints were parked on their own 0xA4 loops and then + # received no frames for the whole wave. Say so if any let go: + # the elbow was found several degrees off rest across runs + # (2026-09-22), and a motor with the communication-interruption + # protection (0xB3) armed cuts its output when the bus goes quiet + # on it — exactly a wave on another joint. + drifted: list[str] = [] + for j, jm in motors.items(): + if j == joint: + continue + hold = other_targets.get( + j, rest_target(j, getattr(jm, "_is_left", None)) + ) + try: + pos = await jm.get_position() + except MotorError: + drifted.append(f"{j.value} (no position reply)") + continue + if abs(pos - hold) > _HOLD_DRIFT_TOL: + drifted.append( + f"{j.value} {math.degrees(pos - hold):+.1f}° off its " + f"{math.degrees(hold):+.0f}° hold" + ) + if drifted: + print( + " ! held joints moved during the wave: " + + ", ".join(drifted) + + " — a joint that lets go while another is streamed points at " + "its communication-interruption protection (0xB3: output cut " + "after N ms without a frame); the tuner sends held joints " + "nothing during the wave" + ) + if reason is not None: + print(f"\n ! aborted: {reason}") + if before_gains is not None: + await _write_gains(driver, before_gains, args.persist) + print(" previous gains restored") + before_gains = None + # The raw 0xA4 stream never fills the driver's position cache; + # read the joint explicitly before holding it where it stopped. + here = await motor.get_position() + await _hold(driver, motor, here, args.cap, 0.5) + report_achieved_rate(log, args.rate) + except KeyboardInterrupt: + print("\n Interrupted.") + finally: + print(" Returning to rest ...") + # Home *before* restoring the firmware gains: the run's gains are + # the stiffer set, and the stock position loop has been seen to + # stall short of rest on a gravity-loaded elbow. The planner is + # restored first only when the run left it at 0, because a + # direct-tracking joint would otherwise execute the homing target + # at the speed cap. + homed = False + try: + if ( + before_accel is not None + and not args.keep + and accel_used + and accel_used[0] == 0 + ): + got = await _write_accel(driver, before_accel[0], before_accel[1]) + print(f" planner accel/decel restored to {got[0]}/{got[1]} dps/s") + before_accel = None + await _ramp_verified(motors, {joint: 0.0}) + await _home_all(motors) + homed = True + except Exception as exc: # noqa: BLE001 - reported below, arm keeps holding + print(f" ! return to rest did not complete: {exc}") + try: + if before_gains is not None and not args.keep: + await _write_gains(driver, before_gains, args.persist) + print(" previous gains restored") + elif before_gains is not None: + print(" gains kept (--keep)") + for hj, prev in held_before.items(): + if args.keep: + print(f" held {hj.value} gains kept (--keep)") + continue + await _write_gains(raw[hj]._driver, prev, False) + print(f" held {hj.value} gains restored") + if ( + before_ramps is not None + and not args.keep + and isinstance(driver, DamiaoMotor) + ): + got = await _dm_write_ramps( + driver, before_ramps[0], before_ramps[1], args.persist + ) + print( + f" profiler ACC/DEC restored to {got[0]:g}/{got[1]:g} rad/s²" + ) + elif before_ramps is not None and ramps_used is not None: + print( + f" profiler left at {ramps_used[0]:g}/{ramps_used[1]:g} rad/s² (--keep)" + ) + if before_accel is not None and not args.keep: + got = await _write_accel(driver, before_accel[0], before_accel[1]) + print(f" planner accel/decel restored to {got[0]}/{got[1]} dps/s") + elif before_accel is not None and args.keep: + print( + f" planner left at {accel_used[0]}/{accel_used[1]} dps/s (--keep) — a direct-tracking joint executes a stored target on wake" + ) + except Exception as exc: # noqa: BLE001 - report, then keep tearing down + print(f" ! restore failed: {exc}") + if not homed: + print(" (torque-off will be refused unless every joint is at rest)") + await _safe_torque_off(motors, raw) + + imu.stop() + if len(log) < 20: + print("\nToo few samples to score.") + return + metrics = a4_metrics(log, args.rate) + metrics["aborted"] = reason is not None + imu_metrics: dict[str, dict[str, float]] = {} + imu_series: dict[str, np.ndarray] = {} + if stream_origin is not None: + imu_metrics, imu_series = imu.run_blocks( + stream_origin, stream_origin + float(log[-1]["t"]), stream_origin + ) + if imu_metrics: + metrics["imu"] = imu_metrics + if held_scores: + metrics["held"] = held_scores + if ring_scores: + metrics["ring"] = {"hz": ring_at, "joints": ring_scores} + print(f"\n{'─' * 66}") + print( + f" tracking RMS {math.degrees(metrics['rms']):.3f}° max {math.degrees(metrics['max']):.3f}° lag {metrics['lag_ms']:.0f} ms" + ) + print( + f" 1-4 Hz band {math.degrees(metrics['band_1_4']):.3f}° >10 Hz buzz {math.degrees(metrics['buzz']):.3f}°" + ) + print( + f" velocity ripple {metrics['v_ripple']:.2f} (MIT stick-slip ≈ 0.8, smooth < 0.2) stuck windows {metrics['stuck_frac']:.2f}" + ) + print( + f" {current_label} RMS {metrics['iq_rms']:.2f} {current_unit} " + f"peak {metrics['iq_max']:.2f} {current_unit} " + f"spread {metrics['iq_sd']:.2f} {current_unit} " + f"3-8 Hz mode {metrics['iq_mode']:.2f} {current_unit} loop {metrics['hz']:.0f} Hz" + ) + for line in format_imu(imu_metrics): + print(line) + print(f"{'─' * 66}") + if args.save_run: + params = { + "wire": "a4", + "mode": args.mode, + "center_deg": math.degrees(center), + "amp_deg": args.amp, + "freq_hz": args.freq if args.mode == "sine" else None, + "speed_dps": args.speed if args.mode == "triangle" else None, + "duration_s": args.duration, + "rate_hz": args.rate, + "cap_dps": args.cap, + "cap_track": cap_track, + "cap_floor_dps": args.cap_floor, + "lead_ms": args.lead_ms, + "accel": list(accel_used) if accel_used else None, + "vendor": "damiao" if is_dm else "myactuator", + "dm_acc": list(ramps_used) if ramps_used else None, + "persist": args.persist, + "pose": args.pose or None, + "held_gains": held_used or None, + } + run_id = save_run( + "sine", + {**log_to_series(log), **held_series(held_log, held_dyn), **imu_series}, + metrics, + side=side, + joint=joint.value, + gains=used_gains, + params=params, + label=args.label, + group=args.group, + ) + print(f"\nSaved tuning run {run_id} (kind=sine, wire=a4)") diff --git a/almond_axol/cli/tune/breakaway.py b/almond_axol/cli/tune/breakaway.py new file mode 100644 index 00000000..64362161 --- /dev/null +++ b/almond_axol/cli/tune/breakaway.py @@ -0,0 +1,642 @@ +""" +axol tune.breakaway + +Measure a joint's **breakaway** (static) torque — the torque at which a +stationary joint first moves — and compare it to the sliding Coulomb torque +``fc`` the friction model already knows. + +Why this exists, and why ``tune.friction`` cannot answer it: + +* ``tune.friction`` sweeps at fixed velocities and fits + ``fc·tanh(0.1·k·v) + fv·v``. That model is monotonic in ``v`` and has no + static/kinetic distinction — it cannot represent breakaway at any + parameter value. +* Its sweep bottoms out around 0.13 rad/s, and one LSB of reported motor + velocity is ~0.022 rad/s. Creep at a few thousandths of a rad/s is below + the velocity channel entirely, so a "slower sweep" fits torque against a + signal that reads zero. + +Stick-slip exists precisely *because* breakaway exceeds sliding friction: a +joint tracking a slow target sticks until ``kp·err`` makes up the gap, lets +go, overshoots, and re-sticks — the 2 Hz, ~0.5° stairs on the X8-P20 +shoulders. That gap is what this measures, and it needs no velocity signal at +all — only the torque at the instant position moves. + +Method, per pose: + +1. Home the arm, apply the shared sweep-safety clearances, and ramp the test + joint to the pose under impedance control. +2. Drop to ``kp = 0`` — no position spring — with a small ``kd`` so the joint + cannot run away once it releases. Gravity feedforward is then the only + thing holding it. +3. Trim that feedforward until the joint stands still. A joint sits still for + any trim inside its stiction band, so this converges quickly; without it + loaded poses are unmeasurable (at 15 Nm a 2 % gravity-model error + outweighs the whole breakaway torque). +4. Ramp an extra torque up and back down in a triangle and watch for the + first motion past ``--move-deg``. Peaks escalate over attempts and the + search stops at the first release, so the joint never sees more torque + than it took to move it; the instant it moves it is caught under ``kp`` + and returned to the pose. +5. Repeat in both directions, ``--trials`` times each. + +Probing both directions separates two things one average hides — the +symmetric half is friction, the antisymmetric half is whatever standing +torque the trimmed feedforward still missed: + +* **F_static** and ``F_static / fc``. The excess over ``fc`` is the torque a + pure velocity feedforward can never supply while the joint is stuck, and + ``(F_static − fc) / kp`` is the predicted stick-slip stair height. If it + matches the stairs a slow ``tune.motion`` replay shows, the diagnosis is + closed. +* ``stiction_gain`` (see ``JointConfig``) should stay **below** + ``F_static / fc − 1`` … the compensation must not exceed the friction it + is compensating, or the joint hunts around the target at rest. + +Examples: + axol tune.breakaway --r --joint shoulder_1 + axol tune.breakaway --r --joint shoulder_2 --trials 5 --csv ~/breakaway-s2.csv + axol tune.breakaway --r --joint shoulder_1 --poses -30 0 30 # load dependence +""" + +from __future__ import annotations + +import argparse +import asyncio +import csv +import math +import time +from dataclasses import dataclass, field +from pathlib import Path + +import numpy as np + +from ...constants import ARM_JOINTS, Joint +from ...motor import CanBus, ControlMode, Motor, MotorError +from ...robot.config import ArmConfig, AxolConfig +from ...robot.gravity import GravityCompensator +from ...tuning import ( + JointFrameMotor, + joint_frame_motors, + ramp_impedance, + ramp_stages, + safe_limits, + safe_outboard_direction, + sweep_safety, +) +from ..motor import add_side_and_channel_arguments, resolve_channel +from .friction import _home_all, _ramp_verified, _safe_torque_off + +_RATE_HZ = 100.0 +#: Drift (rad) over one trim hold below which the joint counts as standing +#: still: ~1.4 LSB of the 16-bit MIT position, i.e. the noise floor. +_HOLD_TOL = math.radians(0.03) +#: Duration of one kp = 0 trim hold, and the settle time skipped at its start. +_TRIM_HOLD_S = 1.0 +_TRIM_SETTLE_S = 0.25 +#: Displacement (rad) from the pose at which a kp = 0 phase is abandoned and +#: the joint is caught under kp — the "it let go" guard for the trim search +#: and the backstop for a release the ramp somehow missed. +_CATCH = math.radians(3.0) +#: Joints with a base-collision boundary at 0 are probed at least this far +#: outboard so a release toward the boundary cannot cross it. +_BOUNDARY_MARGIN = math.radians(5.0) +#: Floor on the friction level the escalation schedule is scaled by, so the +#: low-friction Damiao wrists (fc ≈ 0.1 Nm) still get a usable ramp. +_FC_FLOOR_NM = 0.2 +#: Escalating ramp peaks as multiples of the (floored) fc, cut at --max-torque. +_PEAK_MULTIPLES = (0.5, 0.75, 1.0, 1.25, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0) + + +def peak_schedule(fc: float, max_multiple: float) -> list[float]: + """Ramp peaks (Nm) to try in order: ``_PEAK_MULTIPLES × max(fc, floor)``, + keeping every peak at or below ``max_multiple × max(fc, floor)``.""" + ref = max(fc, _FC_FLOOR_NM) + return [m * ref for m in _PEAK_MULTIPLES if m <= max_multiple + 1e-9] + + +def triangle(t: float, ramp_s: float) -> float: + """Unit triangle: 0 → 1 at ``ramp_s / 2`` → 0 at ``ramp_s``; 0 outside.""" + if t <= 0.0 or t >= ramp_s: + return 0.0 + half = ramp_s / 2.0 + return t / half if t <= half else (ramp_s - t) / half + + +def split_breakaway(plus: list[float], minus: list[float]) -> tuple[float, float]: + """``(F_static, bias)`` from the released torques in each direction. + + With the joint held by ``g_model + trim`` and the true standing torque + ``g_true``, the residual ``r = g_model + trim − g_true`` helps one + direction and hinders the other: ``b+ = F_static − r``, + ``b− = F_static + r``. So the symmetric half is friction and the + antisymmetric half is the residual the trim search left inside the + stiction band (``r`` is what the feedforward *over*-supplies in the + + direction). + """ + bp = float(np.mean(plus)) + bm = float(np.mean(minus)) + return (bp + bm) / 2.0, (bm - bp) / 2.0 + + +def predicted_stair_deg(f_static: float, fc: float, kp: float) -> float: + """Stick-slip stair height (deg) a pure velocity feedforward leaves: + the joint sticks until ``kp·err`` covers what ``fc`` did not.""" + if kp <= 0.0: + return math.nan + return math.degrees(max(f_static - fc, 0.0) / kp) + + +@dataclass +class TrimSearch: + """Bisection-style search for the feedforward trim that holds the joint. + + Feed one drift measurement (rad over a hold) per step; :attr:`trim` is the + correction to apply next. The step shrinks whenever the drift changes + sign, so the search closes on an edge of the stiction band. ``done`` is + set on a hold that stood still; ``failed`` when the trim runs away + (gravity model far off, or the joint is not actually free). + """ + + step: float + max_trim: float + trim: float = 0.0 + done: bool = False + failed: bool = False + _last_sign: int = 0 + steps: int = field(default=0) + + def update(self, drift: float) -> float: + if abs(drift) < _HOLD_TOL: + self.done = True + return self.trim + sign = 1 if drift > 0 else -1 + if self._last_sign and sign != self._last_sign: + self.step /= 2.0 + self._last_sign = sign + # Drifting + means the applied torque exceeds the standing load. + self.trim -= sign * self.step + self.steps += 1 + if abs(self.trim) > self.max_trim or self.steps > 16: + self.failed = True + return self.trim + + +async def _hold( + motor: JointFrameMotor, + pose: float, + kp: float, + kd: float, + t_ff: float, + duration: float, +) -> None: + """Command ``(pose, kp, kd, t_ff)`` at the probe rate for ``duration``.""" + period = 1.0 / _RATE_HZ + deadline = time.perf_counter() + end = deadline + duration + while deadline < end: + deadline += period + await motor.set_impedance(pose, 0.0, kp, kd, t_ff) + await asyncio.sleep(max(0.0, deadline - time.perf_counter())) + + +async def _catch( + motor: JointFrameMotor, + pose: float, + kp: float, + kd: float, + gravity_fn, +) -> None: + """Take a released joint back under ``kp`` and return it to ``pose``.""" + here = motor.position + await _hold(motor, here, kp, kd, gravity_fn(here), 0.4) + await ramp_impedance(motor, kp, kd, pose, gravity_fn, rate_hz=_RATE_HZ) + await _hold(motor, pose, kp, kd, gravity_fn(pose), 0.5) + + +async def _trim( + motor: JointFrameMotor, + pose: float, + kp: float, + kd: float, + kd_probe: float, + gravity_fn, + fc_ref: float, + trim0: float = 0.0, +) -> float | None: + """Find the feedforward trim (Nm) that holds the joint still at kp = 0.""" + search = TrimSearch(step=0.15 * fc_ref, max_trim=3.0 * fc_ref, trim=trim0) + g = gravity_fn(pose) + while True: + period = 1.0 / _RATE_HZ + deadline = time.perf_counter() + t0 = deadline + p_start: float | None = None + caught = False + while time.perf_counter() - t0 < _TRIM_HOLD_S: + deadline += period + await motor.set_impedance(pose, 0.0, 0.0, kd_probe, g + search.trim) + pos = motor.position + if abs(pos - pose) > _CATCH: + caught = True + break + if p_start is None and time.perf_counter() - t0 >= _TRIM_SETTLE_S: + p_start = pos + await asyncio.sleep(max(0.0, deadline - time.perf_counter())) + if caught: + drift = motor.position - pose + await _catch(motor, pose, kp, kd, gravity_fn) + else: + drift = motor.position - (p_start if p_start is not None else pose) + trim = search.update(drift) + print( + f" trim {trim:+.3f} Nm drift {math.degrees(drift):+.3f}°" + f"{' (let go — caught)' if caught else ''}" + ) + if search.done: + return trim + if search.failed: + return None + + +async def _release( + motor: JointFrameMotor, + pose: float, + direction: int, + peak: float, + ramp_s: float, + kd_probe: float, + hold_ff: float, + move_thr: float, + writer: csv.writer | None, + tag: tuple, +) -> float | None: + """One triangle ramp of ``direction × peak``; the extra torque at first + motion past ``move_thr``, or ``None`` if the joint never moved.""" + period = 1.0 / _RATE_HZ + t0 = time.perf_counter() + deadline = t0 + pos0 = motor.position + while True: + t = time.perf_counter() - t0 + if t >= ramp_s: + return None + deadline += period + extra = direction * peak * triangle(t, ramp_s) + await motor.set_impedance(pose, 0.0, 0.0, kd_probe, hold_ff + extra) + pos = motor.position + moved = pos - pos0 + try: + tau = motor.torque + except MotorError: + tau = math.nan + if writer is not None: + writer.writerow( + [ + *tag, + f"{t:.4f}", + f"{extra:.5f}", + f"{math.degrees(pos):.5f}", + f"{tau:.4f}", + ] + ) + if abs(moved) > move_thr: + return abs(extra) + await asyncio.sleep(max(0.0, deadline - time.perf_counter())) + + +def add_parser(subparsers: argparse._SubParsersAction) -> None: # type: ignore[type-arg] + """Register the ``tune.breakaway`` subcommand.""" + p = subparsers.add_parser( + "tune.breakaway", + help="Measure a joint's static (breakaway) friction against its sliding fc.", + formatter_class=argparse.RawDescriptionHelpFormatter, + description=__doc__, + ) + add_side_and_channel_arguments(p) + p.add_argument( + "--joint", + required=True, + choices=[j.value for j in ARM_JOINTS], + help="Joint to probe", + ) + p.add_argument( + "--poses", + type=float, + nargs="+", + default=None, + metavar="DEG", + help="Joint-frame poses (degrees, 0 = rest) to probe at (default: 0; " + "base-collision joints are pushed 5° outboard of their boundary)", + ) + p.add_argument( + "--trials", type=int, default=3, help="Releases per direction (default: 3)" + ) + p.add_argument( + "--ramp-s", + type=float, + default=4.0, + help="Seconds for one up-and-back torque ramp (default: 4)", + ) + p.add_argument( + "--max-torque", + type=float, + default=3.0, + help="Largest ramp peak to try, as a multiple of the joint's fc " + "(floored at 0.2 Nm; default: 3.0)", + ) + p.add_argument( + "--move-deg", + type=float, + default=0.06, + help="Motion (degrees) that counts as a release — about 3 LSB of the " + "16-bit MIT position (default: 0.06)", + ) + p.add_argument( + "--kd", + type=float, + default=1.0, + help="Firmware damping during the kp = 0 phases, so a released joint " + "creeps rather than runs (default: 1.0)", + ) + p.add_argument( + "--csv", type=Path, default=None, help="Dump every ramp sample to this CSV" + ) + p.set_defaults(func=run) + + +def run(args: argparse.Namespace) -> None: + asyncio.run(_run(args)) + + +def _probe_poses( + joint: Joint, is_left: bool, requested: list[float] | None +) -> list[float]: + """Requested poses (rad), clamped inside the safe range with a margin, and + pushed outboard of a base-collision boundary.""" + lo, hi = safe_limits(joint, is_left) + margin = _CATCH + math.radians(2.0) + lo_ok, hi_ok = lo + margin, hi - margin + outboard = safe_outboard_direction(joint, is_left) + if outboard is not None: + if outboard > 0: + lo_ok = max(lo_ok, _BOUNDARY_MARGIN) + else: + hi_ok = min(hi_ok, -_BOUNDARY_MARGIN) + poses = [math.radians(d) for d in (requested if requested is not None else [0.0])] + out = [] + for q in poses: + clamped = min(max(q, lo_ok), hi_ok) + if abs(clamped - q) > 1e-9: + print( + f" ! pose {math.degrees(q):+.1f}° moved to {math.degrees(clamped):+.1f}° " + "(safe range with catch margin)" + ) + out.append(clamped) + return out + + +async def _run(args: argparse.Namespace) -> None: + joint = Joint(args.joint) + is_left = args.l + side = "left" if is_left else "right" + resolved = AxolConfig().resolved() + arm_cfg: ArmConfig = resolved.left if is_left else resolved.right + gains = getattr(arm_cfg, joint.value) + kp, kd = gains.kp, gains.kd + fc = gains.friction.fc + fc_ref = max(fc, _FC_FLOOR_NM) + peaks = peak_schedule(fc, args.max_torque) + move_thr = math.radians(args.move_deg) + poses = _probe_poses(joint, is_left, args.poses) + joint_index = ARM_JOINTS.index(joint) + + print(f"\nAxol breakaway (static friction) probe — {side} {joint.value}") + print( + f" config: kp={kp:g} kd={kd:g} fc={fc:.3f} Nm " + f"stiction_gain={gains.stiction_gain:g}" + ) + print( + f" ramp peaks: {', '.join(f'{p:.2f}' for p in peaks)} Nm over {args.ramp_s:g} s; " + f"release at {args.move_deg:g}°; probe kd={args.kd:g}" + ) + + gravity_comp = GravityCompensator(resolved) + other_targets, _lo, _hi, notes = sweep_safety(joint, is_left) + hold_q = np.zeros(len(ARM_JOINTS), dtype=np.float32) + for j, q in other_targets.items(): + hold_q[ARM_JOINTS.index(j)] = q + + def gravity_fn(q: float) -> float: + arm_q = hold_q.copy() + arm_q[joint_index] = q + return float(gravity_comp.gravity_arm(arm_q, is_left=is_left)[joint_index]) + + writer = csv_file = None + if args.csv is not None: + csv_file = open(args.csv, "w", newline="", encoding="utf-8") + writer = csv.writer(csv_file) + writer.writerow( + [ + "joint", + "side", + "pose_deg", + "direction", + "trial", + "peak_nm", + "t_s", + "extra_nm", + "pos_deg", + "tau_nm", + ] + ) + + results: list[dict] = [] + channel = resolve_channel(args) + async with CanBus(channel) as bus: + raw_motors = {j: Motor(bus, j) for j in ARM_JOINTS} + await asyncio.gather(*[m.enable() for m in raw_motors.values()]) + motors = await joint_frame_motors(raw_motors, is_left) + await asyncio.gather( + *[ + m.set_control_mode(ControlMode.POSITION_VELOCITY) + for m in motors.values() + ] + ) + motor = motors[joint] + try: + print(" Homing all joints to rest (distal to proximal) ...") + await _home_all(motors) + for note in notes: + print(f" {note}") + for stage in ramp_stages(other_targets): + await _ramp_verified(motors, stage) + await motor.set_control_mode(ControlMode.IMPEDANCE) + await asyncio.sleep(1.0) + # Prime the feedback cache at the current pose before any kp = 0. + here = await motor.get_position() + await _hold(motor, here, kp, kd, gravity_fn(here), 0.3) + + for pose in poses: + print( + f"\n pose {math.degrees(pose):+.1f}° (gravity model {gravity_fn(pose):+.2f} Nm)" + ) + await ramp_impedance(motor, kp, kd, pose, gravity_fn, rate_hz=_RATE_HZ) + await _hold(motor, pose, kp, kd, gravity_fn(pose), 0.5) + print(" trimming the hold (kp = 0) ...") + trim = await _trim(motor, pose, kp, kd, args.kd, gravity_fn, fc_ref) + if trim is None: + print( + " ! could not trim the joint still — gravity model too far off here; skipping pose" + ) + await _catch(motor, pose, kp, kd, gravity_fn) + continue + hold_ff = gravity_fn(pose) + trim + found: dict[int, list[float]] = {+1: [], -1: []} + for direction in (+1, -1): + start = 0 + for trial in range(args.trials): + released: float | None = None + for i in range(start, len(peaks)): + peak = peaks[i] + tag = ( + joint.value, + side, + f"{math.degrees(pose):.2f}", + direction, + trial, + f"{peak:.3f}", + ) + released = await _release( + motor, + pose, + direction, + peak, + args.ramp_s, + args.kd, + hold_ff, + move_thr, + writer, + tag, + ) + if released is not None: + print( + f" {'+' if direction > 0 else '-'} trial {trial + 1}: " + f"released at {released:.3f} Nm (ramp peak {peak:.2f})" + ) + found[direction].append(released) + start = max(0, i - 1) + await _catch(motor, pose, kp, kd, gravity_fn) + # The catch may have landed on a different + # edge of the stiction band; re-verify the trim. + trim = await _trim( + motor, + pose, + kp, + kd, + args.kd, + gravity_fn, + fc_ref, + trim0=trim, + ) + if trim is None: + raise RuntimeError("lost the hold trim mid-probe") + hold_ff = gravity_fn(pose) + trim + break + await _hold(motor, pose, 0.0, args.kd, hold_ff, 0.3) + if released is None: + print( + f" {'+' if direction > 0 else '-'} trial {trial + 1}: " + f"no release up to {peaks[-1]:.2f} Nm" + ) + if found[+1] and found[-1]: + f_static, bias = split_breakaway(found[+1], found[-1]) + stair = predicted_stair_deg(f_static, fc, kp) + results.append( + { + "pose_deg": math.degrees(pose), + "trim_nm": trim, + "plus": found[+1], + "minus": found[-1], + "f_static": f_static, + "bias": bias, + "ratio": f_static / fc if fc > 0 else math.nan, + "stair_deg": stair, + } + ) + else: + results.append( + { + "pose_deg": math.degrees(pose), + "trim_nm": trim, + "plus": found[+1], + "minus": found[-1], + } + ) + except KeyboardInterrupt: + print("\n Interrupted.") + finally: + if csv_file is not None: + csv_file.close() + print(" Returning to rest and disabling ...") + in_impedance = motor.motor.mode == ControlMode.IMPEDANCE + if in_impedance: + try: + here = motor.position + await _hold(motor, here, kp, kd, gravity_fn(here), 0.3) + await ramp_impedance( + motor, kp, kd, 0.0, gravity_fn, rate_hz=_RATE_HZ + ) + except Exception: # noqa: BLE001 - best-effort teardown + pass + try: + await _home_all(motors, exclude=joint if in_impedance else None) + except Exception as exc: # noqa: BLE001 - reported, arm keeps holding + print(f" ! return to rest did not complete: {exc}") + await _safe_torque_off(motors) + + _report(results, fc, kp, gains.stiction_gain) + + +def _report(results: list[dict], fc: float, kp: float, stiction_gain: float) -> None: + print(f"\n{'─' * 72}") + if not results: + print(" No poses probed.") + return + print( + f" {'pose':>7s} {'trim':>7s} {'F_static':>9s} {'bias':>7s} {'F_s/fc':>7s} {'stair':>7s} releases + / -" + ) + for r in results: + if "f_static" not in r: + print( + f" {r['pose_deg']:+7.1f} {r['trim_nm']:+7.3f} (incomplete: + {r['plus']} - {r['minus']})" + ) + continue + print( + f" {r['pose_deg']:+7.1f} {r['trim_nm']:+7.3f} {r['f_static']:9.3f} {r['bias']:+7.3f} " + f"{r['ratio']:7.2f} {r['stair_deg']:6.3f}° " + f"{' '.join(f'{v:.2f}' for v in r['plus'])} / {' '.join(f'{v:.2f}' for v in r['minus'])}" + ) + complete = [r for r in results if "f_static" in r] + if complete: + f_static = float(np.mean([r["f_static"] for r in complete])) + ratio = f_static / fc if fc > 0 else math.nan + print( + f"\n F_static ≈ {f_static:.3f} Nm vs sliding fc {fc:.3f} Nm (ratio {ratio:.2f})" + ) + print( + f" predicted stick-slip stair at kp={kp:g}: " + f"{predicted_stair_deg(f_static, fc, kp):.3f}° per stick" + ) + if fc > 0 and ratio > 1.05: + print( + f" stiction_gain ceiling (F_static/fc − 1): {ratio - 1:.2f}; " + f"currently {stiction_gain:g}. Try 60-80 % of the ceiling with " + "tune.motion --gain ..stiction_gain=…" + ) + elif fc > 0: + print( + " breakaway is at or below fc — the velocity feedforward already " + "covers static friction here; stiction_gain would over-compensate." + ) + print(f"{'─' * 72}") diff --git a/almond_axol/cli/tune/friction.py b/almond_axol/cli/tune/friction.py index 67df58d0..56f2200d 100644 --- a/almond_axol/cli/tune/friction.py +++ b/almond_axol/cli/tune/friction.py @@ -43,7 +43,7 @@ from scipy.optimize import curve_fit from ...constants import ARM_JOINTS -from ...motor import CanBus, ControlMode, Joint, Motor +from ...motor import CanBus, ControlMode, Joint, Motor, MotorError from ...robot.axol import arm_limits from ...robot.calibration import CALIBRATION_PATH, update_joint_calibration from ...robot.config import ArmConfig, AxolConfig @@ -131,6 +131,30 @@ async def _ramp_verified( joints = list(targets) if not joints: return + # Read before commanding. The read re-derives each fixed-stop joint's + # ±360° boot wrap (see JointFrameMotor) — the MyActuator reset that + # precedes every ramp can leave a reading a full turn off, and a command + # against it drives the motor a full turn (right elbow into its hard + # stop at 40 Nm, 2026-09-22). Then refuse anything still implausible. + pre = await asyncio.gather(*[motors[j].get_position() for j in joints]) + bad = [] + for j, pos in zip(joints, pre): + is_left = getattr(motors[j], "_is_left", None) + if is_left is None: + continue + lo, hi = arm_limits(j, is_left) + if not (lo - _RAMP_SANITY_SLACK <= pos <= hi + _RAMP_SANITY_SLACK): + bad.append( + f"{j.value} reads {math.degrees(pos):+.1f}° (limits " + f"[{math.degrees(lo):+.0f}, {math.degrees(hi):+.0f}]°)" + ) + if bad: + raise RuntimeError( + "refusing to ramp — implausible joint reading(s): " + + ", ".join(bad) + + " — a multi-turn wrap or an unset zero; power-cycle or reset " + "the motor and re-run `axol motor.set-zero-pos --guided` if it persists" + ) positions: list[float] = [] for _attempt in range(2): await asyncio.gather( @@ -162,18 +186,93 @@ async def _ramp_verified( ) +#: A joint this far (rad) from its rest pose still carries gravity load; the +#: tuners refuse to reset/disable it and leave it holding instead. +_REST_TOL = math.radians(5.0) +#: A joint reading this far outside its arm limits is not a position, it is +#: a wrapped multi-turn count or an unset zero — never ramp from it. +_RAMP_SANITY_SLACK = math.radians(15.0) + + +async def _safe_torque_off( + motors: dict[Joint, JointFrameMotor], raw: dict[Joint, Motor] | None = None +) -> bool: + """Reset every joint to IMPEDANCE and disable — but only when every arm + joint is within ``_REST_TOL`` of rest. + + The MyActuator mode switch is a firmware reset (torque drops for ~2 s) + and disable is torque-off; a joint that did not make it home would fall. + That happened on the elbow after a 0xA4 probe whose return climb stalled + on the stock firmware position loop. When a joint is off rest this leaves + every motor holding its last command, says which joint and where, and + returns ``False`` so the caller can tell the operator what to do. + """ + off_rest: list[str] = [] + for j, m in motors.items(): + try: + pos = await m.get_position() + except MotorError: + off_rest.append(f"{j.value} (no position reply)") + continue + if abs(pos) > _REST_TOL: + off_rest.append(f"{j.value} at {math.degrees(pos):+.1f}°") + if off_rest: + print( + " ! NOT disabling: " + + ", ".join(off_rest) + + " — still under gravity load. Motors are left holding their last " + "command. Home the arm (gravity-comp, or hand-guide it to rest) before " + "powering down." + ) + return False + await asyncio.gather( + *[m.set_control_mode(ControlMode.IMPEDANCE) for m in motors.values()] + ) + await asyncio.gather( + *[ + m.disable() + for m in (raw or {j: m.motor for j, m in motors.items()}).values() + ] + ) + return True + + +#: How far inside its range a joint is parked when its rest pose (0) sits on +#: a hard stop. Held on a firmware position loop *at* the stop, the loop +#: leans on the stop, the motor's stall protection cuts its output and the +#: joint hangs limp: the right elbow (limits −150..0) sagged 2–6° and swung +#: with whatever else was moving during every wrist run (2026-09-22). Two +#: degrees inside is still within ``_REST_TOL`` of rest for the torque-off. +_STOP_STANDOFF = math.radians(2.0) + + +def rest_target(joint: Joint, is_left: bool | None) -> float: + """The hold target for a homed joint: 0, or 2° inside a limit that is 0.""" + if is_left is None: + return 0.0 + lo, hi = arm_limits(joint, is_left) + if abs(hi) < _STOP_STANDOFF: + return hi - _STOP_STANDOFF + if abs(lo) < _STOP_STANDOFF: + return lo + _STOP_STANDOFF + return 0.0 + + async def _home_all( motors: dict[Joint, JointFrameMotor], exclude: Joint | None = None ) -> None: - """Ramp every joint to 0 (the rest pose), one at a time in ``_HOME_ORDER``. + """Ramp every joint to rest, one at a time in ``_HOME_ORDER``. - Joints already at rest verify in one poll, so a mostly-homed arm costs - a fraction of a second per joint. + Rest is 0, except a joint whose 0 is a hard stop, which parks 2° inside + (see :func:`rest_target`). Joints already at rest verify in one poll, so + a mostly-homed arm costs a fraction of a second per joint. """ for j in _HOME_ORDER: if j == exclude or j not in motors: continue - await _ramp_verified(motors, {j: 0.0}) + await _ramp_verified( + motors, {j: rest_target(j, getattr(motors[j], "_is_left", None))} + ) async def _run_sweep_raw( @@ -379,6 +478,8 @@ async def _identify_joint( lo_override: float | None = None, hi_override: float | None = None, dump_csv: Path | None = None, + raw_csv: Path | None = None, + n_bins: int = _N_BINS, ) -> tuple[list[tuple[float, float]], list[tuple[float, float]]]: """Run bidirectional multi-velocity sweep over the full joint range. @@ -389,6 +490,11 @@ async def _identify_joint( If ``dump_csv`` is given, every matched (fwd, bwd) bin is also written to a CSV with the per-velocity, per-position torque values. Useful for plotting the raw friction-vs-velocity curve and comparing arms. + + ``raw_csv`` writes every cruise sample (``q``, ``tau``, speed, direction) + unbinned — the input for a position-periodic (cogging / gear-mesh) torque + analysis, which needs sub-degree resolution the bins do not keep (see + ``scripts/cogging_map.py``). ``n_bins`` sets the bin count for the fit. """ lo, hi = arm_limits(joint, is_left) if lo_override is not None: @@ -410,9 +516,21 @@ async def _identify_joint( all_avg: list[tuple[float, float]] = [] all_halfdiff: list[tuple[float, float]] = [] + # (q, v, tau_half): the half-difference against the pose it was taken at, + # for the load-dependent fit (gear friction grows with gravity torque). + all_halfdiff_q: list[tuple[float, float, float]] = [] csv_file = None csv_writer = None + raw_file = None + raw_writer = None + if raw_csv is not None: + raw_file = secure_open_new_text(raw_csv, newline="") + raw_writer = csv.writer(raw_file) + raw_writer.writerow( + ["joint", "side", "pass", "v_rad_s", "direction", "q_rad", "tau_nm"] + ) + print(f" Dumping every cruise sample to {raw_csv}") if dump_csv is not None: csv_file = secure_open_new_text(dump_csv, newline="") csv_writer = csv.writer(csv_file) @@ -431,7 +549,7 @@ async def _identify_joint( print(f" Dumping per-bin samples to {dump_csv}") try: - for v in velocities: + for pass_index, v in enumerate(velocities): print(f"\n v = {math.degrees(v):.1f} deg/s ...") # Ramp to sweep start with time proportional to distance @@ -449,11 +567,27 @@ async def _identify_joint( bwd = await _run_sweep_raw(motor, kp, kd, cur, -v, sweep_lo) print(f" bwd: {len(bwd)} samples") + if raw_writer is not None: + side_name = "left" if is_left else "right" + for direction, rows in (("+", fwd), ("-", bwd)): + for q, tau in rows: + raw_writer.writerow( + [ + joint.value, + side_name, + pass_index, + f"{v:.6f}", + direction, + f"{q:.6f}", + f"{tau:.6f}", + ] + ) + raw_file.flush() # type: ignore[union-attr] - fwd_bins = _bin_by_position(fwd, sweep_lo, sweep_hi) - bwd_bins = _bin_by_position(bwd, sweep_lo, sweep_hi) + fwd_bins = _bin_by_position(fwd, sweep_lo, sweep_hi, n_bins) + bwd_bins = _bin_by_position(bwd, sweep_lo, sweep_hi, n_bins) matched = sum(1 for q in fwd_bins if q in bwd_bins) - print(f" {matched}/{_N_BINS} position bins matched") + print(f" {matched}/{n_bins} position bins matched") for q_center, tau_f in fwd_bins.items(): if q_center in bwd_bins: @@ -462,6 +596,7 @@ async def _identify_joint( tau_half = (tau_f - tau_b) / 2.0 all_avg.append((q_center, tau_avg)) all_halfdiff.append((v, tau_half)) + all_halfdiff_q.append((q_center, v, tau_half)) if csv_writer is not None: csv_writer.writerow( [ @@ -483,10 +618,54 @@ async def _identify_joint( finally: if csv_file is not None: csv_file.close() + if raw_file is not None: + raw_file.close() + _identify_joint.last_halfdiff_q = all_halfdiff_q # type: ignore[attr-defined] return all_avg, all_halfdiff +def _fit_load_friction( + samples: list[tuple[float, float, float]], + joint: Joint, + is_left: bool, + other_targets: dict[Joint, float], + k_fixed: float, + fv_fixed: float, +) -> tuple[float, float, float] | None: + """Fit ``(Fc + Fl·|g(q)|)·tanh(0.1·k·v) + Fv·v`` to ``(q, v, tau_half)``. + + ``k`` and ``Fv`` are held at the constant-model fit so the two fits differ + only in how the Coulomb level depends on gravity load. Returns + ``(Fc0, Fl, load_span)`` — the zero-load Coulomb level, its slope per Nm + of gravity torque, and how many Nm of load the sweep spanned. A sweep that + stayed within ~3 Nm of load cannot separate the two and returns ``None``. + """ + if len(samples) < 8: + return None + gc = GravityCompensator() + test_idx = ARM_JOINTS.index(joint) + arm_q = np.zeros(len(ARM_JOINTS), dtype=np.float32) + for j, target in other_targets.items(): + if j in ARM_JOINTS and j != joint: + arm_q[ARM_JOINTS.index(j)] = float(target) + load = np.empty(len(samples)) + for i, (q, _v, _t) in enumerate(samples): + arm_q[test_idx] = float(q) + load[i] = abs(float(gc.gravity_arm(arm_q, is_left=is_left)[test_idx])) + span = float(np.ptp(load)) + if span < 3.0: + return None + v = np.array([s[1] for s in samples]) + tau = np.maximum(np.array([s[2] for s in samples]) - fv_fixed * v, 0.0) + sat = np.tanh(0.1 * k_fixed * v) + # Linear least squares in (Fc0, Fl): tau ≈ sat·Fc0 + (sat·load)·Fl. + A = np.c_[sat, sat * load] + coef, _, _, _ = np.linalg.lstsq(A, tau, rcond=None) + fc0, fl = float(coef[0]), float(coef[1]) + return max(fc0, 0.0), max(fl, 0.0), span + + def add_parser(subparsers: argparse._SubParsersAction) -> None: # type: ignore[type-arg] """Register the ``tune.friction`` subcommand.""" p = subparsers.add_parser( @@ -534,6 +713,21 @@ def add_parser(subparsers: argparse._SubParsersAction) -> None: # type: ignore[ metavar="DEG", help="Override upper joint limit for the sweep (degrees)", ) + p.add_argument( + "--raw-csv", + type=Path, + default=None, + metavar="PATH", + help="Write every cruise sample (q, tau, speed, direction) unbinned — the " + "input for scripts/cogging_map.py, which looks for position-periodic " + "torque (cogging / gear mesh) and builds a feedforward table from it", + ) + p.add_argument( + "--bins", + type=int, + default=_N_BINS, + help=f"Position bins the fwd/bwd matching and fit use (default: {_N_BINS})", + ) p.add_argument( "--dump-csv", nargs="?", @@ -649,6 +843,8 @@ async def _run(args: argparse.Namespace) -> None: if args.hi is not None else hi_default, dump_csv=dump_csv, + raw_csv=args.raw_csv, + n_bins=args.bins, ) if not avg_samples and not halfdiff_samples: @@ -667,12 +863,37 @@ async def _run(args: argparse.Namespace) -> None: Fo_out = Fo_result if Fo_result is not None else 0.0 Fc_out = k_out = Fv_out = 0.0 + Fl_out = 0.0 if friction_result is not None: Fc_out, k_out, Fv_out = friction_result print("\n Fitted friction model: τ = Fc·tanh(0.1·k·v) + Fv·v + Fo") print(f" Fc = {Fc_out:.4f} Nm (Coulomb)") print(f" k = {k_out:.2f} (tanh steepness)") print(f" Fv = {Fv_out:.4f} Nm·s/rad (viscous)") + load_fit = _fit_load_friction( + getattr(_identify_joint, "last_halfdiff_q", []), + joint, + is_left, + other_targets, + k_out, + Fv_out, + ) + if load_fit is not None: + fc0, fl, span = load_fit + print( + f"\n Load-dependent Coulomb (gear friction grows with the torque it carries),\n" + f" fitted over {span:.1f} Nm of gravity-load variation:\n" + f" Fc0 = {fc0:.4f} Nm at zero load, Fl = {fl:.4f} Nm per Nm of gravity\n" + f" → {fc0 + fl * 5:.2f} Nm at 5 Nm, {fc0 + fl * 12:.2f} Nm at 12 Nm " + f"(constant model: {Fc_out:.2f} everywhere)" + ) + if fl > 0.0: + Fc_out, Fl_out = fc0, fl + else: + print( + "\n (load-dependent Coulomb not fitted: the sweep spanned < 3 Nm of " + "gravity load — pose the joint under load, e.g. --lo/--hi, to fit fl)" + ) if friction_result is not None or Fo_result is not None: if args.save and friction_result is None: @@ -693,6 +914,7 @@ async def _run(args: argparse.Namespace) -> None: "k": round(k_out, 2), "fv": round(Fv_out, 4), "fo": round(Fo_out, 4), + "fl": round(Fl_out, 4), }, ) print(f"\n Saved to {path}") @@ -707,7 +929,7 @@ async def _run(args: argparse.Namespace) -> None: ) print( f" FrictionParams(fc={Fc_out:.4f}, k={k_out:.2f}, " - f"fv={Fv_out:.4f}, fo={Fo_out:.4f})," + f"fv={Fv_out:.4f}, fo={Fo_out:.4f}, fl={Fl_out:.4f})," ) print(f"{'─' * 50}") @@ -731,9 +953,6 @@ async def _run(args: argparse.Namespace) -> None: # including the base-collision joints the old flow used to # leave in place. await _home_all(motors, exclude=joint if in_impedance else None) - except Exception: - pass - await asyncio.gather( - *[m.set_control_mode(ControlMode.IMPEDANCE) for m in motors.values()] - ) - await asyncio.gather(*[m.disable() for m in motors.values()]) + except Exception as exc: # noqa: BLE001 - reported, arm keeps holding + print(f" ! return to rest did not complete: {exc}") + await _safe_torque_off(motors) diff --git a/almond_axol/cli/tune/motion.py b/almond_axol/cli/tune/motion.py index 241ef8bb..0486b48e 100644 --- a/almond_axol/cli/tune/motion.py +++ b/almond_axol/cli/tune/motion.py @@ -36,27 +36,52 @@ axol tune.motion --motion reach-and-place --gain shoulder_3.kd_host=8 --label "s3 damp" axol tune.motion --motion reach-and-place --stiffness 0.8 axol tune.motion --motion reach-and-place --ik # drive through the IK solver + axol tune.motion --motion slow_osc --arms right # one arm only + axol tune.motion --motion slow_osc --controller position # firmware loops, 400 Hz """ from __future__ import annotations import argparse import asyncio +import itertools import logging import math import time +from dataclasses import replace +from typing import Any import numpy as np -from ...constants import ARM_JOINTS +from ...constants import ARM_JOINTS, Joint from ...robot import Axol -from ...robot.config import AxolConfig +from ...robot.axol import arm_limits +from ...robot.config import ( + CONTROLLERS, + FAST_IMPEDANCE_HZ, + IMPEDANCE_LOOP_HZ, + IMPEDANCE_RATES, + AxolConfig, + check_firmware_extras, + check_loop_hz, + fast_impedance_joints, +) from ...robot.control import ContactWatchdog from ...tuning import save_run, tracking_metrics from ...tuning.motion import ReferenceMotion, list_motions, load_motion +from ...tuning.wrist_imu import WristImu, format_imu from ...utils.logquiet import quiet_noisy_loggers _PLAN_SPEED = 0.1 * np.pi # rad/s — approach/return trajectory speed + +#: A firmware-loop joint (0xA4 / pv) this far (rad) from its command has left +#: its target: the replay stops and returns to rest. Firmware-loop joints +#: carry no torque telemetry for the contact watchdog, and the core has no +#: position-deviation abort (a pushed impedance joint is normal), so nothing +#: else catches one — right elbow on the 0xA4 planner ended 117° from its +#: command (2026-09-22). ``tune.a4``'s own abort is the same 20°; normal lag +#: is ~1° at the approach speed. +_FW_DEVIATION_ABORT = math.radians(20.0) _PLAN_MIN_DURATION = 1.5 # s _GAIN_FIELDS = ( @@ -66,6 +91,50 @@ "kd_host_hz", "kd_host_q", "j_eff", + "stiction_gain", + "stiction_load_gain", + "stiction_err_deg", + "dither_nm", + "dither_hz", + "stribeck_gain", + "stribeck_dfs", + "stribeck_load_gain", + "stribeck_vs", + "stribeck_pole", + # Friction model, addressed as ``joint.friction.fc`` etc. — the sliding + # friction feedforward is the other half of every stick-slip A/B. + "friction.fc", + "friction.k", + "friction.fv", + "friction.fo", + "friction.fl", + # Firmware position-loop gains (``firmware.*`` on JointConfig), for A/B + # runs of the position controller: written to the motors' ROM at enable + # like the config values they replace (so a run leaves them there). + "firmware.position_kp", + "firmware.position_ki", + "firmware.position_kd", + "firmware.speed_kp", + "firmware.speed_ki", + "firmware.profile_acc", + # MyActuator 0xA4: the position planner (0 direct / 60000) and the + # core's per-tick speed-cap tracking that the planner wants. + "firmware.planner_accel", + "firmware.cap_track", + "firmware.planner_lead_ms", + # MyActuator 0x73: the rated current that scales the torque feedforward + # (set = the joint's position command carries gravity + inertia + + # cogging; unset = plain 0xA4). + "firmware.tf_rated_current_a", + # The cogging ("osc") cancellation's share of the calibrated series. + "cogging_gain", + # The gravity model's per-link inertials (the body this joint drives): + # mass (kg) and centre of mass (m, URDF link frame) — for trying a gravity + # correction before committing it to calibration. + "mass", + "com.x", + "com.y", + "com.z", ) # Column names of a 14-wide motion row: left arm then right arm. @@ -74,6 +143,117 @@ ] +def _parse_holds(specs: list[str]) -> dict[int, float | None]: + """``--hold SIDE.JOINT[=DEG]`` → ``{column: angle rad, or None}``. + + ``None`` holds the joint at the motion's own first-row angle; an angle + must sit inside the arm's joint limits. + """ + out: dict[int, float | None] = {} + for spec in specs: + name, eq, deg = spec.partition("=") + if name not in _COLUMNS: + raise SystemExit( + f"--hold wants SIDE.JOINT[=DEG] with an arm joint, got {spec!r}" + ) + angle: float | None = None + if eq: + try: + angle = math.radians(float(deg)) + except ValueError: + raise SystemExit(f"--hold: bad angle in {spec!r}") from None + side, joint = name.split(".") + lo, hi = arm_limits(Joint(joint), side == "left") + if not lo <= angle <= hi: + raise SystemExit( + f"--hold: {name}={deg}° is outside " + f"[{math.degrees(lo):.0f}, {math.degrees(hi):.0f}]° for that arm" + ) + out[_COLUMNS.index(name)] = angle + return out + + +def _apply_holds( + rows: np.ndarray, holds: dict[int, float | None], first: np.ndarray +) -> np.ndarray: + """A copy of ``rows`` with each held column constant (``first`` = row 0).""" + out = np.array(rows, dtype=float, copy=True) + for col, angle in holds.items(): + out[:, col] = first[col] if angle is None else angle + return out + + +#: A joint further than this (rad, ~3°) from the motion's first row after the +#: approach move did not follow it, and playback must not start from there. +_START_POSE_TOL = 0.05 + + +def retime_measurements( + t: np.ndarray, offsets: np.ndarray, actual: np.ndarray, torque: np.ndarray +) -> tuple[np.ndarray, np.ndarray]: + """Put cache reads back on the command clock. + + ``offsets[k, i]`` is how long before log time ``t[k]`` joint ``i``'s + cached sample was really taken (≤ 0; ``0`` when unknown). Each column + is interpolated from its true sample times ``t + offsets`` back onto + ``t``, so the scorecard compares the target with the measurement at the + same instant instead of with a sample up to one core tick old — the + sawtooth in that age is what a 400 Hz core read at 240 Hz shows as an + 80 Hz buzz on every joint. Duplicate samples (the same tick read twice) + collapse to one point; NaN columns (absent arm) pass through. + """ + if len(t) < 2 or offsets.shape != actual.shape: + return actual, torque + out_a = actual.copy() + out_q = torque.copy() + for i in range(actual.shape[1]): + col = actual[:, i] + if not np.any(np.isfinite(col)) or not np.any(offsets[:, i] != 0.0): + continue + ts = t + offsets[:, i] + keep = np.concatenate([[True], np.diff(ts) > 0]) + keep &= np.isfinite(col) + if keep.sum() < 2: + continue + out_a[:, i] = np.interp( + t, ts[keep], col[keep], left=col[keep][0], right=col[keep][-1] + ) + tq = torque[:, i] + if np.any(np.isfinite(tq)): + kq = keep & np.isfinite(tq) + if kq.sum() >= 2: + out_q[:, i] = np.interp( + t, ts[kq], tq[kq], left=tq[kq][0], right=tq[kq][-1] + ) + return out_a, out_q + + +def start_pose_stragglers( + q_now: np.ndarray, + q_start: np.ndarray, + arms: list[tuple[str, np.ndarray]], + tol: float = _START_POSE_TOL, +) -> list[tuple[str, float]]: + """Joints not at the motion start pose: ``[(column, error_deg), ...]``. + + ``arms`` lists the arms actually driven, ``(side, full-N indices)`` — + an arm left off with ``--arms`` reads as rest and must not be judged. + + The approach move is streamed, not verified — a joint that will not + follow the stream (a ``--a4`` joint whose stored planner acceleration + is neither 0 nor 60000 barely moves, see ``tune.a4``) is silently left + at rest, and replaying from there scores garbage for that joint and + swings the others around a pose the motion never planned for. + """ + out: list[tuple[str, float]] = [] + for side, indices in arms: + for j, idx in zip(ARM_JOINTS, indices): + err = float(q_now[idx] - q_start[idx]) + if abs(err) > tol: + out.append((f"{side}.{j.value}", math.degrees(err))) + return out + + def _parse_gain_overrides(specs: list[str]) -> dict[tuple[str, str, str], float]: """Parse ``--gain [side.]joint.field=value`` into ``{(side, joint, field): v}``. @@ -88,6 +268,10 @@ def _parse_gain_overrides(specs: list[str]) -> dict[tuple[str, str, str], float] value = float(raw) except ValueError: raise SystemExit(f"--gain: bad value in {spec!r} (want PATH=NUMBER)") + # ``[side.]joint.friction.fc`` / ``joint.firmware.speed_kp``: fold the + # sub-field back into one token. + if len(parts) >= 2 and parts[-2] in ("friction", "firmware", "com"): + parts = parts[:-2] + [f"{parts[-2]}.{parts[-1]}"] if len(parts) == 3: sides, joint, fld = [parts[0]], parts[1], parts[2] if sides[0] not in ("left", "right"): @@ -103,11 +287,62 @@ def _parse_gain_overrides(specs: list[str]) -> dict[tuple[str, str, str], float] f"--gain: unknown field {fld!r} in {spec!r} " f"(one of {', '.join(_GAIN_FIELDS)})" ) + if fld == "firmware.tf_rated_current_a": + if joint in ("wrist_2", "wrist_3"): + raise SystemExit( + f"--gain: {fld} scales the MyActuator 0x73 feedforward; {joint} " + "is a Damiao wrist" + ) + if not (math.isfinite(value) and value > 0.0): + raise SystemExit(f"--gain {spec}: the rated current in amps, > 0") + if fld in ( + "firmware.planner_accel", + "firmware.cap_track", + "firmware.planner_lead_ms", + ): + if joint in ("wrist_2", "wrist_3"): + raise SystemExit( + f"--gain: {fld} is the MyActuator 0xA4 planner's; {joint} is a " + "Damiao wrist (its profiler is firmware.profile_acc)" + ) + try: + check_firmware_extras( + value if fld == "firmware.planner_accel" else None, + value if fld == "firmware.cap_track" else None, + value if fld == "firmware.planner_lead_ms" else None, + ) + except ValueError as exc: + raise SystemExit(f"--gain {spec}: {exc}") from None for side in sides: out[(side, joint, fld)] = value return out +def _apply_gain_overrides( + config: AxolConfig, overrides: dict[tuple[str, str, str], float] +) -> None: + """Set each ``(side, joint, field)`` override on ``config``, that joint only. + + The ``friction`` / ``firmware`` blocks are shared instances across the + joints of a motor type (shoulder_1 + shoulder_2, both elbows, ...), so + those are replaced with this joint's own copy, never mutated in place — + an in-place set once carried a shoulder_1 planner override onto + shoulder_2 (2026-09-22). + """ + for (side, joint, fld), value in overrides.items(): + target = getattr(getattr(config, side), joint) + if fld.startswith("friction."): + target.friction = replace(target.friction, **{fld.split(".", 1)[1]: value}) + elif fld.startswith("firmware."): + target.firmware = replace(target.firmware, **{fld.split(".", 1)[1]: value}) + elif fld.startswith("com."): + com = list(target.com) + com["xyz".index(fld[-1])] = value + target.com = tuple(com) + else: + setattr(target, fld, value) + + def add_parser(subparsers: argparse._SubParsersAction) -> None: # type: ignore[type-arg] """Register the ``tune.motion`` subcommand.""" p = subparsers.add_parser( @@ -193,6 +428,114 @@ def add_parser(subparsers: argparse._SubParsersAction) -> None: # type: ignore[ action="store_true", help="Don't persist the run artifact (dry run)", ) + p.add_argument( + "--a4", + action="append", + default=[], + metavar="SIDE.JOINT", + help="Drive this MyActuator joint with the firmware's own position loop " + "(0xA4 absolute position closed-loop) instead of the MIT impedance frame, " + "e.g. right.shoulder_1. Repeatable. The joint then has no compliance, no " + "host feedforward and NaN torque telemetry (contact watchdog blind on it); " + "everything else about the replay is unchanged, so runs compare directly.", + ) + p.add_argument( + "--hold", + action="append", + default=[], + metavar="SIDE.JOINT[=DEG]", + help="Hold this joint steady for the replay instead of following the " + "motion, e.g. right.elbow (at the motion's own start angle) or " + "right.elbow=-75 (at that joint-frame angle, degrees; the approach goes " + "there). Repeatable. The joint keeps its controller and gains, commanded " + "to one pose, and is scored as a parked joint (buzz / chatter only). " + "Only the approach is collision-checked: a joint frozen while the others " + "move can bring links closer than the recording ever did.", + ) + p.add_argument( + "--loop-hz", + type=float, + default=None, + help="Realtime-core tick rate override. Default follows the wire modes: " + "240 Hz all impedance, 400 Hz all firmware loops (--controller " + "position), 480 Hz mixed (--a4 joints every tick, impedance joints on " + "alternate ticks). Impedance (MIT) is commanded at 240 Hz only, so with " + "any arm joint on it only 240 or 480 is accepted. For A/B runs: " + "--controller position --loop-hz 240 separates the rate from the " + "controller. Above 300 Hz the core thins the bus schedule.", + ) + p.add_argument( + "--no-imu", + action="store_true", + help="Do not record the wrist cameras' IMUs (by default each driven arm's " + "wrist ZED X One IMU is recorded and the run gets an 'imu' shake score: " + "1-15 Hz displacement p2p in mm, what the encoders cannot see).", + ) + p.add_argument( + "--fast-impedance", + action="append", + default=[], + metavar="SIDE.JOINT", + help="Run this impedance joint at 480 Hz — every tick of a 480 Hz core " + "loop, its host feedforward, damping and tracker stepped at 480 — while " + "every other impedance joint stays at 240 Hz on alternate ticks, e.g. " + "--fast-impedance right.shoulder_1 --fast-impedance right.elbow. " + "Repeatable. An experiment: the gains were tuned at 240.", + ) + p.add_argument( + "--impedance-hz", + type=float, + choices=IMPEDANCE_RATES, + default=None, + help="Command rate of the MyActuator impedance joints for this run: 240 " + "(the config default, verified) or 480 — every tick of a 480 Hz core " + "loop with the Damiao wrists staying at 240 Hz on alternate ticks. An " + "experiment: the impedance gains, host damping and feedforward were " + "tuned at 240.", + ) + p.add_argument( + "--record", + metavar="PREFIX", + default=None, + help="Flight-recorder prefix, as teleop's --teleop.record: the replay's " + "measured joints go to PREFIX_meas.npz and the realtime core's per-tick " + "trace (target, command, measured position, motor speed, feed-forward " + "terms) to PREFIX_rt.npz for `axol diag.teleop-jitter` or offline " + "analysis. A bare name lands in the recordings directory.", + ) + p.add_argument( + "--controller", + choices=CONTROLLERS, + default=None, + help="Which control law the core runs the arms on for this run. " + "'impedance' (the config default) is the production MIT frame at 240 Hz " + "with the host feedforward; 'position' puts every joint on its motor's " + "own position loop (MyActuator 0xA4, Damiao position-velocity; the " + "firmware.* gains) streamed at 400 Hz — stiff, no host feedforward, " + "NaN torque on the MyActuator joints. --a4 still adds single joints " + "inside the impedance controller.", + ) + p.add_argument( + "--repeat", + type=int, + default=1, + metavar="N", + help="Replay the motion N times back to back in one session (default 1; " + "0 = until Ctrl-C): no homing in between — each pass after the first " + "starts with a planned move back to the start pose if the motion does " + "not end there. Each pass is scored and saved as its own run (label " + "suffixed [k/N]) and a one-line-per-pass summary closes the session; " + "--record captures the whole session in one trace. For soak runs and " + "catching an intermittent buzz.", + ) + p.add_argument( + "--arms", + choices=("both", "left", "right"), + default="both", + help="Which arm(s) to bring up and drive (default: both). The other " + "arm's channel is left untouched, so a single-arm bench or an " + "unpowered arm does not block the run.", + ) p.add_argument( "--no-gripper", action="store_true", @@ -412,9 +755,60 @@ async def _run(args: argparse.Namespace) -> None: right_stiffness=args.stiffness, has_gripper=not args.no_gripper, ) + _apply_gain_overrides(config, overrides) for (side, joint, fld), value in overrides.items(): - setattr(getattr(getattr(config, side), joint), fld, value) print(f" gain override: {side}.{joint}.{fld} = {value}") + for spec in args.a4: + parts = spec.split(".") + if len(parts) != 2 or parts[0] not in ("left", "right"): + raise SystemExit(f"--a4 wants SIDE.JOINT, got {spec!r}") + side, joint = parts + if joint not in {j.value for j in ARM_JOINTS}: + raise SystemExit(f"--a4: unknown joint {joint!r}") + getattr(getattr(config, side), joint).wire_mode = "a4" + print(f" wire mode: {side}.{joint} = a4 (firmware position loop)") + holds = _parse_holds(args.hold) + if args.controller is not None: + config.controller = args.controller + if args.impedance_hz is not None: + config.impedance_hz = args.impedance_hz + for spec in args.fast_impedance: + parts = spec.split(".") + if len(parts) != 2 or parts[0] not in ("left", "right"): + raise SystemExit(f"--fast-impedance wants SIDE.JOINT, got {spec!r}") + side, joint = parts + if joint not in {j.value for j in ARM_JOINTS}: + raise SystemExit(f"--fast-impedance: unknown joint {joint!r}") + getattr(getattr(config, side), joint).impedance_hz = FAST_IMPEDANCE_HZ + print(f" impedance rate: {side}.{joint} = {FAST_IMPEDANCE_HZ:.0f} Hz") + if args.repeat < 0: + raise SystemExit("tune.motion: --repeat must be 0 (until Ctrl-C) or more") + try: + # Before anything touches the bus: impedance runs at 240 Hz only. + check_loop_hz(config, args.loop_hz or config.loop_hz) + except ValueError as exc: + raise SystemExit(f"tune.motion: {exc}") from None + core_hz = args.loop_hz or config.loop_hz + fast = bool(fast_impedance_joints(config)) + mixed = config.controller != "position" and core_hz > IMPEDANCE_LOOP_HZ + print( + f" controller: {config.controller} " + f"({core_hz:.0f} Hz core loop" + + ( + ", every joint on its firmware position loop)" + if config.controller == "position" + else ( + f", {', '.join(fast_impedance_joints(config))} every tick, the other " + f"impedance joints at {IMPEDANCE_LOOP_HZ:.0f} Hz on alternate ticks)" + if fast + else ( + f", impedance joints on alternate ticks at {IMPEDANCE_LOOP_HZ:.0f} Hz)" + if mixed + else ")" + ) + ) + ) + ) # The kinematics stack plans the collision-aware approach/return moves. print("Loading kinematics solver (JIT compile may take a few seconds) ...") @@ -456,13 +850,62 @@ def snapshot(axol: Axol) -> np.ndarray: print(f"Re-solving {len(sent)} waypoints through the IK solver ...") sent = _ik_stream(solver, sent, to_full, stream_info) stream_differs = True + if holds: + # After any IK re-solve, so the solver cannot move a held joint back. + # The scoring reference is held the same way: the joint is scored as + # parked (buzz / chatter), not against the motion it no longer runs. + first = np.asarray(ref[0], dtype=float) + sent = _apply_holds(sent, holds, np.asarray(sent[0], dtype=float)) + ref = _apply_holds(ref, holds, first) + for col, angle in holds.items(): + at = ( + f"{math.degrees(angle):+.1f}°" + if angle is not None + else f"{math.degrees(first[col]):+.1f}° (the motion's start)" + ) + print(f" hold: {_COLUMNS[col]} at {at}") + print( + " ! held joints: only the approach is collision-checked — a frozen " + "joint can bring links closer than the recording did; watch the first pass" + ) watchdog = ContactWatchdog(args.torque_threshold) + # Firmware-loop joints, as (side, index in the arm's 7, name), for the + # deviation guard in execute(). + resolved_cfg = config.resolved() + fw_joints = [ + (side, i, f"{side}.{j.value}") + for side in ("left", "right") + for i, j in enumerate(ARM_JOINTS) + if str(getattr(getattr(resolved_cfg, side), j.value).wire_mode).lower() + in ("a4", "pv") + ] log_t: list[float] = [] + # The same samples on the absolute perf_counter clock: log_t restarts at + # 0 with every execute(), the wrist IMU record does not. + log_abs: list[float] = [] log_target: list[np.ndarray] = [] log_sent: list[np.ndarray] = [] log_actual: list[np.ndarray] = [] log_torque: list[np.ndarray] = [] + # When each measured sample was actually taken on the wire (seconds + # relative to the sample's own log time, ≤ 0): the core refreshes the + # caches at its tick rate, this loop reads them at the motion rate, and + # the varying cache age between the two clocks is a sawtooth that a + # 400 Hz core sampled at 240 Hz turns into an 80 Hz "buzz" on every + # joint. Re-timing each sample removes it. + log_meas_offset: list[np.ndarray] = [] + # [start, end) of each pass's samples in the logs above (--repeat). + passes_run: list[tuple[int, int]] = [] + + def _feedback_offsets(arm: Any, now_wall: float) -> np.ndarray: + out = np.zeros(7, dtype=np.float64) + for i, j in enumerate(ARM_JOINTS): + motor = arm.motors.get(j) + ts = getattr(motor, "_feedback_ts", None) if motor is not None else None + if ts is not None: + out[i] = min(0.0, ts - now_wall) + return out async def execute( axol: Axol, @@ -493,16 +936,31 @@ async def execute( left=left if axol.left is not None else None, right=right if axol.right is not None else None, ) + for side, i, name in fw_joints: + arm = axol.left if side == "left" else axol.right + if arm is None: + continue + cmd = float((left if side == "left" else right)[i]) + off = float(arm.positions[i]) - cmd + if abs(off) > _FW_DEVIATION_ABORT: + raise _Runaway(name, math.degrees(off)) if record: row_a = np.full(14, np.nan, dtype=np.float32) row_tq = np.full(14, np.nan, dtype=np.float32) + row_off = np.zeros(14, dtype=np.float64) + now_wall = time.time() if axol.left is not None: row_a[:7] = axol.left.positions[:7] row_tq[:7] = axol.left.torques[:7] + row_off[:7] = _feedback_offsets(axol.left, now_wall) if axol.right is not None: row_a[7:] = axol.right.positions[:7] row_tq[7:] = axol.right.torques[:7] - log_t.append(time.perf_counter() - t0) + row_off[7:] = _feedback_offsets(axol.right, now_wall) + now = time.perf_counter() + log_t.append(now - t0) + log_abs.append(now) + log_meas_offset.append(row_off) row_cmd = np.concatenate( [q[solver.left_indices], q[solver.right_indices]] ).astype(np.float32) @@ -529,7 +987,23 @@ async def execute( traj_playback = [to_full(row) for row in sent] # Production playback always runs through the Rust core, matching teleop. - robot = Axol(config=config) + arm_channels: dict[str, None] = {} + if args.arms == "right": + arm_channels["left_channel"] = None + elif args.arms == "left": + arm_channels["right_channel"] = None + robot = Axol( + config=config, record=args.record, loop_hz=args.loop_hz, **arm_channels + ) + + # The wrist cameras' IMUs see what the encoders cannot (backlash, flex, + # the gripper itself). Opened before bring-up, so a camera that is slow + # to open costs time while nothing moves; stopped after return to rest. + imu = WristImu( + ["left", "right"] if args.arms == "both" else [args.arms], + enabled=not args.no_imu, + ) + imu.start() async with robot as axol: contact: tuple[str, float] | None = None @@ -541,16 +1015,80 @@ async def execute( if contact is not None: raise _Contact(contact) await asyncio.sleep(0.5) - - print(f"Replaying {motion.duration:.1f} s of motion ...") - contact = await execute( - axol, - traj_playback, - record=True, - refs=ref if stream_differs else None, + driven = [ + (side, idx) + for side, arm, idx in ( + ("left", axol.left, solver.left_indices), + ("right", axol.right, solver.right_indices), + ) + if arm is not None + ] + stragglers = start_pose_stragglers(snapshot(axol), q_start, driven) + if stragglers: + raise _NotAtStart(stragglers) + + # The flight recorder captures the replay segment only, like + # teleop's engage→disengage — one segment for the whole session + # when repeating (each new segment truncates the last), so a + # buzz on the move back to the start is in the trace too. + axol.set_recording_engaged(True) + try: + passes = itertools.count() if args.repeat == 0 else range(args.repeat) + total = "∞" if args.repeat == 0 else str(args.repeat) + for k in passes: + if k > 0: + q_now = snapshot(axol) + if float(np.max(np.abs(q_now - q_start))) > 0.02: + print("Back to the motion start pose ...") + contact = await execute(axol, plan(q_now, q_start)) + if contact is not None: + raise _Contact(contact) + stragglers = start_pose_stragglers( + snapshot(axol), q_start, driven + ) + if stragglers: + raise _NotAtStart(stragglers) + print( + f"Replaying {motion.duration:.1f} s of motion" + + (f" (pass {k + 1}/{total})" if args.repeat != 1 else "") + + " ..." + ) + pass_start = len(log_t) + passes_run.append((pass_start, pass_start)) + try: + contact = await execute( + axol, + traj_playback, + record=True, + refs=ref if stream_differs else None, + ) + finally: + passes_run[-1] = (pass_start, len(log_t)) + if contact is not None: + raise _Contact(contact) + finally: + axol.set_recording_engaged(False) + except _NotAtStart as exc: + print( + "\n ! not at the motion start pose after the approach — playback " + "skipped: " + + ", ".join(f"{name} {err:+.1f}° off" for name, err in exc.stragglers) + ) + if args.a4: + print( + " a --a4 joint that did not follow the approach: check its stored " + "planner acceleration (scripts/fw_gains.py --id ; 0 or 60000 " + "follow a stream, anything in between barely moves) and that the " + "realtime core is built from a checkout that holds a4 joints on the " + "position frame (the X6-P20's 2025-07 firmware ignores 0xA4 after " + "an MIT frame until reset)" + ) + except _Runaway as exc: + print( + f"\n ! {exc.joint} is {exc.deg:+.1f}° from its command on the " + f"firmware loop (limit {math.degrees(_FW_DEVIATION_ABORT):.0f}°) — it " + "has left its target; playback aborted, returning to rest" ) - if contact is not None: - raise _Contact(contact) except _Contact as exc: joint, residual = exc.trip print( @@ -578,14 +1116,14 @@ async def execute( "return-to-rest failed", exc_info=True ) + # A daemon subprocess: an exception out of the block above ends it with + # the process; here it hands over its samples. + imu.stop() if not log_t: print("No playback samples recorded — nothing to score.") return - - t = np.asarray(log_t) - target = np.stack(log_target) - actual = np.stack(log_actual) - torque = np.stack(log_torque) + if not passes_run: + passes_run.append((0, len(log_t))) # Tracking quality is only scored for joints that actually moved (> ~1° # of commanded travel) — a joint parked at rest tracks meaninglessly @@ -601,52 +1139,139 @@ async def execute( "peak_hz", "amplification", ) - per_joint: dict[str, dict[str, float]] = {} - moved: dict[str, dict[str, float]] = {} - for i, name in enumerate(_COLUMNS): - if np.isnan(actual[:, i]).all(): - continue - m = tracking_metrics(t, target[:, i], actual[:, i], torque[:, i]) - if float(np.ptp(target[:, i])) >= math.radians(1.0): - moved[name] = m - else: - for key in _TRACKING_KEYS: - m[key] = math.nan - per_joint[name] = m - if not moved: - print("No joint moved more than 1° — nothing to score.") - return - _print_metrics_table(per_joint) - - worst = max(moved.items(), key=lambda kv: kv[1]["rms_err"]) - summary = { - "per_joint": per_joint, - "worst_joint": worst[0], - "mean_rms_err": float(np.mean([m["rms_err"] for m in moved.values()])), - "mean_jitter": float(np.mean([m["err_band_mid"] for m in moved.values()])), - "completed": bool(len(log_t) >= len(sent)), - } - - if not args.no_save_run: - series = {"t": t, "target": target, "actual": actual, "torque": torque} - if log_sent: - series["sent"] = np.stack(log_sent) - run_id = save_run( - "motion", - series, - summary, - gains={f"{s}.{j}.{f}": v for (s, j, f), v in overrides.items()}, - params={ - "motion": motion.name, - "rate": motion.rate, - "stiffness": args.stiffness, - "columns": _COLUMNS, - **stream_info, - }, - label=args.label, + def score_pass(a: int, b: int, tag: str) -> dict[str, Any] | None: + """Score and save one pass's slice of the logs; its summary, or None. + + ``tag`` ("[k/N] ", empty for a single pass) heads the scorecard and is + appended to the saved run's label. + """ + if b - a < 2: + return None + t = np.asarray(log_t[a:b]) + target = np.stack(log_target[a:b]) + actual = np.stack(log_actual[a:b]) + torque = np.stack(log_torque[a:b]) + actual, torque = retime_measurements( + t, np.stack(log_meas_offset[a:b]), actual, torque ) - print(f"\nSaved tuning run {run_id} (kind=motion, motion={motion.name!r})") + per_joint: dict[str, dict[str, float]] = {} + moved: dict[str, dict[str, float]] = {} + for i, name in enumerate(_COLUMNS): + if np.isnan(actual[:, i]).all(): + continue + m = tracking_metrics(t, target[:, i], actual[:, i], torque[:, i]) + if float(np.ptp(target[:, i])) >= math.radians(1.0): + moved[name] = m + else: + for key in _TRACKING_KEYS: + m[key] = math.nan + per_joint[name] = m + if tag: + print(f"\n{tag.strip()}") + _print_metrics_table(per_joint) + + summary: dict[str, Any] = { + "per_joint": per_joint, + "completed": bool(b - a >= len(sent)), + } + if moved: + worst = max(moved.items(), key=lambda kv: kv[1]["rms_err"]) + summary["worst_joint"] = worst[0] + summary["mean_rms_err"] = float( + np.mean([m["rms_err"] for m in moved.values()]) + ) + summary["mean_jitter"] = float( + np.mean([m["err_band_mid"] for m in moved.values()]) + ) + else: + # A hold (the ``hold`` motion): no tracking to score, but the + # buzz columns and the wrist IMU's floor under the running + # controller are the point of it. + print(f"{tag}No joint moved more than 1° — hold: buzz and IMU only.") + # The pass on the wrist IMUs' clock: its log origin is the execute() + # start, so the IMU series shares the run's time axis. + origin = log_abs[a] - log_t[a] + imu_metrics, imu_series = imu.run_blocks( + origin + float(t[0]), origin + float(t[-1]), origin + ) + if imu_metrics: + summary["imu"] = imu_metrics + for line in format_imu(imu_metrics): + print(line) + + if not args.no_save_run: + series = {"t": t, "target": target, "actual": actual, "torque": torque} + series.update(imu_series) + if log_sent: + series["sent"] = np.stack(log_sent[a:b]) + label = " ".join(x for x in (args.label, tag.strip()) if x) or None + run_id = save_run( + "motion", + series, + summary, + gains={f"{s}.{j}.{f}": v for (s, j, f), v in overrides.items()}, + params={ + "motion": motion.name, + "rate": motion.rate, + "stiffness": args.stiffness, + "columns": _COLUMNS, + # Joints driven on the firmware position loop (--a4) for + # this run, so the dashboard can re-arm the same split. + "a4": list(args.a4), + # Joints held steady instead of following the motion. + "hold": list(args.hold), + "arms": args.arms, + # The control law the whole run ran on (impedance at + # 240 Hz or the firmware position loops at 400 Hz). + "controller": config.controller, + "loop_hz": args.loop_hz or config.loop_hz, + "impedance_hz": config.impedance_hz, + # Joints commanded at 480 Hz (--fast-impedance / the + # config-wide 480). + "fast_impedance": fast_impedance_joints(config), + "record": args.record, + **stream_info, + }, + label=label, + ) + print(f"\nSaved tuning run {run_id} (kind=motion, motion={motion.name!r})") + return summary + + many = len(passes_run) > 1 + summaries = [ + score_pass(a, b, f"[{k + 1}/{len(passes_run)}] " if many else "") + for k, (a, b) in enumerate(passes_run) + ] + if many: + # One line per pass: the intermittent faults (a buzz on one pass in + # five) are what repeating is for. + print(f"\n{'─' * 78}\n passes: worst buzz / mean jitter / worst joint") + for k, sm in enumerate(summaries): + if sm is None: + print(f" [{k + 1}] too short to score") + continue + name, m = max( + sm["per_joint"].items(), + key=lambda kv: ( + kv[1]["buzz"] if math.isfinite(kv[1].get("buzz", math.nan)) else 0.0 + ), + ) + print( + f" [{k + 1}] {math.degrees(m.get('buzz', math.nan)):.3f}° on {name} " + f"@ {m.get('buzz_hz', math.nan):.0f} Hz / " + f"{math.degrees(sm.get('mean_jitter', math.nan)):.3f}° / " + f"{sm.get('worst_joint', 'hold')}" + + ("" if sm["completed"] else " (cut short)") + ) + + +class _Runaway(Exception): + """Internal: a firmware-loop joint left its target (``_FW_DEVIATION_ABORT``).""" + + def __init__(self, joint: str, deg: float) -> None: + self.joint = joint + self.deg = deg class _Contact(Exception): @@ -656,6 +1281,13 @@ def __init__(self, trip: tuple[str, float]) -> None: self.trip = trip +class _NotAtStart(Exception): + """Internal: the approach left joints off the start pose; skip playback.""" + + def __init__(self, stragglers: list[tuple[str, float]]) -> None: + self.stragglers = stragglers + + def _load_motion_or_exit(name: str) -> ReferenceMotion: try: return load_motion(name) diff --git a/almond_axol/cli/tune/pid.py b/almond_axol/cli/tune/pid.py index ead41380..45133339 100644 --- a/almond_axol/cli/tune/pid.py +++ b/almond_axol/cli/tune/pid.py @@ -75,6 +75,7 @@ sine_metrics, step_metrics, ) +from ...tuning.wrist_imu import WristImu, format_imu from ..motor import add_side_and_channel_arguments, resolve_channel @@ -373,6 +374,12 @@ def add_parser(subparsers: argparse._SubParsersAction) -> None: # type: ignore[ f"({CALIBRATION_PATH}); it then overrides the shared defaults on this " "machine", ) + p.add_argument( + "--no-imu", + action="store_true", + help="Do not record the wrist camera's IMU (recorded by default: each " + "candidate gets an 'imu' shake score — 1-15 Hz displacement p2p in mm).", + ) p.add_argument( "--save-run", action="store_true", @@ -596,14 +603,35 @@ def gravity_fn(q: float) -> float: # One shared group id per invocation links the sweep's runs for A/B. run_group = uuid.uuid4().hex[:8] if args.save_run else None + # The wrist camera's IMU, recorded for the whole session; each saved run + # takes its own window of it. + imu = WristImu([side_str], enabled=not args.no_imu) + def _persist_run( - kp: float, kd: float, log: list[dict], metrics: dict, mode_label: str + kp: float, + kd: float, + log: list[dict], + metrics: dict, + mode_label: str, + t_end: float | None = None, ) -> None: + imu_series: dict[str, np.ndarray] = {} + if imu.sides and log and t_end is not None: + # The log's own clock ends where the run returned. + origin = t_end - float(log[-1]["t"]) + imu.flush() + imu_metrics, imu_series = imu.run_blocks( + origin + float(log[0]["t"]), t_end, origin + ) + if imu_metrics: + metrics["imu"] = imu_metrics + for line in format_imu(imu_metrics): + print(line) if not args.save_run or not log: return run_id = save_run( args.mode, - log_to_series(log), + {**log_to_series(log), **imu_series}, metrics, side=side_str, joint=joint.value, @@ -639,6 +667,7 @@ def _persist_run( results: list[dict] = [] ref_kp, ref_kd = candidates[0] + imu.start() async with CanBus(channel) as bus: raw_motors = {j: Motor(bus, j) for j in ARM_JOINTS} await asyncio.gather(*[m.enable() for m in raw_motors.values()]) @@ -850,10 +879,11 @@ async def _hand_session() -> None: ff, relative=True, ) + t_end = time.perf_counter() metrics = step_metrics(log, amp, args.hold) _print_stats_step(metrics, len(log), kp, kd) pose_results.append({"kp": kp, "kd": kd, "metrics": metrics}) - _persist_run(kp, kd, log, metrics, f"step@pose{pose_n}") + _persist_run(kp, kd, log, metrics, f"step@pose{pose_n}", t_end) if csv_writer is not None: for r in log: csv_writer.writerow( @@ -963,6 +993,7 @@ async def _hand_session() -> None: monitor=monitor, center=center_rad, ) + t_end = time.perf_counter() metrics = sine_metrics(log) _print_stats_sine(metrics, len(log), kp, kd) else: @@ -979,6 +1010,7 @@ async def _hand_session() -> None: monitor=monitor, center=center_rad, ) + t_end = time.perf_counter() metrics = step_metrics(log, amp, args.hold) _print_stats_step(metrics, len(log), kp, kd) metrics["holder_wobble_deg"] = monitor.report() @@ -988,7 +1020,7 @@ async def _hand_session() -> None: _print_holder_wobble(metrics["holder_wobble_deg"]) results.append({"kp": kp, "kd": kd, "metrics": metrics}) - _persist_run(kp, kd, log, metrics, args.mode) + _persist_run(kp, kd, log, metrics, args.mode, t_end) if csv_writer is not None: for r in log: csv_writer.writerow( @@ -1096,3 +1128,4 @@ async def _hand_session() -> None: ] ) await asyncio.gather(*[m.disable() for m in motors.values()]) + imu.stop() diff --git a/almond_axol/constants.py b/almond_axol/constants.py index fa562428..7b4aacd3 100644 --- a/almond_axol/constants.py +++ b/almond_axol/constants.py @@ -46,6 +46,13 @@ class Joint(Enum): # cron/systemd, so it must live outside the operator-writable state tree. CAN_BRINGUP_SCRIPT: Path = Path("/etc/almond-axol/can/startup.sh") +# USB-resets the arm hub, then runs CAN_BRINGUP_SCRIPT. The hub's firmware +# keeps up to 10 frames per channel through the driver's link-down reset and +# transmits them on the next open (kernel: "Unexpected unused echo id"), so a +# flap cannot purge frames queued behind a stalled bus; this is what the +# purge runs instead. Written by `axol can.setup`, root-owned like the above. +CAN_RESET_SCRIPT: Path = Path("/etc/almond-axol/can/reset_adapter.sh") + # Mantis handheld data-collection rig: one dual-channel adapter, each channel # wired to a single Damiao gripper (CAN ID 0x08, same as Joint.GRIPPER). CAN_MANTIS_LEFT = "can_mantis_l" diff --git a/almond_axol/motor/damiao.py b/almond_axol/motor/damiao.py index 71082367..8d4d53cf 100644 --- a/almond_axol/motor/damiao.py +++ b/almond_axol/motor/damiao.py @@ -7,6 +7,7 @@ import asyncio import math import struct +from collections.abc import Mapping from dataclasses import dataclass from enum import Enum from typing import Callable @@ -544,6 +545,81 @@ async def get_gains(self) -> MotorGains: position_ki=float(pos_ki), ) + async def ensure_rom_gains( + self, wanted: Mapping[str, float] + ) -> dict[str, tuple[float, float]]: + """Bring the named loop gains to ``wanted`` in flash. + + ``wanted`` maps ``position_kp`` / ``position_ki`` / ``speed_kp`` / + ``speed_ki`` (KP_APR / KI_APR / KP_ASR / KI_ASR) to values, plus + ``profile_acc``, the position-velocity profiler ramp written to ACC + and, negated, to DEC. Each is read first and written only when it + differs beyond float32 rounding; one 0xAA store follows if anything + changed. Writes take effect at once, so no reset is needed. Returns + ``{name: (before, after)}`` (``profile_acc`` reports the ACC side). + """ + regs = { + "speed_kp": _DM_REG_SPEED_KP, + "speed_ki": _DM_REG_SPEED_KI, + "position_kp": _DM_REG_POS_KP, + "position_ki": _DM_REG_POS_KI, + "profile_acc": _DM_REG_ACC, + } + unknown = set(wanted) - set(regs) + if unknown: + raise ValueError(f"Damiao loop has no gain(s) {sorted(unknown)}") + changed: dict[str, tuple[float, float]] = {} + for name, value in wanted.items(): + # The profiler ramp is one knob over two registers: ACC = +v, + # DEC = -v (the firmware wants the deceleration negative). + targets = ( + [(_DM_REG_ACC, float(value)), (_DM_REG_DEC, -abs(float(value)))] + if name == "profile_acc" + else [(regs[name], float(value))] + ) + before = float(await self._read_register(targets[0][0])) + matches = abs(before - targets[0][1]) <= 1e-6 * max(1.0, abs(targets[0][1])) + for rid, v in targets[1:]: + have = float(await self._read_register(rid)) + matches = matches and abs(have - v) <= 1e-6 * max(1.0, abs(v)) + if matches: + continue + for rid, v in targets: + await self._write_register(rid, v) + await asyncio.sleep(0.02) + after = float(await self._read_register(rid)) + if abs(after - v) > 1e-6 * max(1.0, abs(v)): + raise MotorError( + f"Damiao motor {self._motor_id:#04x}: wrote {name}={v:g} " + f"(register {rid}) but reads back {after:g}" + ) + changed[name] = (before, targets[0][1]) + if changed: + await self._store_parameters() + await asyncio.sleep(0.3) + return changed + + async def firmware_gain_mismatches( + self, wanted: Mapping[str, float] + ) -> dict[str, tuple[float, float]]: + """``{name: (running, wanted)}`` for each of ``wanted`` that differs. + + Register reads only, so it works on an enabled, holding wrist. + """ + regs = { + "speed_kp": _DM_REG_SPEED_KP, + "speed_ki": _DM_REG_SPEED_KI, + "position_kp": _DM_REG_POS_KP, + "position_ki": _DM_REG_POS_KI, + "profile_acc": _DM_REG_ACC, + } + out: dict[str, tuple[float, float]] = {} + for name, value in wanted.items(): + have = float(await self._read_register(regs[name])) + if abs(have - float(value)) > 1e-6 * max(1.0, abs(float(value))): + out[name] = (have, float(value)) + return out + async def set_gains(self, gains: MotorGains) -> None: await self._write_register(_DM_REG_SPEED_KP, gains.speed_kp) await self._write_register(_DM_REG_SPEED_KI, gains.speed_ki) diff --git a/almond_axol/motor/myactuator.py b/almond_axol/motor/myactuator.py index 4557c639..45f01430 100644 --- a/almond_axol/motor/myactuator.py +++ b/almond_axol/motor/myactuator.py @@ -13,6 +13,7 @@ import math import re import struct +from collections.abc import Mapping from typing import Callable import can @@ -51,6 +52,9 @@ # driver tells the two formats apart at runtime. _MA_READ_GAINS = 0x30 _MA_WRITE_GAINS_ROM = 0x32 # persistent by command; 0x31 (RAM) is not used +# Settle after a 0x32 write before the read-back: the flash commit is not +# instant, and a read that races it returns the old value. +_MA_ROM_SETTLE_S = 0.3 # Indexed float32 parameter indices for 0x30/0x31/0x32 (V4.2+). _MA_PID_IDX = { @@ -62,7 +66,15 @@ "position_ki": 0x08, "position_kd": 0x09, } + + +def _gain_matches(stored: float, wanted: float) -> bool: + """``stored`` is ``wanted`` up to float32 rounding of the wire value.""" + return abs(stored - wanted) <= 1e-6 * max(1.0, abs(wanted)) + + _MA_SET_ACCELERATION = 0x43 # write acceleration to RAM and ROM; persistent by command +_MA_READ_ACCELERATION = 0x42 # read one acceleration type; int32 dps/s in bytes 4-7 # Configuration-parameter access. These two commands are absent from MyActuator's # published protocol (V4.4) — they were recovered from the vendor setup software, @@ -457,18 +469,106 @@ async def is_holding(self) -> bool: async def get_firmware_version(self) -> int | None: return await self._read_firmware_version() + async def get_planner_acceleration(self) -> tuple[int, int]: + """The position planner's stored ``(acceleration, deceleration)`` in + dps/s (0x42 types 0x00 / 0x01). + + 0 puts the position loop (0xA4) in direct PI tracking of each new + target, which a streamed trajectory needs; any other value makes the + firmware plan a velocity profile to every target and a 200 Hz stream + then never gets going. Note that a joint left at 0 executes a stored + target at its speed cap the moment it wakes. + """ + out: list[int] = [] + for kind in (_MA_ACC_POS_PLAN, _MA_DEC_POS_PLAN): + resp = await self._request( + bytes([_MA_READ_ACCELERATION, kind, 0, 0, 0, 0, 0, 0]) + ) + out.append(int(struct.unpack_from(" dict[str, tuple[float, float]]: + """``{name: (running, wanted)}`` for each of ``wanted`` that differs. + + Reads only (0x30 gains, 0x42 planner), so it works on a motor that is + enabled and holding — the values its running loop uses. A gain the + firmware cannot report (pre-V4.2 indexed format) is left out. + """ + wanted = dict(wanted) + out: dict[str, tuple[float, float]] = {} + planner = wanted.pop("planner_accel", None) + if planner is not None: + # Acceleration only: it selects the loop, and the deceleration + # has a firmware floor (see _ensure_planner_acceleration). + acc, _dec = await self.get_planner_acceleration() + if acc != int(round(planner)): + out["planner_accel"] = (float(acc), float(planner)) + for name, value in wanted.items(): + have = await self._read_gain_indexed(_MA_PID_IDX[name]) + if have is not None and not _gain_matches(have, value): + out[name] = (have, float(value)) + return out + + async def _ensure_planner_acceleration( + self, value: int + ) -> tuple[float, float] | None: + """Set the position planner's acceleration to ``value`` if it differs. + + The acceleration is what selects the loop (0 = direct tracking, + 60000 = the planner). The deceleration is written the same, but not + required to match: the X8-P20's 2026042402 firmware keeps its own + floor of 10 dps/s and reads back ``0/10`` after a 0 — requiring 0 + there made every enable fail to apply shoulder_1 / shoulder_2's + firmware gains (2026-09-22). + + Returns ``(before, after)`` acceleration when written, else None. The + caller resets the motor afterwards: on the X6-P20's 2025070202 + firmware a 0 written into a running position loop is ignored until + the reset (non-zero values apply live on every firmware seen). + """ + acc, _dec = await self.get_planner_acceleration() + if acc == value: + return None + for kind in (_MA_ACC_POS_PLAN, _MA_DEC_POS_PLAN): + await self._request( + bytes([_MA_SET_ACCELERATION, kind, 0, 0]) + + struct.pack(" str | None: return await self._read_model() async def disable(self) -> None: await self._request(self._cmd(_MA_SHUTDOWN)) + async def reset(self) -> None: + """0x76 system reset and the settle the motor needs before it answers. + + Reboots the motor: torque drops, RAM state (0x31 gains) is lost, and + the ROM parameters — loop gains written with 0x32, the planner + acceleration — are (re)loaded. On the X6-P20's 2025070202 firmware a + 0x32 write does not reach the running loop without this: the right + elbow provisioned at enable and streamed straight after held its + pose through a whole replay (2026-09-21), and tracked once rebooted. + """ + await self._bus._send(_MA_REQ + self._motor_id, self._cmd(_MA_RESET)) + await asyncio.sleep(_MA_RESET_SETTLE_S) + async def set_control_mode(self, mode: ControlMode) -> None: # MyActuator has no persistent control mode register; the active mode is # determined by which command is sent. Reset the motor to clear internal # state so it comes back ready for the next command type. - await self._bus._send(_MA_REQ + self._motor_id, self._cmd(_MA_RESET)) - await asyncio.sleep(_MA_RESET_SETTLE_S) + await self.reset() async def clear_errors(self) -> None: pass # MyActuator has no clear-errors command @@ -615,6 +715,63 @@ async def get_gains(self) -> MotorGains: values[name] = value return MotorGains(**values) + async def ensure_rom_gains( + self, wanted: Mapping[str, float] + ) -> dict[str, tuple[float, float]]: + """Bring the named firmware loop gains to ``wanted`` in ROM (0x32). + + ``wanted`` maps parameter names (keys of ``_MA_PID_IDX``) to values, + plus ``planner_accel``: the 0xA4 position planner's acceleration and + deceleration (dps/s, 0x43 — RAM and ROM in one command), written raw + so 0 (direct tracking) is reachable; :meth:`set_acceleration` clamps + to its 100 dps/s floor. + Each gain is read first and written only when it differs beyond + float32 rounding, so a provisioned motor costs reads only and the + flash is written once per change. Every write is read back. + + Returns ``{name: (before, after)}`` for the gains that were written. + + The motor must be **disabled**: the firmware commits a 0x32 write to + ROM only in that state (protocol V4.4 §2.3) and silently keeps the + old value otherwise, which the read-back turns into a + :class:`MotorError`. Pre-V4.2 firmware (bulk uint8 gains) is refused + with a :class:`MotorError` rather than written. + """ + wanted = dict(wanted) + planner = wanted.pop("planner_accel", None) + unknown = set(wanted) - set(_MA_PID_IDX) + if unknown: + raise ValueError(f"unknown firmware gain(s) {sorted(unknown)}") + changed: dict[str, tuple[float, float]] = {} + if planner is not None: + moved = await self._ensure_planner_acceleration(int(round(planner))) + if moved is not None: + changed["planner_accel"] = moved + for name, value in wanted.items(): + index = _MA_PID_IDX[name] + before = await self._read_gain_indexed(index) + if before is None: + raise MotorError( + f"MyActuator motor {self._motor_id:#04x}: firmware predates the " + f"indexed gain format (protocol V4.2); cannot set {name}" + ) + if _gain_matches(before, value): + continue + await self._request( + bytes([_MA_WRITE_GAINS_ROM, index, 0, 0]) + + struct.pack(" None: # Command 0x32 writes directly to ROM — no separate store step needed. # Probe the read format first so a V4.2+ motor never receives the diff --git a/almond_axol/robot/__init__.py b/almond_axol/robot/__init__.py index 1b168c34..70844186 100644 --- a/almond_axol/robot/__init__.py +++ b/almond_axol/robot/__init__.py @@ -17,6 +17,7 @@ from .config import ( ArmConfig, AxolConfig, + FirmwareGains, FrictionParams, JointConfig, PositionForceConfig, @@ -39,6 +40,7 @@ "Jelly", "JellyConfig", "detect_jelly", + "FirmwareGains", "BatteryStatus", "battery_percent", "estimate_battery", diff --git a/almond_axol/robot/axol.py b/almond_axol/robot/axol.py index 0b25a8f5..9e76bba7 100644 --- a/almond_axol/robot/axol.py +++ b/almond_axol/robot/axol.py @@ -34,6 +34,8 @@ MotorGains, MotorStatus, ) +from ..motor.damiao import DamiaoMotor +from ..motor.myactuator import MyActuatorMotor from ..settings import SHARED from ..utils.paths import almond_path from ..utils.state_files import secure_atomic_write_json, secure_read_text @@ -45,7 +47,12 @@ VEL_CUTOFF_FREQ, BandPass, Differentiator, + TorqueDither, compute_friction, + stiction_amplitude, + stiction_compensation, + stribeck_amplitude, + stribeck_excess, ) from .gravity import GravityCompensator @@ -151,6 +158,148 @@ async def _arm_is_unpowered(arm: "AxolArm", bus: CanBus) -> bool: ) +def _wanted_firmware(jc: object) -> dict[str, float]: + """The firmware parameters a bring-up should put on this joint's motor. + + ``planner_accel`` only when the joint actually runs on the 0xA4 loop + (``wire_mode`` ``a4``): the 0xA4 stream needs it at 0 or 60000, but on an + impedance joint the planner shapes nothing the core sends — and every + single-target position move the tuners make (``tune.friction`` / + ``tune.breakaway`` homing, one 0xA4 target at 14 deg/s) relies on it. The + jelly robot's right arm, pinned to 0 by every impedance replay, went wild + in ``tune.breakaway``'s homing (2026-09-24): its X8 shoulders (firmware + 2026042403) refuse a deceleration of 0 and sat at 0 / 10 dps/s. + """ + firmware = getattr(jc, "firmware", None) + wanted = firmware.as_dict() if firmware is not None else {} + if str(getattr(jc, "wire_mode", "mit")).lower() != "a4": + wanted.pop("planner_accel", None) + return wanted + + +async def apply_firmware_gains(arm: "AxolArm", joints: Iterable[Joint]) -> None: + """Write the configured firmware loop gains of ``joints`` to their motors' ROM. + + For each joint whose :class:`~almond_axol.robot.config.JointConfig` + carries set :class:`~almond_axol.robot.config.FirmwareGains`, the + MyActuator driver compares them with the motor's stored gains and writes + the ones that differ (see ``MyActuatorMotor.ensure_rom_gains``). Meant for + the *cold* joints of a bring-up, called while they are still disabled and + the bus is quiet: a MyActuator commits a ROM write only when disabled, + and joints found holding from a previous session are never touched. + + A motor that took a write is **reset** afterwards (0x76, ~2 s): on the + X6-P20's 2025070202 firmware a ROM gain write does not reach the running + loop until the motor reboots — the right elbow provisioned here and then + streamed to on the firmware loop held its pose for a whole replay + (2026-09-21). The reset costs nothing on a provisioned motor (no write, + no reset) and only happens while the joint is already disabled. Callers + that derive anything from the motor's post-reset state (multi-turn + offsets) must do so after this returns. + + A joint that will not take the write (pre-V4.2 firmware, no answer, or a + read-back mismatch) keeps its stored gains and is logged as a warning — + it tracks with whatever the motor holds, which is safe, just not the + tuned set — so one joint's firmware cannot fail the whole enable. + """ + arm_config = getattr(arm, "_arm_config", None) + if arm_config is None: + # A bench or test arm built without a config carries no firmware + # gains to apply; there is nothing to compare the motors against. + return + side = "left" if getattr(arm, "_is_left", True) else "right" + for joint in joints: + jc = getattr(arm_config, joint.value, None) + wanted = _wanted_firmware(jc) + if not wanted: + continue + driver = getattr(arm.motors.get(joint), "_driver", None) + if not isinstance(driver, (MyActuatorMotor, DamiaoMotor)): + _logger.warning( + "%s.%s: firmware loop gains configured but the joint has no " + "firmware position loop; ignored", + side, + joint.value, + ) + continue + try: + changed = await driver.ensure_rom_gains(wanted) + except MotorError as exc: + _logger.warning( + "%s.%s: could not apply firmware loop gains %s (%s); the motor " + "keeps its stored gains", + side, + joint.value, + wanted, + exc, + ) + continue + if changed and isinstance(driver, MyActuatorMotor): + _logger.info( + "%s.%s: firmware loop gains written to ROM: %s — resetting the " + "motor so the loop loads them", + side, + joint.value, + ", ".join(f"{n} {b:g} -> {a:g}" for n, (b, a) in changed.items()), + ) + await driver.reset() + elif changed: + # Damiao registers take effect on write; the store persisted them. + _logger.info( + "%s.%s: firmware loop gains written and stored: %s", + side, + joint.value, + ", ".join(f"{n} {b:g} -> {a:g}" for n, (b, a) in changed.items()), + ) + + +async def held_firmware_gain_mismatches( + arm: "AxolArm", joints: Iterable[Joint] +) -> list[str]: + """Why the *held* ``joints`` would not run the firmware gains configured. + + :func:`apply_firmware_gains` writes only cold joints: a MyActuator takes + a ROM write only while disabled, and a joint found holding from a + previous session is attached to, never reset. Such a joint keeps running + whatever it holds — a cut run's test gains included — so a run that + changed a gain would silently not test it (right shoulder_1 kept its + previous speed_kp through a speed_kp sweep, 2026-09-22). This reads each + held joint's running gains (reads work while holding) and returns one + line per joint that differs; empty means every held joint matches. A + joint whose gains cannot be read is skipped with a warning. + """ + arm_config = getattr(arm, "_arm_config", None) + if arm_config is None: + return [] + side = "left" if getattr(arm, "_is_left", True) else "right" + out: list[str] = [] + for joint in joints: + wanted = _wanted_firmware(getattr(arm_config, joint.value, None)) + driver = getattr(arm.motors.get(joint), "_driver", None) + if not wanted or not isinstance(driver, (MyActuatorMotor, DamiaoMotor)): + continue + try: + differs = await driver.firmware_gain_mismatches(wanted) + except MotorError as exc: + _logger.warning( + "%s.%s: held joint's firmware gains could not be read (%s); " + "cannot confirm it runs the configured set", + side, + joint.value, + exc, + ) + continue + if differs: + out.append( + f"{side}.{joint.value}: " + + ", ".join( + f"{n} {have:g} (wanted {want:g})" + for n, (have, want) in differs.items() + ) + ) + return out + + async def _rollback_newly_enabled_motors( motors: list[tuple[str, Motor]], setup_error: BaseException ) -> list[tuple[str, Motor, BaseException]]: @@ -648,6 +797,7 @@ def __init__( for j in Joint ] self._damp_bp = BandPass(n=n_j, w0=self._damp_w0, q=self._damp_q) + self._dither = TorqueDither(len(ARM_JOINTS)) self._last_q_commanded: np.ndarray | None = None self._gc_hold_q: np.ndarray | None = None self._gc_hold_free: frozenset[Joint] | None = None @@ -1139,6 +1289,9 @@ async def _enable_from_holding_state( self, held: list[Joint], cold: list[Joint], *, hold: bool ) -> None: """Attach held motors and bring cold motors up after state is sampled.""" + # Firmware loop gains go to ROM first, while the cold motors are still + # disabled — the only state a MyActuator commits a 0x32 write in. + await apply_firmware_gains(self, cold) await _await_all_hardware_actions( *[ self.motors[j].attach( @@ -1662,6 +1815,10 @@ async def motion_control(self, q: np.ndarray) -> None: return arm_cmds: list[tuple[float, float, float, float, float]] = [] + dither = self._dither.update( + [getattr(self._arm_config, j.value).dither_nm for j in ARM_JOINTS], + [getattr(self._arm_config, j.value).dither_hz for j in ARM_JOINTS], + ) for i, j in enumerate(ARM_JOINTS): gains = getattr(self._arm_config, j.value) f = gains.friction @@ -1672,9 +1829,54 @@ async def motion_control(self, q: np.ndarray) -> None: # phase-safe on the slow shoulder modes, so silently converting # excess firmware damping into it could excite the very # oscillation the oversized kd was meant to kill. + # Stiction compensation acts on the measured error (motor frame + # on both sides); zero until the first feedback frame is cached. + stiction = 0.0 + if gains.stiction_gain != 0.0 or gains.stiction_load_gain != 0.0: + motor = self.motors.get(j) + try: + q_meas = motor.position if motor is not None else None + v_meas = motor.velocity if motor is not None else 0.0 + except MotorError: + q_meas = None + v_meas = 0.0 + if q_meas is not None: + stiction = stiction_compensation( + float(motor_targets[i]) - q_meas, + v_meas, + stiction_amplitude( + f.fc, + gains.stiction_gain, + gains.stiction_load_gain, + float(gravity[i]), + ), + math.radians(gains.stiction_err_deg), + ) + stribeck = 0.0 + if gains.stribeck_gain != 0.0: + motor = self.motors.get(j) + try: + v_now = motor.velocity if motor is not None else 0.0 + except MotorError: + v_now = 0.0 + stribeck = stribeck_excess( + v_now, + stribeck_amplitude( + gains.stribeck_gain, + gains.stribeck_dfs, + gains.stribeck_load_gain, + float(gravity[i]), + ), + gains.stribeck_vs, + ) t_ff = ( float(gravity[i]) - + compute_friction(velocities[i], f.fc, f.k, f.fv, f.fo) + + compute_friction( + velocities[i], f.fc + f.fl * abs(float(gravity[i])), f.k, f.fv, f.fo + ) + + stiction + + stribeck + + dither[i] + gains.j_eff * float(j_scale[i]) * accelerations[i] + float(host_scale[i]) * gains.kd_host * v_damp[i] ) @@ -1852,6 +2054,7 @@ def reset_command_state(self) -> None: self._vel_fast_diff = Differentiator(n=n, cutoff=VEL_CUTOFF_FREQ) self._meas_vel_diff = Differentiator(n=n, cutoff=VEL_CUTOFF_FREQ) self._damp_bp = BandPass(n=n, w0=self._damp_w0, q=self._damp_q) + self._dither = TorqueDither(len(ARM_JOINTS)) def torque_residuals(self) -> np.ndarray: """Measured minus model-gravity torque per arm joint, shape (7,). diff --git a/almond_axol/robot/calibration.py b/almond_axol/robot/calibration.py index 30a5b1cd..d9e58fac 100644 --- a/almond_axol/robot/calibration.py +++ b/almond_axol/robot/calibration.py @@ -34,6 +34,13 @@ is what fixes the static droop a few-percent mass/CoM error causes under load (parked error = unmodeled torque / kp). +``cogging`` is a joint's position-periodic torque (cogging / gear mesh) as a +Fourier series in the joint angle, written by ``scripts/cogging_map.py +--save``: ``{"period_deg": 3.62, "harmonics": [[1, a, b], [2, a, b], ...]}`` +— harmonic ``k`` contributes ``a·cos(2πkθ/P) + b·sin(2πkθ/P)`` Nm, the +torque to *add* so the motor cancels it. The realtime core feeds it forward +on tracked ticks (``JointConfig.cogging``, scaled by ``cogging_gain``). + ``kp`` / ``kd`` are the tuned gains — the top of the stiffness blend (``s=1.0``, the production default) — exactly like the :class:`JointConfig` defaults they replace. @@ -46,6 +53,7 @@ import json import logging +import math import os from datetime import datetime, timezone from pathlib import Path @@ -68,6 +76,39 @@ # ``kd_soft`` entries written by older versions are silently dropped on load. _SCALAR_FIELDS = ("kp", "kd", "j_eff", "kd_host", "kd_host_hz", "kd_host_q") _FRICTION_FIELDS = ("fc", "k", "fv", "fo") +# A cogging series longer than this is a fit gone wrong, not a motor. +_COGGING_MAX_HARMONICS = 16 + + +def clean_cogging(entry: Any) -> dict[str, Any] | None: + """Validate a ``cogging`` entry: ``None`` if it is not a usable series. + + Returns ``{"period_deg": P, "harmonics": [[k, a, b], ...]}`` with ``P > + 0``, integer ``k >= 1`` and finite coefficients. + """ + if not isinstance(entry, dict): + return None + period = _coerce_float(entry.get("period_deg")) + harmonics = entry.get("harmonics") + if ( + period is None + or not math.isfinite(period) + or period <= 0.0 + or not isinstance(harmonics, (list, tuple)) + ): + return None + out: list[list[float]] = [] + for h in harmonics[:_COGGING_MAX_HARMONICS]: + if not isinstance(h, (list, tuple)) or len(h) != 3: + return None + k, a, b = (_coerce_float(v) for v in h) + if k is None or a is None or b is None: + return None + if not all(math.isfinite(v) for v in (k, a, b)) or k < 1 or k != int(k): + return None + out.append([int(k), a, b]) + return {"period_deg": period, "harmonics": out} if out else None + # A corrupt file must never take the robot down, but silently ignoring it # would make a bad calibration mysterious — warn once per process. @@ -196,6 +237,17 @@ def load_calibration( side, joint, ) + if "cogging" in entry: + cogging = clean_cogging(entry["cogging"]) + if cogging is not None: + clean["cogging"] = cogging + else: + _logger.warning( + "Calibration for %s %s has a malformed cogging entry; " + "ignoring it.", + side, + joint, + ) friction = entry.get("friction") if isinstance(friction, dict): fclean = {f: _coerce_float(friction.get(f)) for f in _FRICTION_FIELDS} @@ -264,6 +316,7 @@ def update_joint_calibration( kd_host_q: float | None = None, friction: dict[str, float] | None = None, com: tuple[float, float, float] | None = None, + cogging: dict[str, Any] | None = None, hub_serial: str | None = None, path: Path = CALIBRATION_PATH, ) -> Path: @@ -272,7 +325,8 @@ def update_joint_calibration( Only the provided fields are touched — saving PID gains or its host damping band does not clobber a previously saved friction fit, and vice versa. ``friction`` must carry all of ``fc`` / ``k`` / ``fv`` / ``fo``; - ``com`` is the link's fitted centre of mass (metres, URDF link frame). + ``com`` is the link's fitted centre of mass (metres, URDF link frame); + ``cogging`` a position-periodic torque series (see the module docstring). The document is scoped to ``hub_serial`` (auto-detected when omitted) and stale data for another robot is never merged into it. If an existing file is unscoped or belongs to another robot, it is preserved in a numbered @@ -285,6 +339,11 @@ def update_joint_calibration( missing = [f for f in _FRICTION_FIELDS if f not in friction] if missing: raise ValueError(f"friction is missing fields: {', '.join(missing)}") + cogging_clean = None + if cogging is not None: + cogging_clean = clean_cogging(cogging) + if cogging_clean is None: + raise ValueError(f"not a usable cogging series: {cogging!r}") if hub_serial is None: hub_serial = current_hub_serial() @@ -360,6 +419,8 @@ def update_joint_calibration( if len(com) != 3: raise ValueError(f"com must have 3 components, got {len(com)}") entry["com"] = [float(v) for v in com] + if cogging_clean is not None: + entry["cogging"] = cogging_clean entry["updated_at"] = ( datetime.now(timezone.utc).isoformat(timespec="seconds").replace("+00:00", "Z") ) diff --git a/almond_axol/robot/config.py b/almond_axol/robot/config.py index 5448b9db..1db7d794 100644 --- a/almond_axol/robot/config.py +++ b/almond_axol/robot/config.py @@ -29,10 +29,13 @@ import logging import math from collections.abc import Sequence -from dataclasses import dataclass, field, replace +from dataclasses import dataclass, field, fields, replace from typing import Any -from ..constants import ARM_JOINTS +import numpy as np + +from ..constants import ARM_JOINTS, Joint +from ..motor.motor import _JOINT_CONFIG from .calibration import ( CALIBRATION_PATH, FACTORY_CALIBRATION_PATH, @@ -49,22 +52,250 @@ class FrictionParams: """tanh-Coulomb + viscous friction model. - ``τ_friction = fc · tanh(k · v) + fv · v + fo`` + ``τ_friction = (fc + fl · |τ_gravity|) · tanh(k · v) + fv · v + fo`` - where ``v`` is the joint velocity (rad/s). + where ``v`` is the joint velocity (rad/s) and ``τ_gravity`` the gravity + feedforward the joint is carrying. Attributes: - fc: Coulomb friction magnitude (Nm). + fc: Coulomb friction magnitude (Nm) at zero gravity load. k: Tanh sharpness factor — larger is closer to a sign() function. fv: Viscous friction coefficient (Nm·s/rad). fo: Constant friction offset (Nm). Captures direction-independent biases such as imperfect gravity compensation or motor cogging. + fl: Load-proportional Coulomb friction, Nm per Nm of gravity + feedforward. Planetary gear friction grows with the torque the + meshes carry: right shoulder_1's sliding friction measured + ~0.6 Nm at rest and ~1.5 Nm under 12 Nm of load (slope ≈ 0.08), + and its breakaway 0.66 Nm at rest against 2.4–3.3 Nm loaded. A + constant ``fc`` fitted at moderate load therefore over-compensates + at rest (kicking the joint at every reversal) and under-compensates + at reach. ``0`` (default) keeps the constant model; ``tune.friction`` + fits it when its sweep spans enough load. """ fc: float k: float fv: float fo: float + fl: float = 0.0 + + +@dataclass +class FirmwareGains: + """MyActuator firmware loop gains, written to the motor's ROM at enable. + + These are the gains of the motor's own cascaded controller — position + PI → speed PI → current PI — which is the loop a joint runs on when its + ``wire_mode`` is ``a4``. Under the MIT impedance frame (``wire_mode`` + ``mit``, the production law) the firmware ignores them; ``kp`` / ``kd`` + on :class:`JointConfig` are the impedance gains and travel with every + command. Each field is ``None`` by default, meaning "leave whatever the + motor holds". A set value is compared against the motor's stored gain + while the joint is still disabled at enable (the only state a MyActuator + accepts a ROM write in) and written only if it differs, so the flash is + touched once per change, not once per bring-up. Identified with + ``axol tune.a4``; protocol V4.2+ firmware only. + + Attributes: + position_kp: Position loop proportional gain: position error → + speed setpoint. Sets the loop bandwidth. Stock 0.008 on the + X8-P20 shoulders (~0.3 Hz), which stick-slips at creep + speed; 0.2 removes the stairs and 0.3 is the knee before + the loop's ~5 Hz mode and a speed-loop buzz appear. + position_ki: Position loop integral gain. Leave at the stock 0: on + top of the speed integrator it hunts around the target. + position_kd: Position loop derivative gain (protocol V4.2+ index + 0x09). Stored and read back by the firmware but measured + inert in the 0xA4 loop on the X8-P20 — 0.1, 0.3 and 0.6 + produced identical traces — so it is carried for + completeness, not as a damping knob. + speed_kp: Speed loop proportional gain: velocity error → current. + The only damping term the 0xA4 loop has; also the buzz + knob (0.15 doubled the >20 Hz current on shoulder_1). + speed_ki: Speed loop integral gain. Lower is smoother on a geared + joint: the integrator winds up while the joint is stuck and + dumps it at release, so the stock 1e-4 feeds the surge + (2e-4 limit-cycled at 5 Hz); 1e-5 halves the mode's current. + profile_acc: **Damiao only.** The position-velocity mode's profiler + ramp, rad/s² — written to both ACC and (negated) DEC. Every + streamed target is reached along a trapezoid under this + acceleration, so it caps how fast the wrist can follow: the + wrists ship at 2 rad/s² (115 deg/s²), which cannot keep up + with a 200 Hz stream — the loop hunts at ~5 Hz, 3x the + impedance frame's 3-15 Hz error — and would take half a + second to reach teleop speed. 50 is above the core + tracker's 33 rad/s² limit (the profile never binds) while + still rounding each 5 ms step; 50 and 200 scored alike in + ``tune.a4``. A MyActuator joint uses ``planner_accel``. + planner_accel: **MyActuator only.** The 0xA4 position planner's stored + acceleration and deceleration, dps/s (0x43), written at + enable like the gains. Only two values follow a stream: + ``0`` is direct PI tracking of each target, ``60000`` (the + protocol maximum) makes the planner finish each step within + the tick — anything between re-plans every target and the + joint barely moves. On the X6-P20 elbow ``tune.a4`` tracked a + 3 deg/s triangle to 0.02° RMS with 4 ms lag at 60000 against + 0.23° / 74 ms direct. 60000 wants ``cap_track`` too. + cap_track: **Host side, 0xA4 joints with the planner on.** Each tick's + 0xA4 speed cap as this multiple of the commanded speed + (floor 1 dps) instead of the fixed tracker limit: at a fixed + cap the planner bursts through each step and idles the rest + of the tick (4x the current spread on the elbow); 1.1-1.2 + moved it continuously. Leave unset under direct tracking — + there the cap is a hard limit on the PI output and a tight + one never lets the loop catch up. Not a motor parameter: + carried to the realtime core, never written to the motor. + planner_lead_ms: **Host side, 0xA4 joints on the planner.** Each + tick's 0xA4 target is sent this far ahead along the core + tracker's velocity. The planner reaches a target that is + exactly on the trajectory before its step ends and stops for + the rest of it — a speed ripple at the step rate (half the + right elbow's speed error at 60-120 Hz on the 240 Hz lane); + a few ms ahead keeps it cruising. 0-50; not written to the + motor. + tf_rated_current_a: **Host side, MyActuator 0xA4 joints.** The motor's + rated current (A, from its datasheet, or estimated with + ``tune.a4 --tf-probe``). Set, the realtime core sends the + joint's position command as **0x73** (protocol V4.4: + position control with torque feedforward) carrying the host + feedforward — gravity, inertia and the cogging cancellation + — in the int8 1%-of-rated-current unit the firmware takes, + scaled with the joint's torque constant; faded in over a + second so the speed integrator can hand the load over. Only + on firmware that implements 0x73 (VersionDate 2026042402 or + later: the X8-P20 shoulders, not the X6-P20 elbow's + 2025070202) — elsewhere the joint stays on plain 0xA4 and + the core logs why. Unset (default): plain 0xA4. Direct + tracking only (``planner_accel`` 0): with the planner on the + firmware ignores the feedforward. Not written to the motor. + """ + + position_kp: float | None = None + position_ki: float | None = None + position_kd: float | None = None + speed_kp: float | None = None + speed_ki: float | None = None + profile_acc: float | None = None + planner_accel: float | None = None + cap_track: float | None = None + planner_lead_ms: float | None = None + tf_rated_current_a: float | None = None + + def __post_init__(self) -> None: + check_firmware_extras(self.planner_accel, self.cap_track, self.planner_lead_ms) + if self.tf_rated_current_a is not None and not ( + math.isfinite(self.tf_rated_current_a) and self.tf_rated_current_a > 0.0 + ): + raise ValueError( + f"tf_rated_current_a {self.tf_rated_current_a:g}: the motor's rated " + "current in amps (> 0), or unset for plain 0xA4" + ) + + def as_dict(self) -> dict[str, float]: + """The set motor parameters, keyed by name (``cap_track``, + ``planner_lead_ms`` and ``tf_rated_current_a`` excluded: they are the + realtime core's, not the motor's).""" + return { + f.name: float(v) + for f in fields(self) + if f.name not in _HOST_FIRMWARE_FIELDS + and (v := getattr(self, f.name)) is not None + } + + +#: ``FirmwareGains`` fields the realtime core uses; never written to a motor. +_HOST_FIRMWARE_FIELDS = frozenset( + {"cap_track", "planner_lead_ms", "tf_rated_current_a"} +) + + +def check_firmware_extras( + planner_accel: float | None, + cap_track: float | None, + planner_lead_ms: float | None = None, +) -> None: + """Refuse a planner acceleration or cap tracking that cannot follow a stream. + + Raises: + ValueError: ``planner_accel`` not 0 / 60000, ``cap_track`` below 1 + (a cap under the commanded speed can never keep up), or + ``planner_lead_ms`` outside 0..50. + """ + if planner_accel is not None and float(planner_accel) not in (0.0, 60000.0): + raise ValueError( + f"planner_accel {planner_accel:g}: only 0 (direct tracking) or 60000 " + "(the planner finishing each step within the tick) follow a stream — " + "values in between re-plan every target and the joint barely moves" + ) + if cap_track is not None and not (cap_track == 0.0 or cap_track >= 1.0): + raise ValueError( + f"cap_track {cap_track:g}: a cap under the commanded speed never keeps " + "up — use >= 1 (1.1-1.2 tested), or 0 / unset for the fixed cap" + ) + if planner_lead_ms is not None and not 0.0 <= planner_lead_ms <= 50.0: + raise ValueError(f"planner_lead_ms {planner_lead_ms:g}: must be within 0..50") + + +@dataclass(frozen=True) +class CoggingModel: + """A joint's position-periodic torque (cogging / gear mesh) to cancel. + + A Fourier series in the **joint** angle: harmonic ``(k, a, b)`` adds + ``a·cos(2πkθ/P) + b·sin(2πkθ/P)`` Nm, ``P`` = ``period_deg`` — the torque + to *add* so the motor cancels the ripple. Fitted from a slow friction + sweep (``axol tune.friction --raw-csv`` then ``scripts/cogging_map.py + --save``) and stored in the calibration file; the right shoulder_1's is a + 3.62° series whose 1.81° and 0.905° harmonics carry most of it — the + bumps that land at 1–6 Hz in slow motion (2026-09-23). + """ + + period_deg: float + harmonics: tuple[tuple[int, float, float], ...] + + @classmethod + def from_dict(cls, entry: dict[str, Any]) -> "CoggingModel": + """From a calibration-file ``cogging`` entry.""" + return cls( + period_deg=float(entry["period_deg"]), + harmonics=tuple( + (int(k), float(a), float(b)) for k, a, b in entry["harmonics"] + ), + ) + + def as_dict(self) -> dict[str, Any]: + """The calibration-file form (inverse of :meth:`from_dict`).""" + return { + "period_deg": self.period_deg, + "harmonics": [list(h) for h in self.harmonics], + } + + def torque(self, q_joint: float | np.ndarray) -> float | np.ndarray: + """The series at joint angle ``q_joint`` (rad), Nm.""" + period = math.radians(self.period_deg) + return sum( + a * np.cos(2.0 * math.pi * k * q_joint / period) + + b * np.sin(2.0 * math.pi * k * q_joint / period) + for k, a, b in self.harmonics + ) + + def motor_terms( + self, offset: float, gain: float = 1.0 + ) -> list[tuple[float, float, float]]: + """The series in the **motor** frame, for the realtime core. + + ``joint = motor + offset``, so each harmonic's phase shifts by its + spatial frequency times the offset; torque needs no sign change (the + motor frame is the joint frame shifted). Returns ``(w, a', b')`` with + ``w`` in rad⁻¹, scaled by ``gain``. + """ + period = math.radians(self.period_deg) + out = [] + for k, a, b in self.harmonics: + w = 2.0 * math.pi * k / period + c, s = math.cos(w * offset), math.sin(w * offset) + out.append((w, gain * (a * c + b * s), gain * (b * c - a * s))) + return out @dataclass @@ -165,6 +396,111 @@ class JointConfig: q=3 on both arms even with its pose-tracked centre: hardware traces found a separate 12.5-13.6 Hz mast/forearm mode that the old wide band could feed. + stiction_gain: Error-sign Coulomb compensation, as a fraction of + ``friction.fc`` (see + :func:`almond_axol.robot.control.stiction_compensation`). + ``0`` (default) is the production law. Pushes up to + ``gain·fc`` toward the target while the joint is stuck and + the velocity feedforward has switched itself off, fading + out as that feedforward saturates — the lever for the + slow-motion stick-slip stairs of the high-ratio X8-P20 + shoulders (0.3-0.6° at 2 Hz on right shoulder_1, where + breakaway is ~2.3× the fitted sliding ``fc``). Keep it + below the breakaway/``fc`` ratio ``axol tune.breakaway`` + measures, or the term hunts around the target at rest. + stiction_load_gain: Load-proportional part of that push, in Nm per + Nm of the joint's gravity feedforward: the peak push is + ``stiction_gain·fc + stiction_load_gain·|gravity|``. + Gear friction follows the transmitted torque — right + shoulder_1 broke away at 0.66 Nm at rest but at 2-3.3 Nm + under 10-15 Nm of gravity load — so a constant push + either hunts at rest or does nothing extended. Size it + from ``tune.breakaway --poses`` at loaded poses, or from + the excess torque at release in a slow replay trace. + stiction_err_deg: Position error (degrees) at which that term + saturates. Smaller is a stiffer push-off; 0.1° is a few + encoder LSBs above the feedback noise floor. + dither_nm: Peak amplitude (Nm) of a sinusoidal torque dither on the + feedforward (see :func:`almond_axol.robot.control.dither_step`); + ``0`` (default) off. Keeps a geared joint's meshes sliding + so its velocity-weakening friction cannot re-stick between + cycles — the lever for the X8-P20 shoulders' 2 Hz + stick-slip once feedforward and stiction compensation + have shrunk the stairs as far as they can. Start at 1-2 Nm + on a shoulder; it is audible. + dither_hz: Dither frequency; above the arm's structural modes + (~35 Hz), below the core's 120 Hz Nyquist. + wire_mode: Which frame the realtime core commands this joint with + (the gripper, gravity comp and the limp fallback always + use MIT). ``"mit"`` (default) is the impedance frame and + the production law. ``"a4"`` hands a **MyActuator** joint + to the firmware's own position loop (0xA4 absolute + position closed-loop, speed-capped at the tracker's + velocity limit) from its first frame, holds included — + the X6-P20's 2025070202 firmware ignores 0xA4 after an + MIT frame until the motor is reset, so an a4 joint + hand-guided in gravity comp needs a re-enable before it + tracks again. Its position/speed PI on the motor-side + encoder is the candidate for creeping through the + X8-P20's stick-slip; its gains are the ``firmware`` block + below and its stored planner acceleration must be 0 or + 60000 (see ``tune.a4``). ``"pv"`` is the same thing for a + **Damiao** wrist: its position-velocity mode (0x100+ID, + control-mode register 2, which the core sets at bring-up + and toggles back to MIT for limp / gravity comp), gains + ``firmware.position_kp`` etc. in the KP_APR/KP_ASR + registers. Costs of either: no compliance (the joint + holds position with integral action and pushes back up to + motor torque), no host feedforward (gravity, friction, + stiction, dither and damping are all inert), and on a4 no + torque telemetry — the reply carries q-axis current, so + measured torque reads NaN and the contact watchdog is + blind on that joint (a pv wrist keeps its torque channel). + Position stays 0.01° on a4 via a paired 0x92 read. + ``AxolConfig.controller`` ``"position"`` sets every + joint's position wire mode at once (and runs the core at + 400 Hz); this field is the per-joint override. + stribeck_gain: Friction cancellation on *measured* velocity (see + :func:`almond_axol.robot.control.stribeck_excess`), as a + fraction of the measured static-minus-sliding excess. + ``0`` (default) off. The one feedforward that acts on the + velocity-weakening slope behind the X8-P20 shoulders' + 2 Hz stick-slip; sweep 0.5 → 0.7 → 0.9 and stop when the + arm's 3 Hz mode starts to grow (over-cancellation). + stribeck_dfs: Static-minus-sliding friction excess (Nm) at zero + gravity load; right shoulder_1 measured ~0.3. + stribeck_load_gain: Its growth per Nm of gravity feedforward + (~0.1 on right shoulder_1: excess ≈ 1.5 Nm under 12 Nm). + stribeck_vs: Speed (rad/s) at which the excess has fallen to 1/e + (~0.1 on the X8 shoulders). + stribeck_pole: Low-pass pole (rad/s) of the measured-velocity + estimate the term follows. 20 rad/s (3.2 Hz) is smooth at + 0.05 rad/s but ~40° behind the 2.6 Hz ring, which halved + the cancellation in the first A/B; 40-80 rad/s follows the + surge more closely at the cost of encoder-step noise in + the torque (a 16-bit count at 240 Hz is 0.09 rad/s). + firmware: :class:`FirmwareGains` written to the motor's ROM at + enable — the position/speed loop gains behind + ``wire_mode`` ``a4``. All ``None`` (the default) leaves the + motor's stored gains alone; MyActuator joints only. + cogging: :class:`CoggingModel` — the joint's position-periodic torque, + cancelled by feedforward on tracked ticks (the "osc + cancellation"): added to the MIT ``t_ff`` on an impedance + joint, carried by 0x73 on a firmware-loop joint with + ``firmware.tf_rated_current_a`` set (a plain-0xA4 joint + takes no feedforward, so it has no effect there). Evaluated + in the core at the measured angle. ``None`` (default): none. + Loaded from the calibration file. + cogging_gain: Fraction of ``cogging`` applied, for A/B runs (``1.0`` + default; ``0`` off; ``tune.motion --gain + shoulder_1.cogging_gain=0.5``). + impedance_hz: This joint's impedance command rate: ``480.0`` commands + it every tick of a 480 Hz core loop (its host feedforward, + damping and tracker stepped at 480), ``240.0`` keeps it on + the verified 240 Hz lane, ``None`` (default) follows + ``AxolConfig.impedance_hz`` (which at 480 covers the + MyActuator joints only). Any joint at 480 runs the core + at 480 Hz. ``tune.motion --fast-impedance right.shoulder_1``. """ kp: float @@ -176,6 +512,30 @@ class JointConfig: kd_host: float = 0.0 kd_host_hz: float | None = None kd_host_q: float | None = None + stiction_gain: float = 0.0 + stiction_load_gain: float = 0.0 + stiction_err_deg: float = 0.1 + dither_nm: float = 0.0 + dither_hz: float = 60.0 + wire_mode: str = "mit" + stribeck_gain: float = 0.0 + stribeck_dfs: float = 0.3 + stribeck_load_gain: float = 0.1 + stribeck_vs: float = 0.1 + stribeck_pole: float = 20.0 + firmware: FirmwareGains = field(default_factory=FirmwareGains) + cogging: CoggingModel | None = None + cogging_gain: float = 1.0 + impedance_hz: float | None = None + + def __post_init__(self) -> None: + # The per-type defaults (_X8_FIRMWARE_GAINS, _ZERO_FRICTION, ...) are + # module-level instances every matching joint is built from; each + # joint keeps its own copy so setting a field on one joint — a + # tune.motion override, a calibration overlay, a test — can never + # reach another joint or the next config built. + self.friction = replace(self.friction) + self.firmware = replace(self.firmware) @dataclass @@ -197,6 +557,107 @@ class PositionForceConfig: _ZERO_FRICTION = FrictionParams(fc=0.0, k=1.0, fv=0.0, fo=0.0) +# Firmware loop gains for the X8-P20 shoulders (shoulder_1 / shoulder_2), +# from the 2026-09-21 ``tune.a4`` sweeps on right shoulder_1 — the 400 Hz +# stream the realtime core is moving to, 12 deg/s triangle and 40 deg/s +# sine at -45°, checked at -10° and -70°. Stiffness: the stick-slip stairs +# are gone by position_kp 0.2 and tracking keeps improving (0.44° / 36 ms +# at 0.3 → 0.17° / 12 ms at 1.0 → 0.13° / 9 ms at 1.4); at 400 Hz the +# >10 Hz position buzz no longer moves with it. Damping: speed_kp is the +# speed loop's own ~100 Hz resonance knob, not a damper — it did nothing +# for the 5 Hz reversal mode (2.2 A at 0.13 and 0.16 alike) while the +# 100 Hz current tone went 0.07 → 0.30 → 0.41 → 1.0 A (unstable, 32 A +# abort) at 0.1 / 0.13 / 0.16 / 0.2. Lowering it to 0.07 puts the tone at +# the stock floor (0.04 A) with the tracking intact; 1.4 / 0.07 was also +# clean, so 1.0 / 0.07 carries margin. speed_ki 1e-5: 1e-6 was identical. +_X8_FIRMWARE_GAINS = FirmwareGains( + position_kp=1.0, + position_kd=0.1, + speed_kp=0.07, + speed_ki=1e-5, + # Direct tracking, pinned: a planner left at 60000 by a test run must + # not carry into the next session (see FirmwareGains.planner_accel). + planner_accel=0.0, +) + +# The X6-P20 elbow's set (stock position_kp 0.15, speed_kp 0.01 on firmware +# 2025070202), from the same 2026-09-21 400 Hz sweeps on right elbow (12 deg/s +# triangle and 40 deg/s sine at -75°, checked at -30° and -120°). Same law +# as the shoulders with the speed loop's resonance at ~135 Hz: at speed_kp +# 0.1 it grew 0.04 → 0.21 A from position_kp 0.5 → 1.0 and went unstable at +# 1.5 (34 A abort); at 0.05 it stays at 0.05-0.08 A through 1.4 and even +# 1.8. 1.4 / 0.05: 0.098° RMS, 8 ms lag, velocity ripple 0.13, current +# spread 0.41 A — 1.8 was still clean, so this carries margin. +_X6_ELBOW_FIRMWARE_GAINS = FirmwareGains( + position_kp=1.4, + position_kd=0.1, + speed_kp=0.05, + speed_ki=1e-5, + # Direct tracking, pinned: a planner left at 60000 by a test run must + # not carry into the next session (see FirmwareGains.planner_accel). + planner_accel=0.0, +) + +# shoulder_3 and wrist_1 (both RMD-X6-P20 on the same firmware, identical +# stock gains: position_kp 0.06, speed_kp 0.01, position_kd 0.5). At stock +# they are near-limp under a4: shoulder_3 held at rest during a shoulder_2 +# sweep with the arm extended wobbled 1° peak-to-peak at ~3 Hz whenever the +# arm moved, and wrist_1 tracked a 12 deg/s triangle 124 ms late (2026-09-22). +# 1.0 / 0.05 — the elbow's speed gain, one stiffness step below the elbow — +# is the knee on both at the 400 Hz stream: shoulder_3 0.04° RMS / 9 ms at +# 3 deg/s with the tone at 0.06 A (1.4 doubled it); wrist_1 0.12° / 10 ms at +# 12 deg/s, tone 0.04 A (0.025 → 0.071 A from 0.7 → 1.4), sine ripple 0.05. +# The Damiao DM-J4310 wrists (wrist_2 / wrist_3): their position-velocity +# loop's KP_APR. Stock 54 trails a 12 deg/s stream by 150 ms (2.0° RMS); +# 400 gives 0.35° / 26 ms on both, 800 buzzes wrist_2 (0.12° >10 Hz, guard +# abort). The velocity-loop gains (KP_ASR 0.0037, KI_ASR 0.002) and the +# profiler ramps changed nothing at 12 deg/s and stay stock; there is no kd. +# Registers take effect on write and are stored — no reset (2026-09-22). +_DM_WRIST_FIRMWARE_GAINS = FirmwareGains(position_kp=400.0, profile_acc=50.0) + +# Stock firmware sets for the joints that run on impedance (MIT), where the +# firmware position loop is unused: everything but shoulder_1 and the elbow, +# the two joints on 0xA4 in the mixed setup (``--a4``). Written at enable +# like any firmware set, so a cold bring-up puts these motors back on their +# factory loops (2026-09-22) — the tuned sets above stay defined for the +# position controller, which would otherwise run these joints near-limp. +# Values are the ones recorded at the sweeps (stock X8-P20: position_kp +# 0.008, speed_kp 0.03, speed_ki 1e-4, position_kd 0.1; stock X6-P20 roll: +# position_kp 0.06, speed_kp 0.01, position_kd 0.5; Damiao wrists: KP_APR +# 54, profiler 2 rad/s²) — every joint's since 2026-09-24, shoulder_1 and +# the elbow included: the arms run impedance, where these loops are inert, +# and the tuned 0xA4 sets (_X8_FIRMWARE_GAINS, _X6_ELBOW_FIRMWARE_GAINS) stay +# defined for an --a4 run to override with. The X6 speed_ki 1e-4 was read off +# the jelly robot's untouched left arm. The planner 0 here applies only to a +# joint actually on wire_mode a4 (see axol._wanted_firmware); an impedance +# joint keeps the motor's stock 5000. +_X8_STOCK_FIRMWARE_GAINS = FirmwareGains( + position_kp=0.008, + position_kd=0.1, + speed_kp=0.03, + speed_ki=1e-4, + planner_accel=0.0, +) +_X6_ROLL_STOCK_FIRMWARE_GAINS = FirmwareGains( + position_kp=0.06, + position_kd=0.5, + speed_kp=0.01, + speed_ki=1e-4, + planner_accel=0.0, +) +_DM_WRIST_STOCK_FIRMWARE_GAINS = FirmwareGains(position_kp=54.0, profile_acc=2.0) + +_X6_ROLL_FIRMWARE_GAINS = FirmwareGains( + position_kp=1.0, + position_kd=0.5, + speed_kp=0.05, + speed_ki=1e-5, + # Direct tracking, pinned: a planner left at 60000 by a test run must + # not carry into the next session (see FirmwareGains.planner_accel). + planner_accel=0.0, +) + + @dataclass class ArmConfig: """Per-joint configuration for a single arm. @@ -242,6 +703,7 @@ class ArmConfig: # Pose-tracked band-pass centre (kd_host_hz None): the shoulder # mode is the impedance mode, moving with reflected inertia. kd_host=40.0, + firmware=_X8_STOCK_FIRMWARE_GAINS, ) ) shoulder_2: JointConfig = field( @@ -253,6 +715,7 @@ class ArmConfig: com=(0.0, 0.0115864, -0.0302711), j_eff=1.1, kd_host=35.0, + firmware=_X8_STOCK_FIRMWARE_GAINS, ) ) shoulder_3: JointConfig = field( @@ -275,6 +738,7 @@ class ArmConfig: # though wrist_2 has no host damping. Narrowing shoulder_3 from # Q=0.8 to Q=3 did not remove it. Keep damping on the motor side; # do not chase the coupled wrist symptom with another host term. + firmware=_X6_ROLL_STOCK_FIRMWARE_GAINS, ) ) elbow: JointConfig = field( @@ -292,6 +756,7 @@ class ArmConfig: # active at 9.55 Hz. Hardware step/replay A/Bs found that term # increased overshoot without removing a ring; firmware kd=5 # settled the joint without the host-loop phase risk. + firmware=_X6_ROLL_STOCK_FIRMWARE_GAINS, ) ) wrist_1: JointConfig = field( @@ -301,6 +766,7 @@ class ArmConfig: friction=_ZERO_FRICTION, mass=0.25, com=(0.0, 0.0, -0.0614121), + firmware=_X6_ROLL_STOCK_FIRMWARE_GAINS, ) ) wrist_2: JointConfig = field( @@ -324,6 +790,7 @@ class ArmConfig: friction=_ZERO_FRICTION, mass=0.65, com=(0.0, 0.0285, -0.0285), + firmware=_DM_WRIST_STOCK_FIRMWARE_GAINS, ) ) wrist_3: JointConfig = field( @@ -333,6 +800,7 @@ class ArmConfig: friction=_ZERO_FRICTION, mass=0.75, com=(-0.0285, 0.0, -0.089453), + firmware=_DM_WRIST_STOCK_FIRMWARE_GAINS, ) ) gripper: PositionForceConfig = field( @@ -422,12 +890,36 @@ def _calibrated_joint(jc: JointConfig, entry: dict[str, Any]) -> JointConfig: """Overlay one joint's calibration-file entry onto its config.""" overrides: dict[str, Any] = { f: entry[f] - for f in ("kp", "kd", "j_eff", "kd_host", "kd_host_hz", "kd_host_q") + for f in ( + "kp", + "kd", + "j_eff", + "kd_host", + "kd_host_hz", + "kd_host_q", + "stiction_gain", + "stiction_load_gain", + "stiction_err_deg", + "dither_nm", + "dither_hz", + "wire_mode", + "stribeck_gain", + "stribeck_dfs", + "stribeck_load_gain", + "stribeck_vs", + "stribeck_pole", + ) if f in entry } friction = entry.get("friction") if friction is not None: overrides["friction"] = FrictionParams(**friction) + firmware = entry.get("firmware") + if firmware is not None: + overrides["firmware"] = FirmwareGains(**firmware) + cogging = entry.get("cogging") + if cogging is not None: + overrides["cogging"] = CoggingModel.from_dict(cogging) com = entry.get("com") if com is not None: # Fitted by ``axol tune.gravity --save``; already per-side (measured @@ -665,6 +1157,149 @@ def _apply_stiffness(arm: ArmConfig, s: float | Sequence[float]) -> ArmConfig: ) +#: The two control laws the realtime core can run the arms on +#: (:attr:`AxolConfig.controller`). +#: +#: ``"impedance"`` is the production MIT frame: host gravity / friction / +#: inertia feedforward and host damping around the firmware PD, compliant, +#: at 240 Hz. ``"position"`` hands every joint to its motor's own position +#: loop — 0xA4 on the MyActuator joints, position-velocity on the Damiao +#: wrists, gains from each joint's ``firmware`` block — streamed at 400 Hz, +#: where the loop's target staircase (audible at 200 Hz) is gone. It is +#: stiff: no compliance, no host feedforward, the contact watchdog blind on +#: the MyActuator joints. The bus cannot carry every motor every tick at +#: 400 Hz, so the core thins its schedule (wrists commanded on alternate +#: ticks, one a4 fine-position read per tick round-robin, gripper in that +#: rotation); the MyActuator commands themselves go out every tick. +CONTROLLERS: tuple[str, ...] = ("impedance", "position") + +#: Realtime-core tick rate under each controller (see :data:`CONTROLLERS`). +CONTROLLER_LOOP_HZ: dict[str, float] = {"impedance": 240.0, "position": 400.0} + +#: The only rate the impedance (MIT) frame is commanded at. Its gains, host +#: feedforward and damping filters were tuned and verified at 240 Hz; at +#: 400 Hz with the firmware-loop joints beside it, right shoulder_3 / wrist_1 +#: on impedance shook the arm hard enough to stop the run (2026-09-22). +IMPEDANCE_LOOP_HZ: float = CONTROLLER_LOOP_HZ["impedance"] + +#: The core loop of an arm that mixes impedance joints with firmware-loop +#: ones (``wire_mode`` ``a4`` / ``pv`` on some joints only): twice +#: :data:`IMPEDANCE_LOOP_HZ`. The core commands each impedance joint on +#: alternate ticks — exactly 240 Hz, its whole host pipeline stepped at that +#: rate — and the firmware-loop joints every tick, above the position +#: controller's 400 Hz (see ``Thinning`` in ``rust/axol-rt/src/serve.rs``). +MIXED_LOOP_HZ: float = 2.0 * IMPEDANCE_LOOP_HZ + +#: ``AxolConfig.impedance_hz`` values: the verified 240 Hz, or 480 Hz on the +#: MyActuator impedance joints — commanded every tick of a 480 Hz loop, their +#: host pipeline stepped at 480, while the Damiao wrists stay at 240 Hz on +#: alternate ticks. 480 is an experiment (the gains were tuned at 240) and is +#: never the default. +IMPEDANCE_RATES: tuple[float, ...] = (IMPEDANCE_LOOP_HZ, 2.0 * IMPEDANCE_LOOP_HZ) + +#: The fast impedance rate (see :data:`IMPEDANCE_RATES`). +FAST_IMPEDANCE_HZ: float = IMPEDANCE_RATES[1] + + +def position_wire_mode(joint: Joint) -> str: + """The firmware-position-loop wire token for an arm joint's vendor. + + ``"a4"`` for the MyActuator joints (ids 1-5), ``"pv"`` for the Damiao + wrists. + """ + return "pv" if _JOINT_CONFIG[joint].motor_id >= 6 else "a4" + + +def impedance_joints(config: "AxolConfig") -> list[str]: + """``side.joint`` for every arm joint the config runs on the MIT frame. + + Resolved first, so ``controller`` ``"position"`` counts as it runs. The + gripper is not an arm joint: it is always MIT and is not what the rate + rule protects. + """ + resolved = config.resolved() + return [ + f"{side}.{j.value}" + for side in ("left", "right") + for j in ARM_JOINTS + if str(getattr(getattr(resolved, side), j.value).wire_mode).lower() == "mit" + ] + + +def fast_impedance_joints(config: "AxolConfig") -> list[str]: + """``side.joint`` of every impedance joint that runs at + :data:`FAST_IMPEDANCE_HZ`: its own ``impedance_hz`` 480, or (none of its + own) the config-wide 480 on a MyActuator joint — the rule + ``fast_mit`` in ``rust/axol-rt/src/serve.rs`` applies.""" + resolved = config.resolved() + out = [] + for side in ("left", "right"): + for j in ARM_JOINTS: + jc = getattr(getattr(resolved, side), j.value) + if str(jc.wire_mode).lower() != "mit": + continue + own = jc.impedance_hz + if own is not None and abs(own - FAST_IMPEDANCE_HZ) < 1e-6: + out.append(f"{side}.{j.value}") + elif ( + own is None + and _JOINT_CONFIG[j].motor_id <= 5 + and abs(config.impedance_hz - FAST_IMPEDANCE_HZ) < 1e-6 + ): + out.append(f"{side}.{j.value}") + return out + + +def check_loop_hz(config: "AxolConfig", loop_hz: float) -> None: + """Refuse a core rate that would command an MIT joint off its rate. + + With any arm joint on the impedance frame the core runs at + :data:`IMPEDANCE_LOOP_HZ`, or at :data:`MIXED_LOOP_HZ` with the impedance + joints on alternate ticks — or, at ``impedance_hz`` + :data:`FAST_IMPEDANCE_HZ`, at exactly that rate (MyActuator impedance + joints every tick, wrists on alternate ticks). Without an impedance + joint, any rate goes. + + Raises: + ValueError: If ``loop_hz`` is not one of those while an arm joint is + on the impedance frame — e.g. ``tune.motion --loop-hz 400`` with + ``--a4`` putting only some joints on their firmware loops. + """ + fast = fast_impedance_joints(config) + allowed = (FAST_IMPEDANCE_HZ,) if fast else (IMPEDANCE_LOOP_HZ, MIXED_LOOP_HZ) + if any(abs(loop_hz - hz) < 1e-6 for hz in allowed): + return + mit = impedance_joints(config) + if mit: + shown = ", ".join(mit[:4]) + ( + f" and {len(mit) - 4} more" if len(mit) > 4 else "" + ) + if fast: + raise ValueError( + f"a {loop_hz:g} Hz core loop with {fast[0]} running impedance at " + f"{FAST_IMPEDANCE_HZ:g} Hz: the loop is {FAST_IMPEDANCE_HZ:g} Hz only " + "then. Drop the loop-rate override." + ) + raise ValueError( + f"a {loop_hz:g} Hz core loop with {shown} on the impedance frame: " + f"impedance runs at {IMPEDANCE_LOOP_HZ:g} Hz only — a " + f"{IMPEDANCE_LOOP_HZ:g} Hz loop, or {MIXED_LOOP_HZ:g} Hz with it on " + "alternate ticks (the default when some joints are on their " + "firmware loops). Drop the loop-rate override." + ) + + +def _on_position_loops(arm: ArmConfig) -> ArmConfig: + """Every arm joint on its vendor's firmware position loop.""" + return replace( + arm, + **{ + j.value: replace(getattr(arm, j.value), wire_mode=position_wire_mode(j)) + for j in ARM_JOINTS + }, + ) + + @dataclass class AxolConfig: """Top-level configuration for both arms and grippers. @@ -710,6 +1345,25 @@ class AxolConfig: round-trips cleanly (loading a dumped config and resolving it again is idempotent). right_stiffness: Same, for the **right** arm. + controller: Which control law the realtime core runs the arms + on — see :data:`CONTROLLERS`. ``"impedance"`` + (default) is the production MIT frame at 240 Hz. + ``"position"`` puts every joint on its firmware + position loop (``wire_mode`` ``a4`` / ``pv``, the + ``firmware`` gains) at 400 Hz. Like stiffness it is + baked into the per-joint ``wire_mode`` fields by + :meth:`resolved`; a per-joint ``wire_mode`` set + explicitly under ``"impedance"`` is kept, so one + joint can still be tried on its firmware loop + inside the impedance controller (``tune.motion + --a4``). + impedance_hz: Command rate of the MyActuator impedance joints — + see :data:`IMPEDANCE_RATES`. ``240.0`` (default) is + the verified rate. ``480.0`` runs them every tick of + a 480 Hz core loop with the Damiao wrists at 240 Hz + on alternate ticks (``tune.motion --impedance-hz + 480``) — an experiment: their gains, host damping + and feedforward were tuned at 240. """ left: ArmConfig = field( @@ -722,6 +1376,28 @@ class AxolConfig: max_step_rad: float = 0.5 left_stiffness: float | list[float] = 1.0 right_stiffness: float | list[float] = 1.0 + controller: str = "impedance" + impedance_hz: float = IMPEDANCE_LOOP_HZ + + @property + def loop_hz(self) -> float: + """The realtime-core tick rate this config runs at. + + :data:`CONTROLLER_LOOP_HZ` for a uniform arm — 240 Hz all on + impedance, 400 Hz all on firmware loops (``controller`` + ``"position"``) — and :data:`MIXED_LOOP_HZ` when some arm joints are + on their firmware loops and some on impedance, so the impedance ones + keep exactly 240 Hz on alternate ticks. At ``impedance_hz`` + :data:`FAST_IMPEDANCE_HZ` any impedance joint makes it that rate. + """ + mit = impedance_joints(self) + if not mit: + return CONTROLLER_LOOP_HZ["position"] + if fast_impedance_joints(self): + return FAST_IMPEDANCE_HZ + if len(mit) < 2 * len(ARM_JOINTS): + return MIXED_LOOP_HZ + return CONTROLLER_LOOP_HZ["impedance"] def resolved(self) -> "AxolConfig": """Return a copy with stiffness baked into the ``left``/``right`` gains. @@ -734,11 +1410,41 @@ def resolved(self) -> "AxolConfig": applied once at the single robot-construction boundary (``Axol.__init__``) so every consumer sees consistent gains while the unresolved config stays safe to serialize and reload. + + The ``controller`` is baked in the same way: ``"position"`` sets + every joint's ``wire_mode`` to its vendor's firmware position loop + (:func:`position_wire_mode`); ``"impedance"`` leaves the per-joint + fields as configured. The field itself is kept (the core reads its + loop rate from it). """ + if self.controller not in CONTROLLERS: + raise ValueError( + f"controller {self.controller!r} is not one of {list(CONTROLLERS)}" + ) + if not any(abs(self.impedance_hz - hz) < 1e-6 for hz in IMPEDANCE_RATES): + raise ValueError( + f"impedance_hz {self.impedance_hz:g} is not one of " + f"{[f'{hz:g}' for hz in IMPEDANCE_RATES]}" + ) + for side in ("left", "right"): + for j in ARM_JOINTS: + own = getattr(getattr(self, side), j.value).impedance_hz + if own is not None and not any( + abs(own - hz) < 1e-6 for hz in IMPEDANCE_RATES + ): + raise ValueError( + f"{side}.{j.value}.impedance_hz {own:g} is not one of " + f"{[f'{hz:g}' for hz in IMPEDANCE_RATES]}" + ) + left = _apply_stiffness(self.left, self.left_stiffness) + right = _apply_stiffness(self.right, self.right_stiffness) + if self.controller == "position": + left = _on_position_loops(left) + right = _on_position_loops(right) return replace( self, - left=_apply_stiffness(self.left, self.left_stiffness), - right=_apply_stiffness(self.right, self.right_stiffness), + left=left, + right=right, left_stiffness=1.0, right_stiffness=1.0, ) diff --git a/almond_axol/robot/control.py b/almond_axol/robot/control.py index cccb03a0..4c366d8c 100644 --- a/almond_axol/robot/control.py +++ b/almond_axol/robot/control.py @@ -1,4 +1,4 @@ -"""Motor control utilities: friction model, differentiator, contact watchdog. +"""Motor control utilities: friction/stiction models, differentiator, contact watchdog. Gravity compensation is handled separately — see :class:`almond_axol.robot.gravity.GravityCompensator` — because the simple @@ -139,6 +139,16 @@ def update(self, residuals, now: float | None = None) -> tuple[str, float] | Non FRICTION_FF_K_MAX = 100.0 +def coulomb_unit(velocity: float, k: float) -> float: + """Saturation of the Coulomb feedforward in ``[-1, 1]``: ``tanh(0.1·k·v)``. + + The factor :func:`compute_friction` multiplies ``Fc`` by, with the same + :data:`FRICTION_FF_K_MAX` cap. Exposed so :func:`stiction_compensation` + can fade itself out exactly as the velocity term takes over. + """ + return math.tanh(0.1 * min(k, FRICTION_FF_K_MAX) * velocity) + + def compute_friction( velocity: float, Fc: float, k: float, Fv: float, Fo: float ) -> float: @@ -147,9 +157,156 @@ def compute_friction( ``k`` is capped at :data:`FRICTION_FF_K_MAX` (see above) so the Coulomb term ramps smoothly through zero crossings instead of stepping. """ - return ( - Fc * math.tanh(0.1 * min(k, FRICTION_FF_K_MAX) * velocity) + Fv * velocity + Fo - ) + return Fc * coulomb_unit(velocity, k) + Fv * velocity + Fo + + +def stiction_amplitude( + Fc: float, gain: float, load_gain: float, gravity: float +) -> float: + """Peak stiction push (Nm): ``gain·Fc + load_gain·|gravity|``. + + Gear friction is not a constant: the breakaway measured on right + shoulder_1 (X8-P20, 1:20) was 0.66 Nm at the rest pose but 2-3.3 Nm with + the arm extended under 10-15 Nm of gravity load — the transmitted torque + loads the gear meshes and their static friction scales with it. A push + sized for the loaded pose would hunt at rest, one sized for rest does + nothing under load, so the amplitude follows the gravity feedforward + the joint is carrying this cycle (``load_gain`` in Nm per Nm). + """ + return gain * Fc + load_gain * abs(gravity) + + +# Measured joint speed (rad/s) over which the stiction push fades out — +# ``1 − tanh(|v_meas| / STICTION_FADE_VEL)``. About 1.4 LSB of the motor's +# reported velocity (0.022 rad/s): a joint reading one LSB of motion keeps +# ~40 % of the push, two LSBs ~10 %. The fade used to follow the *commanded* +# velocity, which kept the push on through the whole slip phase of a slow +# move (the command is slow, so the velocity feedforward never saturates), +# so the joint was driven past the target, the push flipped sign and it +# re-stuck — a 2 Hz limit cycle in place of the stairs. Measured velocity +# hands over to the sliding feedforward the moment the joint actually moves. +STICTION_FADE_VEL = 0.03 + +# Per-channel phase offset of the torque dither (rad): the golden angle, +# π(3 − √5), so seven joints never push the structure in unison. +DITHER_PHASE_STAGGER = math.pi * (3.0 - math.sqrt(5.0)) + + +def stiction_compensation( + err: float, v_meas: float, amp: float, err_scale: float +) -> float: + """Error-sign Coulomb compensation: + ``amp·tanh(err/err_scale)·(1 − tanh(|v_meas|/STICTION_FADE_VEL))``. + + The velocity feedforward above is driven by the *commanded* velocity, so + at the creeping speeds where a geared joint stick-slips (the X8-P20 + shoulders below ~0.1 rad/s) it delivers well under half of ``Fc`` and + nothing at all while the joint sits stuck with the target walking away + from it. Breakaway there costs ``(F_static − F_ff)/kp`` of tracking + error — the 0.5°, 2 Hz stairs measured on right shoulder_1 — and a pure + velocity feedforward cannot shrink them: whatever level it holds, the + joint still jumps by ``(F_static − F_kinetic)/kp`` when it lets go. + + This term acts on the *measured* error instead (``err = q_des − q_meas``): + it pushes up to ``amp`` (see :func:`stiction_amplitude`) toward the + target while the joint is stuck, saturating within ``err_scale`` so the + effective stiffness near zero error is far above ``kp`` and breakaway + happens after a fraction of the stair. It fades on the *measured* + velocity (:data:`STICTION_FADE_VEL`), so it is gone as soon as the joint + slides and the velocity feedforward takes over — it never drives the + slip phase. + + Keep ``amp`` below the breakaway torque ``axol tune.breakaway`` measures + at the corresponding load (compensation that exceeds the real static + friction hunts around the target at rest). ``amp == 0`` (the default on + every joint) disables the term exactly. + """ + if amp == 0.0: + return 0.0 + fade = 1.0 - math.tanh(abs(v_meas) / STICTION_FADE_VEL) + return amp * math.tanh(err / max(err_scale, 1e-9)) * fade + + +# Speed (rad/s) over which the Stribeck term passes through zero — a hair above +# the measured-velocity noise, so the sign change is smooth and a joint at rest +# gets no push from it (breakaway is the stiction term's job). +STRIBECK_V0 = 0.02 + + +def stribeck_amplitude( + gain: float, dfs: float, load_gain: float, gravity: float +) -> float: + """Excess of low-speed over sliding friction (Nm) this cycle: + ``gain·(dfs + load_gain·|gravity|)``, load-scaled like the stiction push.""" + return gain * (dfs + load_gain * abs(gravity)) + + +def stribeck_excess( + v_meas: float, amp: float, v_s: float, v0: float = STRIBECK_V0 +) -> float: + """Friction cancellation keyed on *measured* velocity: + ``amp·exp(−(v/v_s)²)·tanh(v/v0)``. + + The X8-P20 shoulders' friction falls as they speed up — 2.1 Nm sliding + at 0.05 rad/s, 1.4 at 0.1, 0.7 at 0.2 rad/s under load — and that + negative slope is negative damping: a joint that speeds up sees less + resistance and speeds up more, until the impedance spring catches it and + it slows back into the friction rise and sticks. That is the 2 Hz + stick-slip cycle no command-driven feedforward can stabilise, because a + term computed from the *commanded* velocity does not change when the + real velocity does. + + This term follows the measured velocity with the measured curve's + shape, so when the joint speeds up the feedforward drops by what the real + friction drops and the net slope is flattened; at ``v_s`` (where the + excess has fallen to 1/e) it hands over to the ordinary Coulomb term. + ``gain`` below 1 under-cancels and leaves some cycle; above the true curve + it over-cancels and the net damping goes negative, which shows as the + arm's 3 Hz mode growing. Zero at rest, so it cannot hunt. + """ + if amp == 0.0 or v_s <= 0.0: + return 0.0 + return amp * math.exp(-((v_meas / v_s) ** 2)) * math.tanh(v_meas / max(v0, 1e-9)) + + +def dither_step(phase: float, nm: float, hz: float, dt: float) -> tuple[float, float]: + """Advance a torque-dither oscillator one step: ``(new_phase, torque)``. + + A small sinusoidal torque on the feedforward keeps a geared joint's + meshes in the sliding regime instead of letting them re-stick between + control cycles. The X8-P20 shoulders' friction is velocity-weakening + (2.1 Nm sliding at 0.05 rad/s, 0.7 Nm at 0.2 rad/s, 2.4-3.3 Nm static + under load), which is what turns a slow move into a 2 Hz stick-slip + cycle no feedforward can stabilise; dither flattens that curve at the + contact. ``hz`` has to clear the arm's structural modes (up to ~35 Hz) + and stay under the Nyquist of the loop emitting it (120 Hz in the core), + so 50-80 Hz. ``nm == 0`` (the default on every joint) is exactly zero and + leaves the phase alone. + """ + if nm == 0.0 or hz <= 0.0: + return phase, 0.0 + phase = math.fmod(phase + 2.0 * math.pi * hz * dt, 2.0 * math.pi) + return phase, nm * math.sin(phase) + + +class TorqueDither: + """N-channel torque dither (see :func:`dither_step`), each channel a golden + angle further round the cycle. Sample spacing comes from the wall clock, + like :class:`BandPass`.""" + + def __init__(self, n: int) -> None: + self._phase = [i * DITHER_PHASE_STAGGER for i in range(n)] + self._last: float | None = None + + def update(self, nm: Sequence[float], hz: Sequence[float]) -> list[float]: + now = time.perf_counter() + dt = 0.0 if self._last is None else now - self._last + self._last = now + out: list[float] = [] + for i in range(len(self._phase)): + self._phase[i], torque = dither_step(self._phase[i], nm[i], hz[i], dt) + out.append(torque) + return out class BandPass: diff --git a/almond_axol/rt/link.py b/almond_axol/rt/link.py index f955c3ff..610359a4 100644 --- a/almond_axol/rt/link.py +++ b/almond_axol/rt/link.py @@ -41,7 +41,7 @@ #: both together whenever the config or target layout changes meaning — a #: package and a binary from different checkouts must fail at configure #: time, not arm and then silently reject every target. -CONFIG_PROTO = 2 +CONFIG_PROTO = 16 def config_header() -> list[str]: diff --git a/almond_axol/rt/robot.py b/almond_axol/rt/robot.py index dbc32cc7..adb3dcb2 100644 --- a/almond_axol/rt/robot.py +++ b/almond_axol/rt/robot.py @@ -79,9 +79,15 @@ from ..motor import ControlMode, Joint, Motor, MotorError, MotorGains, MotorStatus from ..motor.bus import CanBus from ..motor.motor import _JOINT_CONFIG -from ..robot.axol import AxolArm, AxolHardware, _rollback_newly_enabled_motors +from ..robot.axol import ( + AxolArm, + AxolHardware, + _rollback_newly_enabled_motors, + apply_firmware_gains, + held_firmware_gain_mismatches, +) from ..robot.base import RobotBase, mark_hardware_cleanup_uncertain -from ..robot.config import AxolConfig +from ..robot.config import AxolConfig, JointConfig, check_loop_hz from ..settings import SHARED from .link import FeedbackSlot, RtLink, config_header @@ -95,6 +101,26 @@ # ``VRTeleopConfig.reset_gravity_comp_kd`` (the classic contact hold). _LIMP_KD = 0.25 +# Wire-mode tokens the core understands (``bringup::WireMode::parse``). +_WIRE_MODES = frozenset({"mit", "a4", "pv"}) +#: The firmware-position-loop tokens and the motor ids they are valid for: +#: ``a4`` is a MyActuator command (ids 1-5), ``pv`` a Damiao one (the +#: wrists, 6-7). The core would refuse neither on the wire — the motor would +#: simply ignore the frame — so the mismatch is caught here. +_WIRE_VENDOR_IDS = {"a4": range(1, 6), "pv": range(6, 8)} + + +def tf_nm_per_pct(joint: Joint, gains: JointConfig) -> float: + """The 0x73 feedforward scale the core takes for a joint: output-shaft Nm + per 1% of rated current — its torque constant times + ``firmware.tf_rated_current_a`` over 100 — or 0 (plain 0xA4) when the + rated current is unset or the joint is not a MyActuator motor.""" + cfg = _JOINT_CONFIG[joint] + rated = gains.firmware.tf_rated_current_a + if rated is None or cfg.motor_id > 5: + return 0.0 + return float(cfg.kt) * float(rated) / 100.0 + class Axol(RobotBase): """Dual-arm Axol robot interface. @@ -132,7 +158,7 @@ def __init__( left_joints: Iterable[Joint] | None = None, right_joints: Iterable[Joint] | None = None, *, - loop_hz: float = 240.0, + loop_hz: float | None = None, watchdog_ms: float = 150.0, max_vel: float = 2.0 * math.pi, max_accel: float = 7.0 * math.pi, @@ -166,7 +192,10 @@ def __init__( changing: Args: - loop_hz: Core tick rate. + loop_hz: Core tick rate. ``None`` (default) follows + ``config.controller``: 240 Hz on the impedance controller, + 400 Hz on the firmware position controller + (:data:`almond_axol.robot.config.CONTROLLER_LOOP_HZ`). watchdog_ms: Core watchdog — how long it holds the last target without a fresh one before treating the host as gone. max_vel: Teleop joint-velocity cap (rad/s) — the core's tracker @@ -198,7 +227,7 @@ def _wrap( cls, hardware: AxolHardware, *, - loop_hz: float = 240.0, + loop_hz: float | None = None, watchdog_ms: float = 150.0, max_vel: float = 2.0 * math.pi, max_accel: float = 7.0 * math.pi, @@ -225,13 +254,17 @@ def _init_core( self, hardware: AxolHardware, *, - loop_hz: float, + loop_hz: float | None, watchdog_ms: float, max_vel: float, max_accel: float, record: str | None, ) -> None: self._robot = hardware + if loop_hz is None: + loop_hz = self._axol_config().loop_hz + # Impedance (MIT) joints run at 240 Hz only, whoever asked otherwise. + check_loop_hz(self._axol_config(), loop_hz) # ``_core_started``: an ``axol-rt`` process exists for this session # (from ``enable`` until teardown) — teardown must go through the # core. ``_armed``: the core holds the buses (from its ``arm`` ack @@ -298,6 +331,10 @@ def _require_quiet_bus(self, what: str) -> None: "enable(), or after disable()." ) + def _axol_config(self) -> AxolConfig: + """The (resolved) robot config the arms were built from.""" + return self._arms()[0][1]._config + def _arms(self) -> list[tuple[int, AxolArm]]: out = [] if self._robot.left is not None: @@ -306,11 +343,37 @@ def _arms(self) -> list[tuple[int, AxolArm]]: out.append((1, self._robot.right)) return out - def _config_text(self) -> str: - max_step = self._arms()[0][1]._config.max_step_rad + def _config_text(self, *, cogging: bool = False) -> str: + """The core's config. + + ``cogging``: also the joints' ``cogging`` lines (position-periodic + torque cancellation), which need the resolved joint offsets — the + motor-frame series is the joint-frame one shifted by the offset — so + they go on the second configure, after :meth:`_enable` resolved them. + """ + + def _wire_token(mode: str, joint: Joint, motor_id: int) -> str: + token = str(mode).lower() + if token not in _WIRE_MODES: + raise ValueError( + f"wire_mode {mode!r} is not one of {sorted(_WIRE_MODES)}" + ) + ids = _WIRE_VENDOR_IDS.get(token) + if ids is not None and motor_id not in ids: + vendor = "MyActuator" if token == "a4" else "Damiao" + raise ValueError( + f"wire_mode {token!r} is the {vendor} position loop; " + f"{joint.value} (motor {motor_id}) is not a {vendor} motor — " + f"use {'pv' if token == 'a4' else 'a4'}, or " + "AxolConfig.controller = 'position' to pick per vendor" + ) + return token + + max_step = self._axol_config().max_step_rad lines = [ *config_header(), f"loop_hz {self._loop_hz}", + f"impedance_hz {self._axol_config().impedance_hz}", f"watchdog_ms {self._watchdog_ms}", # Corruption defense on the core side; the Python max-step gate # in motion_control is the real per-command limit. @@ -333,14 +396,63 @@ def _config_text(self) -> str: lines.append( f"joint {side} {iface} {j.value} {motor_id} " f"{gains.kp} {gains.kd} {trk_vel} {trk_acc} " - f"{f.fc} {f.k} {f.fv} {f.fo}" + f"{f.fc} {f.k} {f.fv} {f.fo} " + f"{gains.stiction_gain} {math.radians(gains.stiction_err_deg)} " + f"{gains.stiction_load_gain} {gains.dither_nm} {gains.dither_hz} " + f"{_wire_token(gains.wire_mode, j, motor_id)} " + f"{gains.stribeck_gain} {gains.stribeck_dfs} " + f"{gains.stribeck_load_gain} {gains.stribeck_vs} {f.fl} " + f"{gains.stribeck_pole} " + # 0xA4 speed-cap tracking (the planner's; 0 = fixed cap), + # target lead (ms), and the 0x73 feedforward scale (0 = + # plain 0xA4). + f"{gains.firmware.cap_track or 0.0} " + f"{gains.firmware.planner_lead_ms or 0.0} " + f"{tf_nm_per_pct(j, gains)} " + # This joint's impedance rate (0 = the config-wide one). + f"{gains.impedance_hz or 0.0}" ) + if cogging and gains.cogging is not None and gains.cogging_gain != 0.0: + offset = float(arm._joint_offsets[ARM_JOINTS.index(j)]) + if math.isfinite(offset): + terms = gains.cogging.motor_terms(offset, gains.cogging_gain) + lines.append( + f"cogging {side} {iface} {motor_id} {len(terms)} " + + " ".join(f"{w!r} {a!r} {b!r}" for w, a, b in terms) + ) if arm._has_gripper: lines.append( f"gripper {side} {iface} {_JOINT_CONFIG[Joint.GRIPPER].motor_id}" ) return "\n".join(lines) + "\n" + def _warn_wire_modes(self) -> None: + firmware = [ + f"{'left' if side == 0 else 'right'}.{j.value}" + for side, arm in self._arms() + for j in ARM_JOINTS + if j in arm.motors + and str(getattr(arm._arm_config, j.value).wire_mode).lower() in ("a4", "pv") + ] + if not firmware: + return + controller = self._axol_config().controller + if controller == "position": + _logger.warning( + "rt: position controller — %s on the firmware position loop " + "at %.0f Hz: no compliance, no host feedforward, torque telemetry " + "NaN on the a4 joints — the contact watchdog cannot see them", + ", ".join(firmware), + self._loop_hz, + ) + return + _logger.warning( + "rt: %s on the firmware position loop (wire_mode a4 / pv): no " + "compliance, no host feedforward, torque telemetry NaN on a4 joints " + "— the contact watchdog cannot see these joints", + ", ".join(firmware), + ) + async def enable(self, hold: bool = True) -> None: """Bring every motor up. @@ -365,6 +477,7 @@ async def enable(self, hold: bool = True) -> None: self._require_quiet_bus("enable(hold=False)") await self._robot.enable(hold=False) return + self._warn_wire_modes() try: await self._enable() except BaseException as setup_error: @@ -438,15 +551,40 @@ async def _enable(self) -> None: # is still holding, so this is exactly the classic held/cold split. # Only the cold set is rolled back if the bring-up fails from here. cold: list[tuple[str, Motor]] = [] + cold_joints: dict[int, list[Joint]] = {} for side, arm in self._arms(): label = "left" if side == 0 else "right" flags = await arm.get_holding() for joint, holding in zip(arm.motors, flags): if not holding: cold.append((f"{label}.{joint.value}", arm.motors[joint])) + cold_joints.setdefault(side, []).append(joint) self._enable_cold = cold - for _side, arm in self._arms(): + # A held joint is never reset, so apply_firmware_gains cannot reach + # it: refuse a run whose held joints do not already run the firmware + # gains it asks for, rather than silently test the old ones. + stale: list[str] = [] + for side, arm in self._arms(): + held = [j for j in arm.motors if j not in cold_joints.get(side, [])] + stale += await held_firmware_gain_mismatches(arm, held) + if stale: + raise MotorError( + "joints found holding from an earlier session run different firmware " + "gains than this run wants — a holding motor cannot take a ROM write: " + + "; ".join(stale) + + ". Power-cycle the arm (or disable it) and run again." + ) + + for side, arm in self._arms(): + # The configured firmware loop gains (wire_mode a4's controller) + # go to ROM now: the cold joints have just been reset by prep and + # are disabled, which is the only state a MyActuator commits a + # ROM write in, and the bus is quiet. Held joints are skipped. A + # motor that took a write is reset again so its loop loads the + # new gains — hence this runs *before* the offsets are resolved + # from the (post-reset) multi-turn reading. + await apply_firmware_gains(arm, cold_joints.get(side, [])) await arm.resolve_joint_offsets() # Python never calls Motor.enable() in production control, so run the # MyActuator capability detection (position/torque decode ranges) @@ -459,6 +597,17 @@ async def _enable(self) -> None: await driver._detect_capabilities() await driver._apply_low_voltage_threshold() + # The offsets are resolved: hand the core the cogging cancellation, + # which lives in the motor frame. Re-sending the config replaces the + # one the prep ran from; the bus threads start from this one at arm. + if any( + getattr(arm._arm_config, j.value).cogging is not None + for _side, arm in self._arms() + for j in ARM_JOINTS + if j in arm.motors + ): + await self._link.configure(self._config_text(cogging=True)) + # Gripper bring-up runs from Python while the bus is still quiet — # the exact classic flow (enable/calibrate or attach/restore) the # core can't do. The core then streams its POSITION_FORCE commands. diff --git a/almond_axol/serve/app.py b/almond_axol/serve/app.py index 872bf147..b2033d98 100644 --- a/almond_axol/serve/app.py +++ b/almond_axol/serve/app.py @@ -1936,7 +1936,10 @@ async def tuning_gains() -> dict[str, Any]: file overlaid — exactly what a tuning run uses when a gain field is left empty. The workbench shows these as the slider baselines. ``kd_host_hz`` is resolved to the shared default where a joint - doesn't set its own band centre. + doesn't set its own band centre. ``wire_modes`` carries each joint's + configured controller (``mit`` impedance or ``a4`` firmware position + loop): the Recorded-motion tab's per-joint controller picker seeds + from it, since a run adds ``--a4`` joints on top of the config. """ import math @@ -1947,11 +1950,14 @@ async def tuning_gains() -> dict[str, Any]: def _load() -> dict[str, Any]: cfg = AxolConfig() out: dict[str, Any] = {} + wire: dict[str, Any] = {} for side in ("left", "right"): arm_cfg = getattr(cfg, side) joints: dict[str, Any] = {} + modes: dict[str, str] = {} for j in ARM_JOINTS: jc = getattr(arm_cfg, j.value) + modes[j.value] = str(jc.wire_mode).lower() joints[j.value] = { "kp": jc.kp, "kd": jc.kd, @@ -1965,11 +1971,33 @@ def _load() -> dict[str, Any]: jc.kd_host_q if jc.kd_host_q is not None else DAMP_BP_Q ), "j_eff": jc.j_eff, + "stiction_gain": jc.stiction_gain, + "stiction_load_gain": jc.stiction_load_gain, + "dither_nm": jc.dither_nm, + "stribeck_gain": jc.stribeck_gain, + # The cogging ("osc") cancellation's share; the series + # itself is calibration (scripts/cogging_map.py --save). + "cogging_gain": jc.cogging_gain if jc.cogging else None, + # Firmware position-loop set (tune.motion's + # ``firmware.*`` overrides): the grid's baselines. + **{ + f"firmware.{name}": getattr(jc.firmware, name) + for name in ( + "position_kp", + "speed_kp", + "speed_ki", + "planner_accel", + "cap_track", + "planner_lead_ms", + "tf_rated_current_a", + ) + }, } out[side] = joints - return out + wire[side] = modes + return {"gains": out, "wire_modes": wire} - return {"gains": await asyncio.to_thread(_load)} + return await asyncio.to_thread(_load) @app.get("/api/tuning/runs") async def tuning_runs() -> dict[str, Any]: diff --git a/almond_axol/serve/commands.py b/almond_axol/serve/commands.py index 48e83245..0d35ad93 100644 --- a/almond_axol/serve/commands.py +++ b/almond_axol/serve/commands.py @@ -561,6 +561,28 @@ def load() -> Any: requires_hardware=True, drives_motors=True, hardware_profiles=("axol",), + # Records the wrist cameras' IMUs (almond_axol.tuning.wrist_imu). + uses_cameras=True, + section="tuning", + ), + "tune.a4": CommandDef( + "tune.a4", + "tune.a4", + "Firmware position loop (0xA4)", + "Tune a MyActuator joint's own position/speed loop — the controller " + "behind wire_mode a4 — with a sine or constant-speed triangle: set " + "firmware gains (RAM unless persisted), the speed cap and the " + "planner acceleration, score tracking and creep smoothness, and " + "save the run. A buzz guard restores the previous gains on any " + "high-frequency motion.", + "Diagnostics", + "argparse", + _argparse_loader("..cli.tune.a4"), + requires_hardware=True, + drives_motors=True, + hardware_profiles=("axol",), + # Records the wrist cameras' IMUs (almond_axol.tuning.wrist_imu). + uses_cameras=True, section="tuning", ), "tune.friction": CommandDef( @@ -640,6 +662,8 @@ def load() -> Any: requires_hardware=True, drives_motors=True, hardware_profiles=("axol",), + # Records the wrist cameras' IMUs (almond_axol.tuning.wrist_imu). + uses_cameras=True, section="tuning", ), "tune.filter": CommandDef( diff --git a/almond_axol/serve/introspect.py b/almond_axol/serve/introspect.py index 94c953b4..f44413bb 100644 --- a/almond_axol/serve/introspect.py +++ b/almond_axol/serve/introspect.py @@ -34,6 +34,7 @@ "mantis_source": ["quest", "lighthouse", "ultimate"], "dataset_resolution": ["SVGA", "HD1080", "HD1200"], "eyes": ["both", "left", "right"], + "controller": ["impedance", "position"], "policy_type": [ "act", "smolvla", diff --git a/almond_axol/serve/robot_link.py b/almond_axol/serve/robot_link.py index 6b86c3f3..d2970cd6 100644 --- a/almond_axol/serve/robot_link.py +++ b/almond_axol/serve/robot_link.py @@ -1164,6 +1164,16 @@ async def read(coro: Any) -> Any: status = await read(motor.get_error_code()) mode = await read(motor.get_control_mode()) gains = await read(motor.get_gains()) + # MyActuator only: the 0xA4 position planner's stored acceleration — + # 0 = direct tracking (what wire_mode a4 and tune.a4 need), anything + # else re-plans every streamed target. Shown and edited on the + # dashboard's Firmware-loop tab next to the loop gains. + planner = None + driver = getattr(motor, "_driver", None) + if isinstance(driver, MyActuatorMotor): + acc = await read(driver.get_planner_acceleration()) + if acc is not None: + planner = {"accel": acc[0], "decel": acc[1]} return { "arm": arm_link.side, "joint": joint.name, @@ -1177,4 +1187,5 @@ async def read(coro: Any) -> Any: "temperature": await read(motor.get_temperature()), "voltage": await read(motor.get_voltage()), "gains": vars(gains) if gains is not None else None, + "planner": planner, } diff --git a/almond_axol/teleop/recorder.py b/almond_axol/teleop/recorder.py index 39bfb165..7093defc 100644 --- a/almond_axol/teleop/recorder.py +++ b/almond_axol/teleop/recorder.py @@ -82,13 +82,21 @@ "friction_ff", "inertia_ff", "damping_ff", + "stiction_ff", + "dither_ff", + "stribeck_ff", "total_ff", "kd_host", "damp_w0", "damp_q", "tick_dt", "fb_dt", + "cogging_ff", + "tf_pct", ) +# The layout before the cogging cancellation and 0x73 feedforward columns: a +# CSV a proto-14 core left behind still compacts. +_RT_TRACE_COLUMNS_V1 = _RT_TRACE_COLUMNS[:-2] def resolve_prefix(prefix: str) -> str: @@ -123,18 +131,25 @@ def compact_rt_trace(prefix: str) -> Path | None: with path.open("rb") as raw: header = raw.readline().decode("ascii", "replace").strip().split(",") has_rows = bool(raw.read(1)) - if header != list(_RT_TRACE_COLUMNS): + if header == list(_RT_TRACE_COLUMNS): + columns = _RT_TRACE_COLUMNS + elif header == list(_RT_TRACE_COLUMNS_V1): + columns = _RT_TRACE_COLUMNS_V1 + else: raise ValueError(f"unexpected Rust trace schema in {path}") if not has_rows: continue values = np.loadtxt(path, delimiter=",", skiprows=1, ndmin=2) - if values.shape[1] != len(_RT_TRACE_COLUMNS): + if values.shape[1] != len(columns): raise ValueError( f"unexpected Rust trace width in {path}: {values.shape[1]}" ) found_rows = True chunks["side"].append(np.full(len(values), side, dtype=np.uint8)) - for index, source_name in enumerate(_RT_TRACE_COLUMNS): + for source_name in _RT_TRACE_COLUMNS[len(columns) :]: + # A legacy file has no such column: NaN, so both sides concatenate. + chunks[source_name].append(np.full(len(values), np.nan, dtype=np.float32)) + for index, source_name in enumerate(columns): name = "t" if source_name == "time_s" else source_name if source_name == "tick": array = values[:, index].astype(np.uint64) diff --git a/almond_axol/tuning/cogging.py b/almond_axol/tuning/cogging.py new file mode 100644 index 00000000..cd51ad5f --- /dev/null +++ b/almond_axol/tuning/cogging.py @@ -0,0 +1,192 @@ +"""Fit a joint's position-periodic torque (cogging / gear mesh) for cancellation. + +The realtime core cancels it by feedforward (``JointConfig.cogging``, +``filter::cogging`` in the core): a Fourier series in the joint angle, fitted +here from a slow constant-speed sweep — ``axol tune.friction --raw-csv``, +where every cruise sample carries the angle, the torque the motor supplied +and the sweep direction. + +Only the position-periodic part is wanted, so each direction's torque is +high-passed **in the angle domain** first: subtracting its moving average over +``detrend_deg`` removes gravity, friction (constant per direction at one +speed) and anything else slow in angle, and leaves the ripple. The window is +rounded to a whole number of fundamental periods — a boxcar that long has a +null at every harmonic, so none of the ripple leaks into the trend (a 6° +window on the 1.81° harmonic leaked 8% and inflated the fit by as much). The torque the +motor supplied to hold the sweep through a bump is the torque to *add*, so the +fit is used as-is — no sign flip. + +On the right shoulder_1 the ripple sits at 1.81° and 0.905° (with a smaller +3.62° term): a 3.62° fundamental with harmonics 1, 2 and 4 covers it, which +is the default. +""" + +from __future__ import annotations + +import math +from dataclasses import dataclass + +import numpy as np + +from ..robot.config import CoggingModel + +#: The right shoulder_1's series: 3.62° fundamental, harmonics at 3.62°, +#: 1.81° and 0.905° (2026-09-23 analysis of the 2026-09-18 sweep). +DEFAULT_PERIOD_DEG = 3.62 +DEFAULT_HARMONICS = (1, 2, 4) + +_GRID_DEG = 0.02 + + +@dataclass(frozen=True) +class CoggingFit: + """A fitted series and how well it explains the sweep's ripple.""" + + model: CoggingModel + #: Share of the detrended ripple the series explains (0..1). + r2: float + #: Amplitude (Nm) of each harmonic, in ``model.harmonics`` order. + amplitudes: tuple[float, ...] + #: RMS (Nm) of the detrended ripple the fit saw. + ripple_rms: float + samples: int + + +def angle_highpass( + q_deg: np.ndarray, tau: np.ndarray, detrend_deg: float = 6.0 +) -> tuple[np.ndarray, np.ndarray]: + """``(q, ripple)`` for one sweep direction: the torque minus its moving + average over ``detrend_deg`` of travel, on a fine angle grid. + + Returns empty arrays when the direction spans less than three windows — + too short to separate a ripple from the trend. + """ + q_deg = np.asarray(q_deg, dtype=float) + tau = np.asarray(tau, dtype=float) + ok = np.isfinite(q_deg) & np.isfinite(tau) + q_deg, tau = q_deg[ok], tau[ok] + if len(q_deg) < 10: + return np.empty(0), np.empty(0) + order = np.argsort(q_deg) + qs, ts = q_deg[order], tau[order] + grid = np.arange(qs[0], qs[-1], _GRID_DEG) + n = max(1, int(round(detrend_deg / _GRID_DEG))) + if len(grid) < 3 * n: + return np.empty(0), np.empty(0) + values = np.interp(grid, qs, ts) + trend = np.convolve(values, np.ones(n) / n, mode="same") + keep = slice(n // 2, len(grid) - n // 2) + return grid[keep], (values - trend)[keep] + + +def _whole_periods(window_deg: float, period_deg: float) -> float: + """``window_deg`` rounded to a whole number (≥ 1) of ``period_deg``.""" + return max(1, round(window_deg / period_deg)) * period_deg + + +def _signs(direction: np.ndarray) -> np.ndarray: + """``+1`` / ``-1`` per sample from ``+`` / ``-`` tokens or signed numbers.""" + d = np.asarray(direction) + if d.dtype.kind in "OUS": + text = d.astype(str) + return np.where(text == "+", 1.0, np.where(text == "-", -1.0, 0.0)) + return np.sign(d.astype(float)) + + +def _features(q_deg: np.ndarray, period_deg: float, harmonics: tuple[int, ...]): + cols = [] + for k in harmonics: + w = 2.0 * math.pi * k / period_deg + cols += [np.cos(w * q_deg), np.sin(w * q_deg)] + return np.stack(cols, axis=1) + + +def fit_cogging( + q_rad: np.ndarray, + tau_nm: np.ndarray, + direction: np.ndarray, + *, + period_deg: float = DEFAULT_PERIOD_DEG, + harmonics: tuple[int, ...] = DEFAULT_HARMONICS, + detrend_deg: float = 6.0, +) -> CoggingFit: + """Fit the series to a sweep's samples. + + Args: + q_rad: Joint angle of each sample (rad, joint frame). + tau_nm: Torque the motor supplied (Nm). + direction: Sweep direction per sample (``+`` / ``-``, or its sign). + period_deg: The series' fundamental period. + harmonics: Harmonic numbers to fit. + detrend_deg: Angle-domain high-pass window (see :func:`angle_highpass`). + + Raises: + ValueError: When no direction spans enough travel to fit. + """ + if period_deg <= 0.0 or not harmonics or min(harmonics) < 1: + raise ValueError("period_deg must be > 0 and harmonics >= 1") + detrend_deg = _whole_periods(detrend_deg, period_deg) + q_deg = np.degrees(np.asarray(q_rad, dtype=float)) + tau_nm = np.asarray(tau_nm, dtype=float) + sign = _signs(direction) + qs, rs = [], [] + for s in (1.0, -1.0): + sel = sign == s + q, r = angle_highpass(q_deg[sel], tau_nm[sel], detrend_deg) + qs.append(q) + rs.append(r) + q = np.concatenate(qs) + r = np.concatenate(rs) + if len(q) < 50: + raise ValueError( + f"too little travel to fit: each sweep direction needs > " + f"{3 * detrend_deg:g}° of cruise samples" + ) + x = _features(q, period_deg, tuple(harmonics)) + coef, *_ = np.linalg.lstsq(x, r, rcond=None) + resid = r - x @ coef + var = float(np.var(r)) + r2 = 1.0 - float(np.var(resid)) / var if var > 0 else 0.0 + model = CoggingModel( + period_deg=float(period_deg), + harmonics=tuple( + (int(k), float(coef[2 * i]), float(coef[2 * i + 1])) + for i, k in enumerate(harmonics) + ), + ) + return CoggingFit( + model=model, + r2=r2, + amplitudes=tuple(float(math.hypot(a, b)) for _, a, b in model.harmonics), + ripple_rms=float(np.sqrt(np.mean(r**2))), + samples=len(q), + ) + + +def prediction_r( + model: CoggingModel, + q_rad: np.ndarray, + tau_nm: np.ndarray, + direction: np.ndarray, + detrend_deg: float = 6.0, +) -> float: + """Correlation between the model and another sweep's detrended ripple — + the out-of-sample check (a table that only fits its own pass cancels + nothing).""" + detrend_deg = _whole_periods(detrend_deg, model.period_deg) + q_deg = np.degrees(np.asarray(q_rad, dtype=float)) + tau_nm = np.asarray(tau_nm, dtype=float) + sign = _signs(direction) + preds, ripples = [], [] + for s in (1.0, -1.0): + sel = sign == s + q, r = angle_highpass(q_deg[sel], tau_nm[sel], detrend_deg) + if len(q): + preds.append(np.asarray(model.torque(np.radians(q)))) + ripples.append(r) + if not preds: + return float("nan") + p, r = np.concatenate(preds), np.concatenate(ripples) + if np.std(p) == 0 or np.std(r) == 0: + return float("nan") + return float(np.corrcoef(p, r)[0, 1]) diff --git a/almond_axol/tuning/joint_frame.py b/almond_axol/tuning/joint_frame.py index 42578bbc..1a7442e9 100644 --- a/almond_axol/tuning/joint_frame.py +++ b/almond_axol/tuning/joint_frame.py @@ -25,15 +25,16 @@ from ..motor import ControlMode, Joint, Motor from ..motor.damiao import DamiaoMotor from ..motor.myactuator import ( - MyActuatorMotor, _MA_KD_MAX, _MA_KP_MAX, _MA_V_MAX, + MyActuatorMotor, ) from ..robot.axol import ( EITHER_STOP_JOINTS, closer_end_stop, end_stop_offset_from_position, + fixed_stop_wrap_correction, ) @@ -43,20 +44,52 @@ class JointFrameMotor: Construct via :func:`joint_frame_motors`, which resolves the per-joint motor→joint offset. Only the calls the tuners use are exposed; add passthroughs as needed. + + **Boot wrap.** A fixed-stop joint's multi-turn reading can come back + exactly ±360° off after any MyActuator 0x76 reset — which every tuner + issues through ``set_control_mode`` — because the motor re-derives it + within ±180° of the single-turn zero, and the right elbow at rest sits + 34° from that boundary. Commanding the joint frame through a fixed + offset against a wrapped reading sends the motor a full turn: right + elbow, 2026-09-22, into its hard stop at 40 Nm until its stall + protection tripped. Every :meth:`get_position` therefore re-derives the + wrap with :func:`fixed_stop_wrap_correction` (the production bring-up's + check) and folds it into :attr:`frame_offset`, which every command uses. + Read before you command — :func:`~almond_axol.cli.tune.friction._ramp_verified` + does — and a wrapped reading is corrected instead of chased. """ - def __init__(self, motor: Motor, offset: float) -> None: + def __init__( + self, motor: Motor, offset: float, is_left: bool | None = None + ) -> None: self.motor = motor self.offset = offset + self._is_left = is_left + #: ±2π correction the last read said the motor frame needs (0 if none). + self.wrap = 0.0 @property def joint(self) -> Joint: return self.motor.joint + @property + def frame_offset(self) -> float: + """motor→joint offset including the current boot-wrap correction.""" + return self.offset + self.wrap + + def _refresh_wrap(self, motor_pos: float) -> None: + if ( + self._is_left is None + or self.joint in EITHER_STOP_JOINTS + or self.joint == Joint.GRIPPER + ): + return + self.wrap = fixed_stop_wrap_correction(self.joint, self._is_left, motor_pos) + @property def position(self) -> float: """Latest cached position (rad, joint frame).""" - return self.motor.position + self.offset + return self.motor.position + self.frame_offset @property def torque(self) -> float: @@ -69,18 +102,24 @@ def feedback_ts(self) -> float: return self.motor.feedback_ts async def get_position(self) -> float: - """Current position (rad, joint frame).""" - return await self.motor.get_position() + self.offset + """Current position (rad, joint frame), re-deriving the boot wrap. + + Raises ``MotorError`` (from :func:`fixed_stop_wrap_correction`) when + the reading fits no plausible band — the zero is unset or stale. + """ + motor_pos = await self.motor.get_position() + self._refresh_wrap(motor_pos) + return motor_pos + self.frame_offset async def set_impedance( self, p_des: float, v_des: float, kp: float, kd: float, t_ff: float ) -> None: """Impedance command with ``p_des`` in the joint frame.""" - await self.motor.set_impedance(p_des - self.offset, v_des, kp, kd, t_ff) + await self.motor.set_impedance(p_des - self.frame_offset, v_des, kp, kd, t_ff) async def set_position_velocity(self, position: float, max_speed: float) -> None: """Position-velocity command with ``position`` in the joint frame.""" - await self.motor.set_position_velocity(position - self.offset, max_speed) + await self.motor.set_position_velocity(position - self.frame_offset, max_speed) async def set_control_mode(self, mode: ControlMode) -> None: await self.motor.set_control_mode(mode) @@ -125,7 +164,7 @@ async def run_experiment( motor_id=driver._motor_id, differentiate=differentiate, rate_hz=rate_hz, - offset=self.offset, + offset=self.frame_offset, kp=kp, kd=kd, ranges=ranges, @@ -155,5 +194,9 @@ async def joint_frame_motors( ) else: offset = closer_end_stop(j, is_left)[0] - wrapped[j] = JointFrameMotor(m, offset) + jm = JointFrameMotor(m, offset, is_left) + # Read once now: derives the boot wrap (and refuses an unset zero) + # before any tuner commands the joint. + await jm.get_position() + wrapped[j] = jm return wrapped diff --git a/almond_axol/tuning/motions/el_creep.npz b/almond_axol/tuning/motions/el_creep.npz new file mode 100644 index 00000000..8d02e58e Binary files /dev/null and b/almond_axol/tuning/motions/el_creep.npz differ diff --git a/almond_axol/tuning/motions/hold.npz b/almond_axol/tuning/motions/hold.npz new file mode 100644 index 00000000..2ce4d802 Binary files /dev/null and b/almond_axol/tuning/motions/hold.npz differ diff --git a/almond_axol/tuning/motions/s1_creep.npz b/almond_axol/tuning/motions/s1_creep.npz new file mode 100644 index 00000000..bcaa4301 Binary files /dev/null and b/almond_axol/tuning/motions/s1_creep.npz differ diff --git a/almond_axol/tuning/motions/slow_osc.npz b/almond_axol/tuning/motions/slow_osc.npz new file mode 100644 index 00000000..2d07589c Binary files /dev/null and b/almond_axol/tuning/motions/slow_osc.npz differ diff --git a/almond_axol/tuning/runner.py b/almond_axol/tuning/runner.py index 74ba3d98..14ca3abd 100644 --- a/almond_axol/tuning/runner.py +++ b/almond_axol/tuning/runner.py @@ -114,7 +114,11 @@ def probe_clearance_targets(test_joint: Joint, is_left: bool) -> dict[Joint, flo # Three joints hang axis-vertical and need other joints posed to tilt them. # Clearances below were verified against the torso collision model; signal # figures are the CAD gravity model's torque variation over the sweep. -SHOULDER_1_LOAD = math.radians(90.0) # humerus horizontal for shoulder_3 +# Humerus horizontal for shoulder_3 — a *left-arm* joint-frame value. The +# shoulder_1 frame is mirrored across arms (left −90..+180, right −180..+90), +# so the right arm's copy of this pose is −90°: +90° there is the hard stop, +# and an unmirrored raise drove right shoulder_1 into it (2026-09-22). +SHOULDER_1_LOAD = math.radians(90.0) WRIST_2_LOAD = math.radians(85.0) # hand off wrist_1's axis (85°: limit is 90) WRIST_1_LOAD = math.radians(90.0) # hand off wrist_2's axis # shoulder_3 / wrist_1 sweep cap at their loaded poses: ±90° keeps the bent @@ -185,13 +189,15 @@ def sweep_safety( "the base is inboard." ) elif joint == Joint.SHOULDER_3: - clearance[Joint.SHOULDER_1] = SHOULDER_1_LOAD + s1_load = SHOULDER_1_LOAD if is_left else -SHOULDER_1_LOAD + clearance[Joint.SHOULDER_1] = s1_load clearance[Joint.ELBOW] = elbow_mid lo_cap, hi_cap = -LOADED_SWEEP_CAP, LOADED_SWEEP_CAP notes.append( - "Raising shoulder_1 to 90° and bending the elbow so gravity " - "loads shoulder_3 (its axis is vertical at rest — zero gravity " - "moment there); sweep capped at ±90° to stay clear of the torso." + f"Raising shoulder_1 to {math.degrees(s1_load):+.0f}° and bending the " + "elbow so gravity loads shoulder_3 (its axis is vertical at rest — " + "zero gravity moment there); sweep capped at ±90° to stay clear of " + "the torso." ) elif joint == Joint.WRIST_1: clearance[Joint.ELBOW] = elbow_mid diff --git a/almond_axol/tuning/wrist_imu.py b/almond_axol/tuning/wrist_imu.py new file mode 100644 index 00000000..ae46de62 --- /dev/null +++ b/almond_axol/tuning/wrist_imu.py @@ -0,0 +1,464 @@ +"""Record the wrist cameras' IMUs during a tuning run and score the shake. + +The joint encoders sit on the motor side of the gearboxes: they cannot see +backlash, link flex or anything past the last joint, and on 2026-09-22 they +put the right arm's slow-motion tool-tip shake at ~0.6 mm peak-to-peak while +it looked like a couple of millimetres. The ZED X One on each wrist carries an +IMU that measures the gripper's actual motion, so every tuning tool that +moves an arm (``tune.motion``, ``tune.a4``, ``tune.pid``) records it and adds +a ``imu`` block to the run's metrics plus the raw samples to its series. + +Each camera is opened in its own subprocess (:mod:`almond_axol.zed.imu_worker`, +light to import): the ZED SDK can wedge its process, and a stuck SDK must +never stall — or crash — a process that is streaming motor commands. The +worker polls the latest IMU sample, drops repeats by timestamp and maps the +SDK's wall-clock stamps onto ``time.perf_counter`` (``CLOCK_MONOTONIC``, +shared by every process), the clock the tuning logs use. The cameras are exclusive: nothing else may hold +them for the run (the tools declare ``uses_cameras`` so the dashboard blocks +previews meanwhile), and a camera that cannot be opened just means no IMU +metrics — the run itself goes ahead. + +The metric (:func:`shake_metrics`): acceleration band-passed to the shake +band (1–15 Hz — above the motion, below the structure's buzz), integrated +twice in the frequency domain to displacement, and reported as the median and +90th-percentile 2 s peak-to-peak excursion in millimetres — overall (3-D) and +along gravity (vertical, the direction the tool tip was seen to bounce), the +vertical split into 1–3 Hz (the impedance sway) and 3–15 Hz — plus the band's +acceleration and angular-rate RMS and its dominant frequency. +""" + +from __future__ import annotations + +import logging +import math +import os +import subprocess +import sys +import tempfile +import threading +import time +from dataclasses import dataclass, field +from pathlib import Path +from typing import Any + +import numpy as np + +_logger = logging.getLogger(__name__) + +#: The shake band (Hz). Under impedance the visible wobble is at 1–3 Hz — +#: ~2 mm in each of 1–2 and 2–3 Hz on the jelly robot's right arm, against +#: ~1 mm in 3–6 and 0.3 mm in 6–15 (2026-09-23) — so the band starts at 1 Hz; +#: ``slow_osc``'s own commanded motion is below 1 Hz (0.3 mm left in 1–2 Hz). +#: A faster motion leaks more of itself in: the score does not subtract the +#: commanded motion. +SHAKE_BAND = (1.0, 15.0) +#: Sub-bands reported alongside: the impedance sway and the faster shake. +LOW_BAND = (1.0, 3.0) +HIGH_BAND = (3.0, 15.0) + +_OPEN_TIMEOUT_S = 12.0 +_STOP_TIMEOUT_S = 6.0 + + +def imu_serial(side: str) -> int | None: + """The ``{side}_arm`` wrist camera's serial from the shared settings, or + ``None`` when unassigned or the settings cannot be read.""" + try: + from ..settings import load_store + + cameras = load_store(strict=False).cameras() or {} + except Exception as exc: # noqa: BLE001 - no settings → no IMU + _logger.debug("wrist IMU: no camera settings (%s)", exc) + return None + serial = (cameras.get("serials") or {}).get(f"{side}_arm") + try: + serial = int(serial) + except (TypeError, ValueError): + return None + return serial or None + + +@dataclass +class _Recorder: + side: str + serial: int + path: str + proc: subprocess.Popen[str] + ready: threading.Event = field(default_factory=threading.Event) + dumped: threading.Event = field(default_factory=threading.Event) + errors: list[str] = field(default_factory=list) + data: dict[str, np.ndarray] = field(default_factory=dict) + stopped: bool = False + + def send(self, word: str) -> None: + try: + assert self.proc.stdin is not None + self.proc.stdin.write(word + "\n") + self.proc.stdin.flush() + except (OSError, ValueError): + pass + + def close(self) -> None: + """Close the worker's stdin once it is done (it may have exited).""" + try: + if self.proc.stdin is not None: + self.proc.stdin.close() + except (OSError, ValueError): + pass + + def listen(self) -> None: + """Reader thread: the worker's protocol lines → events / errors.""" + assert self.proc.stdout is not None + for line in self.proc.stdout: + word, _, rest = line.strip().partition(" ") + if word == "ready": + self.ready.set() + elif word == "dumped": + self.dumped.set() + elif word == "error": + self.errors.append(rest) + + +class WristImu: + """Record the wrist IMUs of ``sides`` for the length of a ``with`` block. + + >>> with WristImu(["right"]) as imu: + ... t0 = time.perf_counter(); run(); t1 = time.perf_counter() + >>> imu.metrics("right", t0, t1) + + Every failure — no pyzed, no camera assigned, a camera that will not + open — is logged once and leaves that side without data; it never + raises into the run. + """ + + def __init__( + self, + sides: list[str], + *, + enabled: bool = True, + serial_of: Any = None, + worker: str | None = None, + ) -> None: + """``serial_of`` replaces :func:`imu_serial`; ``worker`` (a + ``module:function`` the subprocess runs in place of + :func:`almond_axol.zed.imu_worker.record`) replaces the camera (tests).""" + self._sides = [s for s in sides if s in ("left", "right")] + self._enabled = enabled + self._serial_of = serial_of or imu_serial + self._worker = worker + self._recorders: dict[str, _Recorder] = {} + self._tmp: tempfile.TemporaryDirectory[str] | None = None + + def __enter__(self) -> "WristImu": + self.start() + return self + + def __exit__(self, *exc: object) -> None: + self.stop() + + @property + def sides(self) -> list[str]: + """Sides that are recording (or have recorded) IMU data.""" + return list(self._recorders) + + def start(self) -> None: + if not self._enabled: + return + self._tmp = tempfile.TemporaryDirectory(prefix="axol-imu-") + for side in self._sides: + serial = self._serial_of(side) + if serial is None: + _logger.info( + "wrist IMU: no %s_arm camera assigned (Settings → cameras) — " + "no IMU metrics for the %s arm", + side, + side, + ) + continue + path = str(Path(self._tmp.name) / f"{side}.npz") + cmd = [ + sys.executable, + "-m", + "almond_axol.zed.imu_worker", + "--serial", + str(serial), + "--out", + path, + ] + if self._worker: + cmd += ["--worker", self._worker] + try: + proc = subprocess.Popen( + cmd, + stdin=subprocess.PIPE, + stdout=subprocess.PIPE, + stderr=subprocess.DEVNULL, + text=True, + bufsize=1, + # The parent's import path: the worker (and a test's fake) + # resolve exactly as they do here. + env={ + **os.environ, + "PYTHONPATH": os.pathsep.join(p for p in sys.path if p), + }, + ) + except OSError as exc: + _logger.warning("wrist IMU: could not start the recorder: %s", exc) + continue + rec = _Recorder(side, serial, path, proc) + threading.Thread(target=rec.listen, daemon=True).start() + # Ready, or the worker gave up (no pyzed, camera missing): no + # need to sit out the whole timeout for a worker that has exited. + deadline = time.perf_counter() + _OPEN_TIMEOUT_S + while ( + not rec.ready.wait(0.1) + and proc.poll() is None + and time.perf_counter() < deadline + ): + pass + if not rec.ready.is_set(): + rec.ready.wait(0.2) # a last line racing the exit + if not rec.ready.is_set(): + reason = "; ".join(rec.errors) or "camera did not open in time" + _logger.warning( + "wrist IMU: %s camera %d unavailable (%s) — no IMU metrics", + side, + serial, + reason, + ) + rec.send("stop") + try: + proc.wait(timeout=1.0) + except subprocess.TimeoutExpired: + proc.kill() + rec.close() + continue + _logger.info("wrist IMU: recording the %s camera (%d)", side, serial) + self._recorders[side] = rec + + def flush(self, timeout: float = 2.0) -> None: + """Load the samples recorded so far without stopping — for tools that + save a run per candidate mid-session (``tune.pid``).""" + for rec in self._recorders.values(): + if rec.stopped: + continue + rec.dumped.clear() + rec.send("dump") + if rec.dumped.wait(timeout): + self._load(rec) + + def stop(self) -> None: + """Stop every recorder and load its samples (idempotent).""" + for rec in self._recorders.values(): + if rec.stopped: + continue + rec.stopped = True + rec.send("stop") + try: + rec.proc.wait(timeout=_STOP_TIMEOUT_S) + except subprocess.TimeoutExpired: + rec.proc.kill() + _logger.warning( + "wrist IMU: the %s recorder did not stop — its last samples " + "are lost", + rec.side, + ) + rec.close() + if rec.errors: + _logger.warning("wrist IMU: %s: %s", rec.side, "; ".join(rec.errors)) + self._load(rec) + if not len(rec.data["t"]): + _logger.warning( + "wrist IMU: the %s camera returned no IMU samples", rec.side + ) + if self._tmp is not None and all(r.stopped for r in self._recorders.values()): + self._tmp.cleanup() + self._tmp = None + + @staticmethod + def _load(rec: _Recorder) -> None: + try: + with np.load(rec.path) as z: + rec.data = {k: z[k] for k in ("t", "acc", "gyro")} + except (OSError, KeyError, ValueError): + if not rec.data: + rec.data = { + "t": np.empty(0), + "acc": np.empty((0, 3)), + "gyro": np.empty((0, 3)), + } + + def window( + self, side: str, t0: float, t1: float, origin: float | None = None + ) -> dict[str, np.ndarray] | None: + """``{t, acc, gyro}`` of ``side`` between perf_counter ``t0`` and ``t1``, + ``t`` relative to ``origin`` (default ``t0``) — pass the run log's own + time origin to put the IMU on its time axis; ``None`` without data.""" + rec = self._recorders.get(side) + if rec is None or not rec.data or not len(rec.data["t"]): + return None + t = rec.data["t"] + sel = (t >= t0) & (t <= t1) + if sel.sum() < 2: + return None + return { + "t": t[sel] - (t0 if origin is None else origin), + "acc": rec.data["acc"][sel], + "gyro": rec.data["gyro"][sel], + } + + def metrics(self, side: str, t0: float, t1: float) -> dict[str, float] | None: + """:func:`shake_metrics` over a window, or ``None`` without data.""" + w = self.window(side, t0, t1) + if w is None: + return None + return shake_metrics(w["t"], w["acc"], w["gyro"]) + + def run_blocks( + self, t0: float, t1: float, origin: float | None = None + ) -> tuple[dict[str, dict[str, float]], dict[str, np.ndarray]]: + """The ``imu`` metrics block (per side) and the series entries + (``imu_{side}_t/acc/gyro``, ``t`` relative to ``origin``) for a run + window — both empty without data, so callers can merge them + unconditionally.""" + metrics: dict[str, dict[str, float]] = {} + series: dict[str, np.ndarray] = {} + for side in self.sides: + w = self.window(side, t0, t1, origin) + if w is None: + continue + m = shake_metrics(w["t"], w["acc"], w["gyro"]) + if m: + metrics[side] = m + series[f"imu_{side}_t"] = w["t"].astype(np.float64) + series[f"imu_{side}_acc"] = w["acc"].astype(np.float32) + series[f"imu_{side}_gyro"] = w["gyro"].astype(np.float32) + return metrics, series + + +def format_imu(metrics: dict[str, dict[str, float]]) -> list[str]: + """One scorecard line per side of an ``imu`` metrics block.""" + return [ + f" wrist IMU ({side}): shake {m['shake_mm']:.2f} mm p2p " + f"(p90 {m['shake_mm_p90']:.2f}; vertical {m['vertical_mm']:.2f} = " + f"{LOW_BAND[0]:g}-{LOW_BAND[1]:g} Hz {m.get('low_mm', math.nan):.2f} + " + f"{HIGH_BAND[0]:g}-{HIGH_BAND[1]:g} Hz {m.get('high_mm', math.nan):.2f}), " + f"accel {m['acc_rms']:.3f} m/s², gyro {m['gyro_rms']:.2f} °/s, " + f"peak {m['peak_hz']:.1f} Hz" + for side, m in metrics.items() + ] + + +def _band_integrate(x: np.ndarray, fs: float, band: tuple[float, float]) -> np.ndarray: + """Displacement from acceleration within ``band``: ``X(f) / -(2πf)²``, + zero outside it (per column).""" + n = len(x) + spec = np.fft.rfft(x - x.mean(axis=0), axis=0) + f = np.fft.rfftfreq(n, 1.0 / fs) + gain = np.zeros_like(f) + inside = (f >= band[0]) & (f <= band[1]) + gain[inside] = -1.0 / (2.0 * math.pi * f[inside]) ** 2 + return np.fft.irfft(spec * gain[:, None], n, axis=0) + + +def shake_metrics( + t: np.ndarray, + acc: np.ndarray, + gyro: np.ndarray | None = None, + *, + band: tuple[float, float] = SHAKE_BAND, + window_s: float = 2.0, +) -> dict[str, float]: + """Score the shake in an IMU record. + + Args: + t: Sample times (s), increasing. + acc: ``(N, 3)`` linear acceleration (m/s², gravity included — it is + the band-pass's DC and gives the vertical). + gyro: ``(N, 3)`` angular rate (deg/s), optional. + band: Shake band (Hz). + window_s: Peak-to-peak window — 2 s holds two cycles of the 1 Hz + band edge. + + Returns: + ``{"shake_mm", "shake_mm_p90", "vertical_mm", "vertical_mm_p90", + "low_mm", "high_mm", "acc_rms", "gyro_rms", "peak_hz", "rate_hz", + "seconds"}`` — the windowed peak-to-peak displacement over ``band`` + (3-D: twice the largest excursion from the window's mean; vertical: + along the record's mean acceleration, i.e. gravity), the vertical + one again within :data:`LOW_BAND` and :data:`HIGH_BAND`, band + acceleration RMS (m/s²), band angular-rate RMS (deg/s), the band's + dominant frequency. Empty when the record is too short (under two + windows or 50 samples). + """ + from scipy.signal import butter, sosfiltfilt + + t = np.asarray(t, dtype=float) + acc = np.asarray(acc, dtype=float).reshape(-1, 3) + ok = np.isfinite(t) & np.all(np.isfinite(acc), axis=1) + t, acc = t[ok], acc[ok] + if len(t) < 50 or t[-1] - t[0] < 2.0 * window_s: + return {} + dt = float(np.median(np.diff(t))) + if dt <= 0: + return {} + fs = 1.0 / dt + hi = min(band[1], 0.45 * fs) + if hi <= band[0]: + return {} + grid = np.arange(t[0], t[-1], dt) + a = np.stack([np.interp(grid, t, acc[:, i]) for i in range(3)], axis=1) + up = a.mean(axis=0) + up = up / (np.linalg.norm(up) or 1.0) + sos = butter(4, [band[0], hi], btype="band", fs=fs, output="sos") + a_bp = sosfiltfilt(sos, a, axis=0) + disp = _band_integrate(a, fs, (band[0], hi)) + vert = disp @ up + low_v = _band_integrate(a, fs, (LOW_BAND[0], min(LOW_BAND[1], hi))) @ up + high_v = ( + _band_integrate(a, fs, (HIGH_BAND[0], min(HIGH_BAND[1], hi))) @ up + if hi > HIGH_BAND[0] + else np.zeros(len(grid)) + ) + w = max(2, int(round(window_s * fs))) + edge = min(w // 2, len(grid) // 4) + starts = range(edge, len(grid) - w - edge + 1, max(1, w // 2)) + p2p_3d, p2p_v, p2p_low, p2p_high = [], [], [], [] + for s in starts: + seg = disp[s : s + w] + p2p_3d.append( + 2.0 * float(np.max(np.linalg.norm(seg - seg.mean(axis=0), axis=1))) + ) + p2p_v.append(float(np.ptp(vert[s : s + w]))) + p2p_low.append(float(np.ptp(low_v[s : s + w]))) + p2p_high.append(float(np.ptp(high_v[s : s + w]))) + if not p2p_3d: + return {} + # Power summed over the axes: a magnitude would rectify each axis and + # double its frequency. + win = np.hanning(len(a_bp))[:, None] + spec = np.sum(np.abs(np.fft.rfft(a_bp * win, axis=0)) ** 2, axis=1) + freq = np.fft.rfftfreq(len(a_bp), dt) + inband = (freq >= band[0]) & (freq <= hi) + out = { + "shake_mm": 1e3 * float(np.median(p2p_3d)), + "shake_mm_p90": 1e3 * float(np.percentile(p2p_3d, 90)), + "vertical_mm": 1e3 * float(np.median(p2p_v)), + "vertical_mm_p90": 1e3 * float(np.percentile(p2p_v, 90)), + "low_mm": 1e3 * float(np.median(p2p_low)), + "high_mm": 1e3 * float(np.median(p2p_high)), + "acc_rms": float(np.sqrt(np.mean(np.sum(a_bp**2, axis=1)))), + "gyro_rms": math.nan, + "peak_hz": float(freq[inband][np.argmax(spec[inband])]) + if inband.any() + else math.nan, + "rate_hz": fs, + "seconds": float(t[-1] - t[0]), + } + if gyro is not None: + g = np.asarray(gyro, dtype=float).reshape(-1, 3)[ok] + if len(g) == len(t) and np.all(np.isfinite(g)): + gg = np.stack([np.interp(grid, t, g[:, i]) for i in range(3)], axis=1) + g_bp = sosfiltfilt(sos, gg, axis=0) + out["gyro_rms"] = float(np.sqrt(np.mean(np.sum(g_bp**2, axis=1)))) + return out diff --git a/almond_axol/utils/can_purge.py b/almond_axol/utils/can_purge.py index 8e6029e9..9db7df28 100644 --- a/almond_axol/utils/can_purge.py +++ b/almond_axol/utils/can_purge.py @@ -47,6 +47,7 @@ CAN_LEFT, CAN_MANTIS_LEFT, CAN_MANTIS_RIGHT, + CAN_RESET_SCRIPT, CAN_RIGHT, ) from .rtprio import operator_user @@ -92,10 +93,10 @@ def purge_commands() -> list[str]: """Every command line the realtime core runs to purge a poisoned queue. Two callers, one grant: the realtime core's ``purge_tx_queue`` - (``rust/axol-rt/src/safety.rs``) runs the bring-up script, falling back - to a per-interface down/up pair, and the bring-up backstop - (``almond_axol.cli.can.setup.purge_stale_tx``) runs the same script, - falling back to the full configure sequence. + (``rust/axol-rt/src/safety.rs``) runs the arm hub's USB reset script, + else the bring-up script, falling back to a per-interface down/up pair, + and the bring-up backstop (``almond_axol.cli.can.setup.purge_stale_tx``) + runs the same scripts, falling back to the full configure sequence. Imported lazily: ``cli.can.setup`` is a heavy import and this is the only thing needed from it. @@ -103,7 +104,10 @@ def purge_commands() -> list[str]: from ..cli.can.setup import _BITRATE, _TXQUEUELEN commands = [ - f"{bash} {CAN_BRINGUP_SCRIPT}" + f"{bash} {script}" + # The reset script first: the core prefers it for the arm hub, whose + # firmware keeps frames a flap of the bring-up script cannot reach. + for script in (CAN_RESET_SCRIPT, CAN_BRINGUP_SCRIPT) for bash in _program_paths("bash", ("/usr/bin/bash", "/bin/bash")) ] for ip in _program_paths("ip", ("/usr/sbin/ip", "/sbin/ip", "/usr/bin/ip")): diff --git a/almond_axol/zed/imu_worker.py b/almond_axol/zed/imu_worker.py new file mode 100644 index 00000000..dbfe599a --- /dev/null +++ b/almond_axol/zed/imu_worker.py @@ -0,0 +1,192 @@ +"""Subprocess that records one ZED X One camera's IMU. + +Run by :class:`almond_axol.tuning.wrist_imu.WristImu` as ``python -m +almond_axol.zed.imu_worker --serial N --out PATH``, in a process of its own so +a wedged ZED SDK can never stall — or crash — the tuning process streaming +motor commands. Deliberately light to import (numpy only; ``pyzed`` inside +:func:`record`): a spawn that re-imported the ``axol`` CLI took seconds of the +camera-open budget. + +Protocol, one line each way on stdin/stdout: the worker prints ``ready`` once +the camera is open, ``dumped`` after each ``dump`` request (the samples so far +written to ``PATH``), ``error `` for anything that goes wrong; ``stop`` +(or stdin closing) ends it after a final write. + +``PATH`` is an ``.npz`` of ``t`` (``time.perf_counter`` seconds — +``CLOCK_MONOTONIC``, shared across processes, the clock the tuning logs use), +``acc`` (m/s², gravity included) and ``gyro`` (deg/s). + +What the SDK (5.4.1, ``pyzed/sl.pyi``) specifies and the right wrist camera (a +ZED X One GS, serial 308393615) showed on 2026-09-23: + +- ``CameraOne.get_sensors_data(data, TIME_REFERENCE.CURRENT)`` returns the + latest sample received (the SDK's advice: poll at 800 Hz in a thread to get + them all). It needs no ``grab()`` — an open, never-grabbed camera delivers. + 800 Hz is not enough here: a 1 ms poll (~940 calls/s) phase-locked with the + SDK's update and caught only 128 of the ~200 samples a second; 0.5 ms lost + 2%; 0.2–0.3 ms caught all of them for ~5% of a core, hence ``_POLL_S``. +- The IMU is specified at 400 Hz (``sensors_configuration``) but delivers + ~200 Hz (``IMUData.effective_rate`` 202): 5 ms apart, the newest ~5 ms old. + Nyquist 100 Hz — the 3–15 Hz shake band is well inside it. +- ``get_linear_acceleration()`` is m/s² and ``get_angular_velocity()`` deg/s, + both calibrated (bias, scale, misalignment); ranges ±78.5 m/s², ±1000 deg/s. +- ``IMUData.timestamp`` is the acquisition time in UNIX nanoseconds (the + wall clock), mapped here onto ``perf_counter``. Right after ``open()`` the + first sample can be stale — 36 minutes old on the jelly robot's wrist + cameras (SDK 5.2.3) — so a sample older than ``_STALE_S`` is dropped. +- ``get_sensors_data_batch`` (every sample of the last grabbed frame) is + lossless too, but only behind a ``grab()`` loop — the 1080p/30 fps image + pipeline running for nothing — so it is not used. +""" + +from __future__ import annotations + +import argparse +import importlib +import os +import sys +import threading +import time +from typing import Any + +import numpy as np + +# Poll period (s): 0.25 ms catches every ~200 Hz sample (1 ms caught 64%). +_POLL_S = 0.00025 +# An open camera that has delivered no IMU sample for this long is reported. +_SILENT_S = 1.0 +# A sample this much older than the moment it is read is a leftover, not data. +_STALE_S = 0.5 + + +def write_samples( + path: str, + ts: list[float], + acc: list[tuple[float, float, float]], + gyro: list[tuple[float, float, float]], +) -> None: + """Write the samples atomically (tmp + rename).""" + tmp = path + ".tmp.npz" + np.savez( + tmp, + t=np.asarray(ts, dtype=np.float64), + acc=np.asarray(acc, dtype=np.float32).reshape(-1, 3), + gyro=np.asarray(gyro, dtype=np.float32).reshape(-1, 3), + ) + os.replace(tmp, path) + + +def record( + serial: int, + out_path: str, + ready: Any, + stop: Any, + dump: Any, + dumped: Any, + errors: Any, +) -> None: + """Open camera ``serial`` and record its IMU until ``stop`` is set. + + ``ready`` / ``dumped`` are set, ``stop`` / ``dump`` polled (Event-like), + ``errors.put(text)`` reports a failure. + """ + try: + import pyzed.sl as sl + except ImportError as exc: + errors.put(f"pyzed not importable ({exc}) — run `axol zed.install`") + return + zed = sl.CameraOne() + init = sl.InitParametersOne() + init.set_from_serial_number(serial) + if hasattr(init, "sdk_verbose"): + init.sdk_verbose = 0 + err = zed.open(init) + if err != sl.ERROR_CODE.SUCCESS: + errors.put(f"camera {serial} did not open: {err}") + return + ts: list[float] = [] + acc: list[tuple[float, float, float]] = [] + gyro: list[tuple[float, float, float]] = [] + try: + sensors = sl.SensorsData() + # IMU stamps are UNIX nanoseconds; map them to CLOCK_MONOTONIC. + wall_minus_perf = time.time() - time.perf_counter() + last = None + last_new = time.perf_counter() + silent_reported = False + ready.set() + while not stop.is_set(): + if ( + zed.get_sensors_data(sensors, sl.TIME_REFERENCE.CURRENT) + == sl.ERROR_CODE.SUCCESS + ): + imu = sensors.get_imu_data() + stamp = imu.timestamp.get_nanoseconds() + fresh = time.time() - stamp * 1e-9 < _STALE_S + if stamp and stamp != last and fresh: + last = stamp + last_new = time.perf_counter() + ts.append(stamp * 1e-9 - wall_minus_perf) + acc.append(tuple(imu.get_linear_acceleration())) + gyro.append(tuple(imu.get_angular_velocity())) + if not silent_reported and time.perf_counter() - last_new > _SILENT_S: + silent_reported = True + errors.put(f"camera {serial}: no IMU sample for {_SILENT_S:g} s") + if dump.is_set(): + dump.clear() + write_samples(out_path, ts, acc, gyro) + dumped.set() + time.sleep(_POLL_S) + except Exception as exc: # noqa: BLE001 - reported to the parent + errors.put(f"camera {serial}: {exc}") + finally: + zed.close() + write_samples(out_path, ts, acc, gyro) + + +class _Line: + """``set()`` prints a protocol line (``ready`` / ``dumped``).""" + + def __init__(self, word: str) -> None: + self._word = word + + def set(self) -> None: + print(self._word, flush=True) + + +class _Errors: + def put(self, text: str) -> None: + print("error " + str(text).replace("\n", " "), flush=True) + + +def main(argv: list[str] | None = None) -> None: + p = argparse.ArgumentParser(description="Record a ZED X One IMU (see module docs)") + p.add_argument("--serial", type=int, required=True) + p.add_argument("--out", required=True) + p.add_argument( + "--worker", + default=f"{__name__}:record", + help="module:function to run in place of the camera (tests)", + ) + args = p.parse_args(argv) + module, _, name = args.worker.partition(":") + worker = getattr(importlib.import_module(module), name) + stop, dump = threading.Event(), threading.Event() + + def _commands() -> None: + for line in sys.stdin: + word = line.strip() + if word == "dump": + dump.set() + elif word == "stop": + break + stop.set() + + threading.Thread(target=_commands, daemon=True).start() + worker( + args.serial, args.out, _Line("ready"), stop, dump, _Line("dumped"), _Errors() + ) + + +if __name__ == "__main__": + main() diff --git a/docs/cli/can-setup.mdx b/docs/cli/can-setup.mdx index 240d3a86..26b74fe1 100644 --- a/docs/cli/can-setup.mdx +++ b/docs/cli/can-setup.mdx @@ -46,7 +46,9 @@ The Mantis profile gets its own udev rule file, startup script, and hotplug brin The same bring-up script also recovers the bus after an **emergency stop**. Cutting motor power mid-motion leaves the in-flight CAN commands unacknowledged, so the kernel parks up to `txqueuelen` of them on the interface instead of dropping them; without intervention they replay the instant the arm is powered back on and enabled. The arm then jerks to the pose it was *commanded* as power died — not the pose it sagged to once torque was gone, which is why the position it snaps to often looks like nowhere the robot has been. The CAN layer detects the stalled bus, flaps the interfaces to drop the stale frames, and holds every send until a probe frame is acknowledged on the wire again, so the arm stays put until the bus is genuinely back online. - Flapping an interface needs root. The hosted install's `axol serve` is root and has it; a **manual** `axol serve` / `axol teleop` from the operator's shell gets it from the `sudoers.d` drop-in [`axol provision`](/cli/provision) installs for exactly the bring-up script (`almond_axol.utils.can_purge`) — without that grant the purge silently does nothing and the stale frames survive to the next session. As a backstop, every arm bring-up checks each interface's queue before enabling a motor and flaps it if anything is still parked there, refusing to enable rather than replaying it. + On the arm hub a flap is not enough. The hub's firmware keeps the frames it had already accepted from the host — up to 10 per channel, the driver's in-flight limit — through the interface reset and transmits them on the *next* open, possibly minutes later: a new session, or a `can.setup` recovery cycle (the kernel logs `Unexpected unused echo id` for each one). The purge therefore USB-resets the hub instead (`/etc/almond-axol/can/reset_adapter.sh`, written by `can.setup`: `usbreset`, or a sysfs deauthorize/reauthorize where that is missing, then the bring-up script), and `can.setup`'s own recovery cycles of a silent arm hub reset it the same way. The wheel and chest adapters keep the plain flap. Until a robot has re-run `can.setup` and `axol provision` for the script and its grant, the purge falls back to the flap — unplug and replug the hub after any `TX queue stalled` report before running anything that talks to the arms. + + Flapping an interface needs root. The hosted install's `axol serve` is root and has it; a **manual** `axol serve` / `axol teleop` from the operator's shell gets it from the `sudoers.d` drop-in [`axol provision`](/cli/provision) installs for exactly the reset and bring-up scripts (`almond_axol.utils.can_purge`) — without that grant the purge silently does nothing and the stale frames survive to the next session. As a backstop, every arm bring-up checks each interface's queue before enabling a motor and flaps it if anything is still parked there, refusing to enable rather than replaying it. A **momentary** TX-queue overflow is handled more gently. Both arm channels share one dual-channel USB adapter, so a brief stall of its USB pipe — e.g. camera traffic during data collection — can back the queue up while the bus itself is healthy and draining. Told apart from a dead bus by how long it persists, a brief overflow just drops the frame (a rate-limited warning) and the next command proceeds, so heavy USB traffic no longer crashes teleop or data collection mid-session; only an overflow that keeps persisting (nothing draining) escalates to the interface flap and probe-gated hold above. diff --git a/docs/cli/provision.mdx b/docs/cli/provision.mdx index 7f33aa4f..cd85ac0c 100644 --- a/docs/cli/provision.mdx +++ b/docs/cli/provision.mdx @@ -9,7 +9,7 @@ Installs and refreshes the pieces `uv tool install` can't manage on its own: - [`tracker.install`](/cli/tracker) — a pinned **libsurvive** build and Vive USB permissions for Mantis Lighthouse tracking. - [`zed.driver`](/cli/zed-driver) — replaces a ZED Box's outdated factory **GMSL camera driver** (`stereolabs-zedbox-duo` on the Duo, `stereolabs-zedbox-mini` on the Mini) with the release pinned for the ZED SDK. Takes effect on the next reboot (prints a notice; never reboots itself). A no-op off a factory-flashed ZED Box; warns about a Stereolabs driver package it has no pin for. - **board IMU** (`gyro.install`) — grants the `imu` group access to the carrier board's BMI088 sampling timer, [Jelly](/guides/vr-interface#jelly)'s heading-hold yaw reference, so teleop can start it without root. Self-gates on the board driver's presence. -- **CAN e-stop purge grant** — writes `/etc/sudoers.d/50-axol-can-purge` so a manual `axol serve` / `axol teleop` can flap a CAN interface without a password when motor power dies. The realtime core purges the interface's queued position commands the moment it declares the bus stalled; without this grant that escalation fails on a non-root session, the frames survive, and the arm jerks to its pre-e-stop command on the next bring-up (see [`can.setup`](/cli/can-setup)). The rule covers only the root-owned bring-up script and the named CAN interfaces, and is checked with `visudo` before it is installed. Takes effect immediately. +- **CAN e-stop purge grant** — writes `/etc/sudoers.d/50-axol-can-purge` so a manual `axol serve` / `axol teleop` can flap a CAN interface without a password when motor power dies. The realtime core purges the interface's queued position commands the moment it declares the bus stalled; without this grant that escalation fails on a non-root session, the frames survive, and the arm jerks to its pre-e-stop command on the next bring-up (see [`can.setup`](/cli/can-setup)). The rule covers only the root-owned reset and bring-up scripts (the arm hub is USB-reset, since its firmware keeps frames a flap cannot reach) and the named CAN interfaces, and is checked with `visudo` before it is installed. Takes effect immediately. - **real-time scheduling grant** — writes `/etc/security/limits.d/50-axol-rtprio.conf` so your login can run the control loop and the camera relay's capture chain `SCHED_FIFO` from a manual `axol serve` / `axol teleop` (the systemd unit already has `LimitRTPRIO`). Login shells otherwise start with an `rtprio` limit of zero, and then: - **the control loop refuses to start** — `axol teleop`, `collect-data`, `collect-dagger` and `run-policy` raise `refusing to run without real-time scheduling` rather than run a loop that lands roughly one tick in fifty 15–65 ms late (felt as the arms hitching and lunging, and masked by turning the cameras off). Set `AXOL_ALLOW_CFS_CONTROL=1` to run degraded anyway. - **the relay silently falls back to CFS** — `camera capture threads stay SCHED_OTHER` at startup, and recordings under load discard episodes on skipped exposures. diff --git a/docs/cli/teleop.mdx b/docs/cli/teleop.mdx index a079f31a..b750fd5c 100644 --- a/docs/cli/teleop.mdx +++ b/docs/cli/teleop.mdx @@ -5,7 +5,7 @@ description: "Launch a VR teleoperation session on the robot or simulator." Launches a VR teleoperation session. When started, the hostname (`.local`) and local IP address are printed — enter either of these in the VR app at [axol.almond.bot](https://axol.almond.bot) to connect. -Runs on the **real robot by default**; pass `--sim` for the browser visualizer. Hardware control always uses the required `axol-rt` realtime core, which owns CAN at 240 Hz while Python handles VR, IK, and the slow model. The full robot config is exposed via draccus, so every gain, stiffness, and gripper limit is reachable from the CLI or a config file — see [Command configuration](/cli/configuration) for the syntax. The table below covers the commonly used flags; every field of the nested `axol`, `teleop`, `kinematics`, `vr_server`, and `cart` configs is listed with its default in [Teleoperation Config Parameters](/operations/teleop-config). +Runs on the **real robot by default**; pass `--sim` for the browser visualizer. Hardware control always uses the required `axol-rt` realtime core, which owns CAN — at 240 Hz on the default impedance controller, or at 400 Hz with `--axol.controller position`, which runs every joint on its motor's own firmware position loop (see `controller` in the [robot config](/cli/configuration)) — while Python handles VR, IK, and the slow model. The full robot config is exposed via draccus, so every gain, stiffness, and gripper limit is reachable from the CLI or a config file — see [Command configuration](/cli/configuration) for the syntax. The table below covers the commonly used flags; every field of the nested `axol`, `teleop`, `kinematics`, `vr_server`, and `cart` configs is listed with its default in [Teleoperation Config Parameters](/operations/teleop-config). For a guided, step-by-step walkthrough including the VR controller layout, see the [Teleoperation guide](/operations/teleop). diff --git a/docs/cli/tune-a4.mdx b/docs/cli/tune-a4.mdx new file mode 100644 index 00000000..792ee0b2 --- /dev/null +++ b/docs/cli/tune-a4.mdx @@ -0,0 +1,58 @@ +--- +title: "tune.a4" +description: "Tune a MyActuator joint's firmware position loop (0xA4) with a sine or constant-speed triangle." +--- + +Tunes the **firmware** position loop of one MyActuator joint — the controller the realtime core hands a joint to when its `wire_mode` is `a4` (see the [config reference](/cli/configuration)). Streams a sine or a constant-speed triangle target over 0xA4 at `--rate` Hz with the firmware gains, 0xA4 speed cap and planner acceleration you choose, reads the fine 0.01° position (0x92) every cycle, scores tracking and creep smoothness, and saves the run for the diagnostics dashboard (Tuning → Firmware loop). Also available from the dashboard (`axol serve`). + +[`tune.pid`](/cli/tune-pid) tunes the MIT impedance frame, whose gains are the host's. Under 0xA4 the whole controller is the motor's own position PI → speed PI → current loop, so the knobs here are the firmware gains (`position_kp/ki/kd`, `speed_kp/ki`, `current_kp/ki`), the speed cap, and the position planner's acceleration: **0 puts the loop in direct PI tracking of the stream, and the protocol maximum 60000 makes the planner finish each 200 Hz step inside the tick — anything in between re-plans every streamed target and the joint will not follow the wave.** On the X6-P20 elbow 60000 tracked a 3 deg/s triangle to 0.02° RMS with 4 ms lag against 0.23° / 74 ms for direct tracking, so try both. The tool writes the planner **before** the mode-switch reset: on the elbow's 2025-07 firmware a 0 written into a running position loop is silently ignored (the joint holds and executes nothing), while the same 0 applied through the reset works. + +The triangle is the stick-slip probe: every pass runs at one creep speed, so the behaviour is not confined to the sine's turnarounds. Compare runs on **velocity ripple** (std of measured minus commanded velocity over the commanded speed: the MIT frame's stick-slip sits near 0.8, smooth is under 0.2), **stuck windows**, the **1–4 Hz error band**, **lag** and **>10 Hz buzz**. + +Safety, built in: + +- Gains are written to **RAM** (0x31) unless `--persist`, and the pre-run values are written back when the run ends; `--keep` leaves a winner in place. Gains are written *after* homing and the mode switch, since those reset the motor and reload ROM. +- A **buzz guard** aborts on high-frequency position motion or excess current and restores the previous gains at once. Start every sweep from the stock values in small steps: shoulder_1 at 3× stock `speed_kp` vibrated immediately. +- The joint holds position stiffly in this mode and pushes back against contact up to motor torque. Keep the workspace clear and the e-stop in reach. + +| Flag | Description | +|---|---| +| `--l` / `--r` | Arm side (required) | +| `--channel IFACE` | SocketCAN interface override | +| `--joint JOINT` | Any arm joint with a firmware position loop: the MyActuator `shoulder_1`, `shoulder_2`, `shoulder_3`, `elbow`, `wrist_1` on 0xA4, or the Damiao `wrist_2`, `wrist_3` on their position-velocity mode (required) | +| `--mode sine\|triangle` | Wave shape (default: `triangle`) | +| `--center DEG` | Centre, joint-frame degrees (default: midpoint of the safe range) | +| `--amp DEG` | Half-travel, degrees (default: 10) | +| `--freq HZ` | Sine frequency (default: 0.3) | +| `--speed DPS` | Triangle pass speed, deg/s (default: 3) | +| `--duration S` | Seconds of wave (default: 12) | +| `--rate HZ` | Command rate (default: 400, the rate the realtime core streams a4 joints at; tune at the rate you will run — the step size is the excitation, and 200 Hz put an audible staircase on the shoulder at 12 deg/s that 400 removed) | +| `--cap DPS` | 0xA4 speed cap (default: 60) | +| `--dm-acc RAD_S2` | Damiao wrists (`wrist_2`, `wrist_3`) only: the position-velocity profiler's ACC and −DEC registers for the run, restored afterwards unless `--keep` (`--persist` stores them). The Damiao loop is the same cascade with its gains in RAM registers (`position_kp`/`position_ki` = KP_APR/KI_APR, `speed_kp`/`speed_ki` = KP_ASR/KI_ASR, no kd or current gains) behind an always-on trapezoidal profiler; each 0x100 command answers with the feedback frame, so position is 16-bit and the "current" columns carry torque in Nm. The wrists were found at 2 rad/s² (~115 °/s²) | +| `--pose JOINT=DEG` | Hold another joint at this joint-frame angle during the run, repeatable, overriding the sweep's own clearance pose for that joint (same rules as [`tune.pid`](/cli/tune-pid): inside the arm's limits, shoulder_2 outboard only). A firmware loop that is well damped with the arm hanging can oscillate with it extended — right shoulder_2 did, held 10° outboard with shoulder_1 raised and the elbow bent during a shoulder_3 sweep — so tune the worst-case pose too. Every held joint is sampled round-robin during the wave (one 0x92 read per tick) and scored: drift from its hold, peak-to-peak, std and dominant frequency, printed and saved under `metrics.held` | +| `--cap-track K` | Per-command cap = `K × |commanded speed|`, floored at `--cap-floor`, never above `--cap` (default: 0 = fixed cap). With `--accel 60000` a fixed cap lets the planner burst through each 200 Hz step at the cap and idle the rest of the tick — 4× the current spread on the elbow, 68–82 Hz velocity content; `1.1`–`1.2` keeps the joint moving continuously at about the commanded speed. Ignored (with a warning) when the planner is at 0: under direct tracking the cap is a hard limit on the PI output and would only throttle the loop | +| `--cap-floor DPS` | Lowest cap `--cap-track` may set, so a stationary or reversing target still corrects (default: 1) | +| `--accel DPS/S` | Planner acceleration for the run, written to ROM before the mode-switch reset and restored afterwards unless `--keep`. `0` = direct PI tracking; `60000` = step-follow (see above); values in between will not follow a stream | +| `--position-kp`, `--position-ki`, `--position-kd`, `--speed-kp`, `--speed-ki`, `--current-kp`, `--current-ki` | Firmware gains for the run (default: leave as is). The tool prints the motor's live values at start; the dashboard tab shows them next to each field and seeds its sliders there | +| `--persist` | Write gains to ROM instead of RAM | +| `--keep` | Leave the run's gains and planner acceleration in the motor | +| `--buzz-abort DEG` | Abort past this >10 Hz position motion, degrees RMS over 0.1 s (default: 0.3; 0 off) | +| `--iq-abort A` | Abort past this reply current (default: 30; 0 off). A loaded X8 shoulder draws ~10 A just holding gravity at −55°, so keep this above the pose's static current | +| `--tf-probe PCT` | Instead of the wave: hold the joint at `--center` on **0x73** (protocol V4.4 position control with torque feed-forward) and step the feed-forward 0 / +PCT / 0 / −PCT % of rated current, 0.25 s each for 8 s. The q-axis current jump at each step — read from the reply to the first frame carrying it, before the loops react — is the current 1% buys, so it prints the motor's rated current: the `firmware.tf_rated_current_a` the realtime core scales its 0x73 feed-forward with. `5` is a gentle ~1 Nm on a shoulder. V4.4 firmware only; run with `--accel 0` | +| `--no-imu` | Skip the wrist IMU (recorded by default: the run gets an `imu` shake score — 1–15 Hz displacement p2p in mm at the gripper, see [`tune.motion`](/cli/tune-motion)) | +| `--save-run` | Persist the run artifact | +| `--label TEXT`, `--group ID` | Note and sweep id stored on the run | + +```bash +axol tune.a4 --r --joint shoulder_1 --center -35 --amp 10 --mode triangle --speed 3 --accel 0 --save-run +axol tune.a4 --r --joint shoulder_1 --accel 0 --speed-kp 0.05 --speed-ki 0.0005 --save-run --label "skp 0.05" +axol tune.a4 --r --joint shoulder_1 --mode sine --freq 0.3 --accel 0 --position-kp 0.02 --save-run +``` + + + A winning set does not have to be persisted from here. The robot config carries per-joint `firmware.*` gains (see the [config reference](/cli/configuration)); `enable()` compares them with the motor's ROM while the joint is still disabled and writes only what differs. The X8-P20 shoulders ship configured with `position_kp 0.3`, `position_kd 0.1`, `speed_kp 0.1`, `speed_ki 1e-5` from these sweeps. + + + + A joint left with planner acceleration 0 executes any stored position target at the speed cap the moment it wakes. `tune.a4` restores the stored acceleration unless you pass `--keep`; if you keep it, do not drive that joint with any other position command until you have set it back. + diff --git a/docs/cli/tune-breakaway.mdx b/docs/cli/tune-breakaway.mdx new file mode 100644 index 00000000..b52adb85 --- /dev/null +++ b/docs/cli/tune-breakaway.mdx @@ -0,0 +1,35 @@ +--- +title: "tune.breakaway" +description: "Measure a joint's static (breakaway) friction and compare it to its sliding fc." +--- + +Measures the torque at which a stationary joint first moves — its **breakaway** or static friction — in both directions, and compares it to the sliding Coulomb torque `fc` the friction model already carries. + +[`tune.friction`](/cli/tune-friction) cannot answer this: its model `fc·tanh(0.1·k·v) + fv·v` is monotonic in velocity and has no static/kinetic distinction, and its sweep bottoms out around 0.13 rad/s, above the creep speeds where a geared joint stick-slips. Stick-slip exists precisely *because* breakaway exceeds sliding friction — a joint tracking a slow target sticks until `kp·err` covers the gap, lets go, overshoots and re-sticks. On the X8-P20 shoulders that is a 2 Hz, ~0.5° stair pattern during slow extended-reach moves. This command measures the gap directly, with no velocity signal at all. + +**Method, per pose.** The arm is homed and the [sweep-safety](/cli/tune-friction) clearances applied; the test joint ramps to the pose under impedance control, then drops to `kp = 0` with a small `kd` so only the gravity feed-forward holds it. That feed-forward is trimmed until the joint stands still (a joint sits still for any trim inside its stiction band, so this converges quickly — and without it a loaded pose is unmeasurable, since a 2 % gravity-model error at 15 Nm outweighs the whole breakaway torque). An extra torque is then ramped up and down in a triangle; the first motion past `--move-deg` is the release, the extra torque at that instant is recorded, and the joint is caught under `kp` and returned to the pose. Peaks escalate over attempts and stop at the first release, so the joint never sees more torque than it took to move it. + +**What comes out.** Releases in both directions split into a symmetric half — **F_static**, the friction — and an antisymmetric half, the standing torque the trimmed feed-forward still missed. The report gives `F_static / fc`, the predicted stick-slip stair `(F_static − fc) / kp` in degrees (compare it to the stairs a slow [`tune.motion`](/cli/tune-motion) replay shows), and the ceiling for the joint's `stiction_gain` (`F_static / fc − 1`): compensation that exceeds the friction it compensates hunts around the target at rest, so start at 60–80 % of that ceiling. + +| Flag | Description | +|---|---| +| `--l` / `--r` | Arm side (required) | +| `--channel IFACE` | SocketCAN interface override for setups without the Axol hub adapter | +| `--joint JOINT` | `shoulder_1`, `shoulder_2`, `shoulder_3`, `elbow`, `wrist_1`, `wrist_2`, `wrist_3` (required) | +| `--poses DEG [DEG ...]` | Joint-frame poses (degrees, 0 = rest) to probe (default: 0). Base-collision joints are pushed 5° outboard of their boundary so a release cannot cross it | +| `--trials N` | Releases per direction (default: 3) | +| `--ramp-s S` | Seconds for one up-and-back torque ramp (default: 4) | +| `--max-torque M` | Largest ramp peak, as a multiple of the joint's `fc` (floored at 0.2 Nm for the low-friction wrists; default: 3.0) | +| `--move-deg DEG` | Motion that counts as a release — about 3 LSB of the 16-bit MIT position (default: 0.06) | +| `--kd KD` | Firmware damping during the `kp = 0` phases, so a released joint creeps rather than runs (default: 1.0) | +| `--csv PATH` | Dump every ramp sample (`t, extra_nm, pos_deg, tau_nm`) for offline plotting | + +```bash +axol tune.breakaway --r --joint shoulder_1 +axol tune.breakaway --r --joint shoulder_2 --trials 5 --csv ~/breakaway-s2.csv +axol tune.breakaway --r --joint shoulder_1 --poses -30 0 30 # load dependence +``` + + + The joint under test spends most of the run with **no position spring** — only a trimmed gravity feed-forward and light damping hold it. Keep the workspace around the arm clear and stay within reach of the e-stop, as for every tuning probe. + diff --git a/docs/cli/tune-friction.mdx b/docs/cli/tune-friction.mdx index 07d4bf20..56aca1ed 100644 --- a/docs/cli/tune-friction.mdx +++ b/docs/cli/tune-friction.mdx @@ -7,7 +7,7 @@ Identifies the four friction-model parameters for one joint via a bidirectional **Friction model:** `τ = Fc·tanh(0.1·k·v) + Fv·v + Fo` -Sweep safety is built in: shoulder_2 sweeps only its outboard side, capped at 0 (starting at rest is fine — travel past it swings into the base); wrist_2 gets the elbow raised to midpoint, which clears the gripper from the base through its **full** range; and shoulder_3 / wrist_1 sweeps hold shoulder_2 10° outboard so the arm swings clear of the chest cameras. The sweep poses are shared with [`tune.gravity`](/cli/tune-gravity), which additionally needs the test joint gravity-*loaded* — so shoulder_3 sweeps with shoulder_1 raised 90° and the elbow bent (capped ±90°), wrist_1 with the elbow bent and wrist_2 at 85° (capped ±90°), and wrist_2 adds wrist_1 at 90° to the elbow raise. The friction fit is pose-independent (the fwd/bwd half-difference cancels gravity), so sharing the loaded poses costs it nothing. +Sweep safety is built in: shoulder_2 sweeps only its outboard side, capped at 0 (starting at rest is fine — travel past it swings into the base); wrist_2 gets the elbow raised to midpoint, which clears the gripper from the base through its **full** range; and shoulder_3 / wrist_1 sweeps hold shoulder_2 10° outboard so the arm swings clear of the chest cameras. The sweep poses are shared with [`tune.gravity`](/cli/tune-gravity), which additionally needs the test joint gravity-*loaded* — so shoulder_3 sweeps with shoulder_1 raised to humerus-horizontal (+90° on the left arm, −90° on the right — the shoulder_1 frame is mirrored, and +90° is the right arm's hard stop) and the elbow bent (capped ±90°), wrist_1 with the elbow bent and wrist_2 at 85° (capped ±90°), and wrist_2 adds wrist_1 at 90° to the elbow raise. The friction fit is pose-independent (the fwd/bwd half-difference cancels gravity), so sharing the loaded poses costs it nothing. Friction is motor-specific — the shared defaults in `config.py` were measured on one reference robot. Run this per joint, per arm, on every robot you build, and pass `--save` to persist the fit to that machine's calibration file (`~/.almond/calibration.json`). `AxolConfig` loads the file on construction, so saved values apply to every operation on the robot without a code change; explicit config overrides still win over them. diff --git a/docs/cli/tune-gravity.mdx b/docs/cli/tune-gravity.mdx index 2349066e..015639db 100644 --- a/docs/cli/tune-gravity.mdx +++ b/docs/cli/tune-gravity.mdx @@ -9,7 +9,7 @@ Identifies one link's real centre of mass from a friction-cancelled torque sweep **How:** the same bidirectional constant-velocity sweep as [`tune.friction`](/cli/tune-friction) — averaging forward and backward torque at the same position cancels friction exactly, leaving `gravity(q) + Fo`. The residual against the current model is fit to a shift of this link's centre of mass, by ridge-regularized least squares straight through the MuJoCo gravity model, so the correction generalizes to every arm pose. Mass stays at CAD: gravity torque only depends on the first moment `m·c`, so a CoM shift with fixed mass covers every identifiable error. A single-joint sweep is a one-dimensional slice of pose space and can never observe all three CoM components equally — the ridge keeps weakly observed directions at their current value instead of letting them absorb torque noise with a huge lever arm, and a fitted shift beyond 60 mm is rejected as bad data. -**Loaded sweep poses:** gravity has zero moment about a vertical axis, no matter where the mass sits — so a joint whose axis hangs vertical at rest carries no CoM signal at all. Three joints are therefore swept with other joints posed to tilt and load them: `shoulder_3` runs with shoulder_1 raised 90° and the elbow bent; `wrist_1` with the elbow bent and wrist_2 rotated 85°; `wrist_2` keeps the elbow raise (base clearance) and adds wrist_1 at 90° to restore the load the raise removed. Clearance and load poses were verified against the torso collision model, are ramped proximal-first, and feed the model predictions so the fit is computed at the pose the sweep actually ran at. A sweep the model says is still unloaded is refused as unobservable rather than fit to noise. +**Loaded sweep poses:** gravity has zero moment about a vertical axis, no matter where the mass sits — so a joint whose axis hangs vertical at rest carries no CoM signal at all. Three joints are therefore swept with other joints posed to tilt and load them: `shoulder_3` runs with shoulder_1 raised to humerus-horizontal (+90° left, −90° right — the frame is mirrored and +90° is the right arm's hard stop) and the elbow bent; `wrist_1` with the elbow bent and wrist_2 rotated 85°; `wrist_2` keeps the elbow raise (base clearance) and adds wrist_1 at 90° to restore the load the raise removed. Clearance and load poses were verified against the torso collision model, are ramped proximal-first, and feed the model predictions so the fit is computed at the pose the sweep actually ran at. A sweep the model says is still unloaded is refused as unobservable rather than fit to noise. Run **distal → proximal** (`wrist_3` → … → `shoulder_1`): a proximal joint's sweep rotates every distal link with it, so on an uncalibrated arm their errors get lumped into the proximal link's CoM — exact at the sweep pose, approximate once the elbow/wrists bend away from it. The tool prints a note when distal links are still uncalibrated. diff --git a/docs/cli/tune-motion.mdx b/docs/cli/tune-motion.mdx index f9901c2f..c9b4bdea 100644 --- a/docs/cli/tune-motion.mdx +++ b/docs/cli/tune-motion.mdx @@ -7,12 +7,12 @@ Replays a [reference motion](/cli/motion-build) through the production Rust-core Every run persists a tuning-run artifact (full per-joint time series + metrics) under `~/.almond/diagnostics/tuning/` for charting and side-by-side comparison. -The arm moves to the motion's start and back to rest on collision-aware planned trajectories; a contact watchdog aborts playback if a sustained torque residual says the arm is pushing on something that isn't in the plan. +The arm moves to the motion's start and back to rest on collision-aware planned trajectories; a contact watchdog aborts playback if a sustained torque residual says the arm is pushing on something that isn't in the plan. Arrival at the start pose is checked before playback: a joint more than ~3° off is named and playback is skipped (the arm returns to rest). A `--a4` joint that fails this almost always has a stored planner acceleration that is neither 0 nor 60000 — read it with `scripts/fw_gains.py --id `; anything in between re-plans every streamed target and the joint barely moves (see [`tune.a4`](/cli/tune-a4)). | Flag | Description | |---|---| | `--motion NAME` | Committed motion name (`axol motion.list`) or a path to a motion `.npz` (required) | -| `--gain [SIDE.]JOINT.FIELD=VALUE` | Override one gain for this run, e.g. `left.elbow.kd=4.5` or `shoulder_3.kd_host=8` (no side = both arms). Fields: `kp`, `kd`, `kd_host`, `kd_host_hz`, `kd_host_q`, `j_eff`. Repeatable | +| `--gain [SIDE.]JOINT.FIELD=VALUE` | Override one gain for this run, e.g. `left.elbow.kd=4.5` or `shoulder_3.kd_host=8` (no side = both arms). Fields: `kp`, `kd`, `kd_host`, `kd_host_hz`, `kd_host_q`, `j_eff`, `stiction_gain`, `stiction_load_gain`, `stiction_err_deg`, `dither_nm`, `dither_hz`, `stribeck_gain`, `stribeck_dfs`, `stribeck_load_gain`, `stribeck_vs`, `stribeck_pole`, the friction model as `friction.fc`, `friction.k`, `friction.fv`, `friction.fo`, `friction.fl`, and the firmware position-loop gains as `firmware.position_kp`, `firmware.position_ki`, `firmware.position_kd`, `firmware.speed_kp`, `firmware.speed_ki`, `firmware.profile_acc`, the MyActuator 0xA4 planner as `firmware.planner_accel` (`0` or `60000`) with its per-tick speed-cap tracking `firmware.cap_track` (≥ 1, e.g. `1.2`) and target lead `firmware.planner_lead_ms`, the 0x73 torque feed-forward's `firmware.tf_rated_current_a` (A; MyActuator a4 joints on V4.4 firmware), , the cogging cancellation's share `cogging_gain`, and the gravity model's link inertials `mass` (kg) and `com.x` / `com.y` / `com.z` (m, the driven link's URDF frame — for trying a gravity correction before calibrating it) (firmware gains are written to the motors' ROM at enable like the config values they replace, so a run leaves them there — the planner is pinned back to `0` by the next run without the override). Repeatable | | `--stiffness S` | Stiffness-slider position in `[0, 1]` for both arms (default: 1.0, the production default — the tuned gains, where gain overrides land exactly; lower only adds compliance) | | `--ik` | Drive the run through the IK solver: each waypoint's end-effector poses (FK of the reference, with elbow hints) are re-solved to joints exactly like teleop's pose→joints loop, and the arms execute the *solver's* output — still scored against the clean reference, so IK reconstruction error and tracking error show up together. Per-solve times and the solved-vs-reference deviation are stored on the run (`ik_solve_ms_*`, `ik_dev_*`) | | `--noise MODE` | Corrupt the motion before streaming it, at the noise source's real pipeline entry point: `network` (jitter/outliers/stalls), `ik` (solver churn/jumps), `combined`, or `none` (default). Deterministic per `--seed` | @@ -21,11 +21,22 @@ The arm moves to the motion's start and back to rest on collision-aware planned | `--label TEXT` | Free-form note stored on the run artifact | | `--torque-threshold NM` | Contact watchdog threshold (default: 8.0; 0 disables) | | `--no-save-run` | Don't persist the run artifact (dry run) | +| `--a4 SIDE.JOINT` | Drive one MyActuator joint with the firmware position loop (0xA4) instead of the MIT frame for this run, e.g. `right.shoulder_1`; repeatable. No compliance, no host feed-forward, NaN torque on that joint — see `wire_mode` in the [config reference](/cli/configuration) | +| `--loop-hz HZ` | Realtime-core tick rate override for A/B runs (default follows `--controller`: 240 impedance, 400 position) — e.g. `--controller position --loop-hz 240` to separate the rate from the controller. Impedance (MIT) is commanded at 240 Hz only: with `--a4` joints inside the impedance controller the default is 480 Hz, the `--a4` joints every tick and the impedance joints on alternate ticks (exactly 240 Hz each, their host feedforward and damping stepped at that rate); with any arm joint on impedance only 240 or 480 is accepted | +| `--repeat N` | Replay the motion N times back to back in one session (default `1`; `0` = until Ctrl-C). No homing in between: each pass after the first starts with a planned move back to the start pose if the motion does not end there. Each pass is scored and saved as its own run (label suffixed `[k/N]`), a one-line-per-pass summary (worst buzz, mean jitter, worst joint) closes the session, and `--record` captures the whole session in one trace — for soak runs and catching an intermittent buzz | +| `--hold SIDE.JOINT[=DEG]` | Hold a joint steady instead of following the motion (repeatable): at the motion's own start angle, or at the given joint-frame angle (degrees, inside the joint limits; the approach goes there). It keeps its controller and gains, commanded to one pose, and is scored as a parked joint (buzz and torque chatter only). Only the approach is collision-checked — a joint frozen while the others move can bring links closer than the recording did, so watch the first pass | +| `--record PREFIX` | Flight recorder, as teleop's `--teleop.record`: the replay's measured joints to `PREFIX_meas.npz` and the core's per-tick trace (target, command, measured position, motor speed, feed-forward terms) to `PREFIX_rt.npz` | +| `--controller impedance\|position` | Which control law the core runs the arms on for this run: `impedance` (the config default) is the production MIT frame at 240 Hz with the host feed-forward; `position` puts every joint on its motor's own position loop — MyActuator 0xA4, Damiao position-velocity, the `firmware.*` gains — streamed at 400 Hz. Stiff, no host feed-forward, NaN torque on the MyActuator joints. Same motion and scoring, so the two controllers compare directly; `--a4` still adds single joints inside the impedance controller. See `controller` in the [config reference](/cli/configuration) | +| `--arms both\|left\|right` | Which arm(s) to bring up and drive (default: `both`). The other arm's bus is left untouched, so an unpowered arm or a single-arm bench does not block the run | +| `--fast-impedance SIDE.JOINT` | Run this impedance joint at 480 Hz — every tick of a 480 Hz core loop — while every other impedance joint stays at 240 Hz on alternate ticks (repeatable; e.g. `--fast-impedance right.shoulder_1 --fast-impedance right.elbow`). Sets the joint's `impedance_hz`; an experiment, the gains were tuned at 240 | +| `--impedance-hz 240\|480` | Command rate of the MyActuator impedance joints for this run: `240` (the config default, verified) or `480` — every tick of a 480 Hz core loop, the Damiao wrists staying at 240 Hz on alternate ticks. An experiment: the impedance gains, host damping and feed-forward were tuned at 240. See `impedance_hz` in the [config reference](/cli/configuration) | +| `--no-imu` | Skip the wrist IMU. By default each driven arm's wrist ZED X One IMU is recorded (in its own process; a camera that will not open just means no IMU score) and every pass gets an `imu` score — see below | | `--no-gripper` | Run on the gripperless SKU | ```bash axol tune.motion --motion reach-and-place # baseline run axol tune.motion --motion reach-and-place --gain left.elbow.kd=4.5 # A/B a gain +axol tune.motion --motion slow_osc --controller position # firmware position loops, 400 Hz axol tune.motion --motion reach-and-place --gain shoulder_3.kd_host=8 --label "s3 damp" axol tune.motion --motion reach-and-place --noise network # raw noise, no filters axol tune.motion --motion reach-and-place --noise network --filter # same noise, stack on @@ -46,4 +57,6 @@ With `--noise`, `--filter`, and/or `--ik` the run is still scored against the *c | `trq HF` | Cycle-to-cycle torque chatter (Nm RMS) | | `buzz °` / `@Hz` | Sustained ≥ 20 Hz motion — what you *hear*. Median over 0.5 s windows of the high-band RMS, so a limit cycle that buzzes through the run stands out while one-off reversal transients wash out; `@Hz` is where the loud windows agree. Healthy joints sit near 0.005°; an audible limit cycle (e.g. wrist_2 near its firmware kd clamp, ~110 Hz) reads 2–5× that | +Each pass also prints — and stores as `metrics.imu` with the raw samples as `imu_{side}_t/acc/gyro` — the **wrist IMU shake**: the gripper camera's acceleration band-passed to 1–15 Hz (above the motion, below the buzz), integrated to displacement and scored as the median (and p90) 2 s peak-to-peak excursion in mm, overall and along gravity (`vertical`, split into `low_mm` 1–3 Hz — the impedance sway — and `high_mm` 3–15 Hz), plus the band's acceleration and angular-rate RMS and its peak frequency. Unlike the joint columns it sees what the motor-side encoders cannot — gear backlash, link flex, the gripper itself — so it is the number that matches a tool tip you can see shaking. + Joints the motion never exercises (under 1° of commanded travel) show `-` in the tracking columns — a parked joint tracks meaninglessly well — but keep their row and are still scored for buzz and torque chatter, which is exactly where a hold-pose limit cycle shows up. diff --git a/docs/cli/tune-pid.mdx b/docs/cli/tune-pid.mdx index 48a9276f..b908e110 100644 --- a/docs/cli/tune-pid.mdx +++ b/docs/cli/tune-pid.mdx @@ -29,6 +29,7 @@ During every probe the other joints are held at rest by the motor firmware's own | `--hold FLOAT` | [step] Hold time per phase in seconds (default: 2.0) — also the length of the unscored settle at the center before the step, so arrival/gain-change ringing dies out before anything is measured | | `--rate FLOAT` | Command rate in Hz (default: 240.0 — the production control rate, so gains see the same host-damping transport delay teleop gives them). The loop runs open-throttle when CAN round trips exceed the cycle budget, so the report prints the rate actually achieved and warns when it saturates below the request | | `--save` | Save the best candidate's Kp/Kd to `~/.almond/calibration.json` | +| `--no-imu` | Skip the wrist IMU (recorded by default: each candidate gets an `imu` shake score — 1–15 Hz displacement p2p in mm at the gripper, see [`tune.motion`](/cli/tune-motion)) | | `--save-run` | Persist each candidate's full time series and metrics as a tuning-run artifact (`~/.almond/diagnostics/tuning/`) for charting and A/B comparison in the diagnostics UI | | `--dump-csv [PATH]` | Write per-sample `(kp, kd, t, target, actual, error)` rows to a CSV for offline plotting. Pass without a value to auto-name as `logs/pid___.csv` | diff --git a/docs/docs.json b/docs/docs.json index 14c4f5c4..33dac650 100644 --- a/docs/docs.json +++ b/docs/docs.json @@ -176,6 +176,8 @@ "pages": [ "cli/tune-pid", "cli/tune-friction", + "cli/tune-breakaway", + "cli/tune-a4", "cli/tune-gravity", "cli/tune-factory", "cli/tune-motion", diff --git a/docs/guides/diagnostics-dashboard.mdx b/docs/guides/diagnostics-dashboard.mdx index 3e10da0a..d0cadc3c 100644 --- a/docs/guides/diagnostics-dashboard.mdx +++ b/docs/guides/diagnostics-dashboard.mdx @@ -21,7 +21,7 @@ The selected profile and saved mapping drive the whole dashboard: diagnostics an ## Control loop and CAN timing -The **Control loop & CAN timing** panel measures what actually reached each arm's CAN wire. Keep the Diagnostics page open, then start `axol teleop` from the panel or a terminal. A new graph begins when PyRoKi has produced its first solution and the shared teleop run loop starts; CAN traffic from Rust bring-up and the PyRoKi compile wait is deliberately excluded. Logging stops with that run. Per-arm cards report command and feedback Hz, p95 command-period jitter, worst interval, p95 per-motor command-to-feedback latency, p95 command-send batch, p95 feedback batch, the full first-command-to-last-feedback CAN cycle, bus occupancy and remaining cycle headroom, missed 240 Hz deadlines, and lost feedback frames. Views cover loop rate, jitter, motor latency, full-arm CAN cycle, worst gaps, and misses. The rate and max-gap views include the 240 Hz / 4.17 ms references. +The **Control loop & CAN timing** panel measures what actually reached each arm's CAN wire. Keep the Diagnostics page open, then start `axol teleop` from the panel or a terminal. A new graph begins when PyRoKi has produced its first solution and the shared teleop run loop starts; CAN traffic from Rust bring-up and the PyRoKi compile wait is deliberately excluded. Logging stops with that run. Per-arm cards report command and feedback Hz, p95 command-period jitter, worst interval, p95 per-motor command-to-feedback latency, p95 command-send batch, p95 feedback batch, the full first-command-to-last-feedback CAN cycle, bus occupancy and remaining cycle headroom, missed 240 Hz deadlines, and lost feedback frames. Views cover loop rate, jitter, motor latency, full-arm CAN cycle, worst gaps, and misses. The rate and max-gap views include the loop-rate reference — 240 Hz / 4.17 ms on the impedance controller, 400 Hz / 2.5 ms on the position controller; the observer infers which is running from the measured command rate. These are passive kernel timestamps, not timing printed by the controller being tested. The observer follows the first commanded joint per arm (every teleop tick commands every joint once), pairs each command with its feedback, and adds no CAN frames of its own. **Send batch** is first-to-last arm command; **full cycle** continues through the last arm feedback. **Missed 240 Hz** counts the nominal 4.17 ms cycles represented by longer command gaps: a clean Rust run stays near 240 Hz and zero misses, while a 120 Hz loop visibly records about one missed 240 Hz cycle per command. A card only reads **240 Hz clean** when command and feedback are both within 2% of target, both p95 jitter values are at most 0.5 ms, motor response p95 is at most 3 ms, the CAN cycle retains at least 0.25 ms of p05 headroom, and neither counter reports a miss. @@ -61,7 +61,7 @@ Gain fields (kp, kd, kd_host, kd_host_hz, kd_host_q) show the selected joint's * - **Factory** — [`tune.factory`](https://docs.almond.bot/cli/tune-factory) runs friction + gravity for **all 14 joints** in one go (both arms, distal→proximal), saving every fit to this robot's calibration and uploading the document to the cloud keyed by the hub adapter serial when Supabase credentials are configured. This is the one-button factory calibration; expect a long run. To compare two runs, tick their checkboxes in the run list (same run kind — sine vs sine, motion vs motion). The view switches to an overlay: commanded plus both runs' outputs per joint (A yellow, B blue), both error traces in the lane, a verdict line naming the better run by its headline score, and an A-vs-B score table with the better value of each pair in green. Untick (or Exit compare) to go back to single-run view. -- **Recorded motion** — [`tune.motion`](https://docs.almond.bot/cli/tune-motion) replays a committed reference motion through the production Rust-core control path and scores tracking per joint. The gain table shows every joint's current config value (defaults + this robot's calibration) for each gain field; edit a cell to override it **for this run only, on both arms** — edited cells highlight, everything else runs with config. Clearing a cell drops the override and snaps it back to the config value (**reset overrides** clears them all). Tick **run as IK** to drive the run through the IK solver instead of raw joint replay: the motion's Cartesian end-effector path (FK of the reference, with elbow hints) is re-solved to joints like teleop's pose→joints loop and the arms execute the solver's output — still scored against the clean reference, so IK reconstruction error and tracking show up together in the same charts. The tab also carries the filter-stack test on real hardware: pick a **noise** source (**network** = seeded jitter/outliers/stalls injected before the pose low-pass, **IK** = solver churn/jumps injected after it, or **combined** — each at its real entry point in the pipeline, deterministic per seed) and toggle the **filter stack** (pose low-pass → EMA → trapezoid). The run is scored against the clean reference either way, and the charts overlay **commanded (clean), sent (what was actually streamed), and actual** per joint — run once with the filters off and once on to see, on the physical arm, what the stack removes and what it costs in lag. +- **Recorded motion** — [`tune.motion`](https://docs.almond.bot/cli/tune-motion) replays a committed reference motion through the production Rust-core control path and scores tracking per joint. The gain table shows every joint's current config value (defaults + this robot's calibration) for each gain field; edit a cell to override it **for this run only, on both arms** — edited cells highlight, everything else runs with config. Clearing a cell drops the override and snaps it back to the config value (**reset overrides** clears them all). The **controller** select picks the control law for the whole run: **impedance** (the production MIT frame with the host feed-forward, 240 Hz) or **position** (`--controller position`: every joint on its motor's own firmware position loop — MyActuator 0xA4, Damiao position-velocity, the gains on the Firmware-loop tab — streamed at 400 Hz). The **controller per joint** grid below it picks, inside the impedance controller, per arm and MyActuator joint, whether that joint runs on **impedance** or on its **firmware** loop (`--a4 side.joint`) for this run — e.g. shoulder_1 on the firmware loop with everything else on impedance, scored against the identical motion; a joint the config already pins to `wire_mode a4` shows as firmware and cannot be switched back. Tick **run as IK** to drive the run through the IK solver instead of raw joint replay: the motion's Cartesian end-effector path (FK of the reference, with elbow hints) is re-solved to joints like teleop's pose→joints loop and the arms execute the solver's output — still scored against the clean reference, so IK reconstruction error and tracking show up together in the same charts. The tab also carries the filter-stack test on real hardware: pick a **noise** source (**network** = seeded jitter/outliers/stalls injected before the pose low-pass, **IK** = solver churn/jumps injected after it, or **combined** — each at its real entry point in the pipeline, deterministic per seed) and toggle the **filter stack** (pose low-pass → EMA → trapezoid). The run is scored against the clean reference either way, and the charts overlay **commanded (clean), sent (what was actually streamed), and actual** per joint — run once with the filters off and once on to see, on the physical arm, what the stack removes and what it costs in lag. - **Build motion** — [`motion.build`](https://docs.almond.bot/cli/motion-build) turns a recorded session — teleoperated (`axol teleop --teleop.record`) or hand-guided in gravity comp (`axol gravity-comp --record`, also settable per run on the gravity-comp operation panel) — into a reference motion, which then shows up in the Recorded motion picker. Every build saves a **before/after run**: the raw recorded command stream vs the built motion per joint (with a built−recorded lane), so the smoothing pass and the collision projection are visually checkable — with the same zoom/fullscreen as every other chart — before the motion is replayed. - **IK** — [`diag.offline kinematics`](https://docs.almond.bot/cli/diag-offline) analyzes a teleop recording offline (no hardware, defaults to the newest recording): per-axis charts of the **EE pose set vs actual** (the world target given to the solver vs FK of what it solved) with an error lane in mm, the per-tick **solve time** trace (spikes here surface as teleop lurches), and a per-joint churn/jitter scorecard that catches a restless null space. diff --git a/docs/snippets/config/robot.mdx b/docs/snippets/config/robot.mdx index 578da421..98d72325 100644 --- a/docs/snippets/config/robot.mdx +++ b/docs/snippets/config/robot.mdx @@ -6,6 +6,8 @@ export const F = ({ children }) => {child | --{prefix}right_stiffness | `1.0` | Same, for the right arm. | | --{prefix}has_gripper | `true` | Set `false` on the gripperless model. The gripper motors are never enabled and datasets record 7 channels per arm instead of 8. | | --{prefix}max_step_rad | `0.5` | Largest joint move (rad) allowed between two consecutive commands. Bigger moves are dropped with a warning. `inf` disables. | +| --{prefix}controller | `impedance` | Which control law the realtime core runs the arms on. `impedance` is the production MIT frame at **240 Hz**: host gravity / friction / inertia feed-forward and host damping around the firmware PD, compliant. `position` hands every joint to its motor's own position loop at **400 Hz** — 0xA4 on the MyActuator joints, position-velocity on the Damiao wrists, the `firmware.*` gains below (written to ROM at enable) — where the position loop's target staircase (audible at 200 Hz) is gone. It is stiff: no compliance, no host feed-forward, and the contact watchdog is blind on the MyActuator joints (their torque telemetry reads NaN). At 400 Hz the bus cannot carry every motor every tick, so the core thins its schedule: every MyActuator command still goes out every tick, the two wrists are commanded on alternate ticks (200 Hz each), and one 0.01° a4 position read per tick rotates through the a4 joints and the gripper (~67 Hz each; between reads a joint's position is carried on the speed its command echo reports every tick). The choice is baked into each joint's `wire_mode` when the robot is constructed; under `impedance` a per-joint `wire_mode` set explicitly is kept. Impedance joints are commanded at **240 Hz only**: an arm that mixes them with firmware-loop joints (a per-joint `wire_mode a4` under `impedance`, or `tune.motion --a4`) runs its core at **480 Hz**, the firmware-loop joints every tick and each impedance joint on alternate ticks — exactly 240 Hz, with its host feed-forward and damping stepped at that rate — and any other rate with an impedance joint on the arm is refused. Recorded-motion replays take `--controller` ([`tune.motion`](/cli/tune-motion)); teleop takes `--axol.controller position`. | +| --{prefix}impedance_hz | `240.0` | Command rate of the MyActuator impedance joints: `240.0` (the verified rate) or `480.0` — every tick of a **480 Hz** core loop, their host feed-forward, damping and tracker stepped at 480, while the Damiao wrists stay at 240 Hz on alternate ticks (the gripper and any a4 position reads take one tick in eight). An experiment, never the default: the impedance gains, host damping and feed-forward were tuned at 240. With it set, any arm with an impedance joint runs its core at 480 Hz only. `tune.motion --impedance-hz 480` sets it for a run. | | --{prefix}left.gripper.torque_limit | `0.5` | Grip force — peak gripper torque (Nm). | | --{prefix}left.gripper.max_speed | `10.0` | Maximum gripper speed (rad/s). | | --{prefix}left.elbow.kp | per joint | Position stiffness of one joint, in `[0, 500]`. This is the stiffness-`1` value. | @@ -14,12 +16,36 @@ export const F = ({ children }) => {child | --{prefix}left.elbow.kd_host_hz | `null` | Centre frequency (Hz) of the band the host damping acts in. `null` tracks the arm's pose automatically. | | --{prefix}left.elbow.kd_host_q | `null` | Width of that band (bandwidth = centre ÷ q). `null` uses `0.8`. | | --{prefix}left.elbow.j_eff | per joint | Effective inertia (kg·m²) used for acceleration feed-forward. | +| --{prefix}left.elbow.stiction_gain | `0.0` | Error-sign Coulomb compensation as a fraction of `friction.fc`: pushes up to `gain × fc` toward the target while the joint is stuck and the velocity feed-forward is off, fading out once the joint slides. The lever for slow-motion stick-slip stairs on the high-ratio X8 shoulders; keep it below the breakaway/`fc` ratio [`tune.breakaway`](/cli/tune-breakaway) measures. `0` is the production law. | +| --{prefix}left.elbow.stiction_load_gain | `0.0` | Load-proportional part of that push, in Nm per Nm of the joint's gravity feed-forward (peak push = `stiction_gain × fc + stiction_load_gain × |gravity|`). Gear friction follows the transmitted torque — right shoulder_1 breaks away at 0.66 Nm at rest but at 2–3 Nm with the arm extended — so a constant push either hunts at rest or does nothing under load. | +| --{prefix}left.elbow.stiction_err_deg | `0.1` | Position error (degrees) at which that term saturates. The push fades on *measured* velocity (gone above ~0.05 rad/s), so it never drives the slip. | +| --{prefix}left.elbow.dither_nm | `0.0` | Peak amplitude (Nm) of a sinusoidal torque dither on the feed-forward, `0` off. Keeps a geared joint's meshes sliding so its velocity-weakening friction (the X8-P20 shoulders: 2.1 Nm sliding at 0.05 rad/s, 0.7 Nm at 0.2 rad/s) cannot re-stick between cycles. Start at 1–2 Nm on a shoulder; it is audible. | +| --{prefix}left.elbow.dither_hz | `60.0` | Dither frequency: above the arm's structural modes (~35 Hz), below the core's 120 Hz Nyquist. | +| --{prefix}left.elbow.stribeck_gain | `0.0` | Friction cancellation on **measured** velocity, as a fraction of the measured static-minus-sliding excess: `gain × (stribeck_dfs + stribeck_load_gain × |gravity|) × exp(−(v/stribeck_vs)²) × tanh(v/0.02)`. The one feed-forward that acts on the velocity-weakening slope behind the X8 shoulders' 2 Hz stick-slip (a command-driven term cannot). Zero at rest. Sweep 0.5 → 0.9 and stop when the arm's 3 Hz mode grows. | +| --{prefix}left.elbow.stribeck_dfs | `0.3` | Static-minus-sliding friction excess (Nm) at zero gravity load. | +| --{prefix}left.elbow.stribeck_load_gain | `0.1` | Growth of that excess per Nm of gravity feed-forward (right shoulder_1: ~1.5 Nm under 12 Nm). | +| --{prefix}left.elbow.stribeck_vs | `0.1` | Speed (rad/s) at which the excess has fallen to 1/e. | +| --{prefix}left.elbow.stribeck_pole | `20.0` | Low-pass pole (rad/s) of the measured velocity the term follows. 20 is smooth but ~40° behind a 2.6 Hz surge; 40–80 follows it at the cost of encoder-step noise in the torque. | +| --{prefix}left.elbow.wire_mode | `mit` | Frame the realtime core commands the joint with while tracking. `mit` is the impedance frame (production). `a4` hands a **MyActuator** joint to the firmware's own position loop (0xA4, speed-capped at the tracker limit) with a paired 0x92 read for 0.01° position — the candidate for creeping through X8-P20 stick-slip. The joint is on 0xA4 from its first frame, holds included: the X6-P20's 2025-07 firmware ignores 0xA4 after an MIT frame until the motor is reset (the 2026 X8-P20 firmware switches freely). `pv` is the same for a **Damiao** wrist: its position-velocity mode (0x100+ID; the core sets control-mode register 2 at bring-up and switches it back to MIT for limp / gravity comp), gains in the KP_APR/KP_ASR registers, ramps in ACC/DEC. Costs of either: no compliance, no host feed-forward, and on a4 NaN torque telemetry (the contact watchdog is blind on that joint; a pv wrist keeps its torque channel). The gripper, gravity comp and the limp fallback always use MIT — so on the older firmware an a4 joint that has been hand-guided needs a re-enable before it tracks again. An a4 joint's stored planner acceleration must be 0 or 60000 (see [`tune.a4`](/cli/tune-a4)); loop gains come from `firmware.*` below. `controller` `position` (above) sets every joint's position wire mode at once; this field is the per-joint override. | | --{prefix}left.elbow.mass | per joint | Mass (kg) of the link, used for gravity compensation. Tune if an arm sags or pushes back in [gravity comp](/operations/gravity-comp). | | --{prefix}left.elbow.com | per joint | Centre of mass `[x, y, z]` (m) of the link. | | --{prefix}left.elbow.friction.fc | per motor | Coulomb friction (Nm). Measured per motor with [`tune.friction`](/cli/tune-friction). | | --{prefix}left.elbow.friction.k | per motor | Friction curve sharpness. | | --{prefix}left.elbow.friction.fv | per motor | Viscous friction (Nm·s/rad). | | --{prefix}left.elbow.friction.fo | per motor | Constant torque offset (Nm). | +| --{prefix}left.elbow.friction.fl | `0.0` | Load-proportional Coulomb friction, Nm per Nm of gravity feed-forward: the effective `fc` is `fc + fl × |gravity|`. Planetary gear friction grows with the torque it carries (right shoulder_1: ~0.6 Nm sliding at rest, ~1.5 Nm under 12 Nm), so a constant `fc` over-compensates at rest and under-compensates at reach. `0` keeps the constant model. | +| --{prefix}left.elbow.firmware.position_kp | `null` (shoulder_1 `1.0`, elbow `1.4`; the impedance joints their stock values: shoulder_2 `0.008`, shoulder_3 and wrist_1 `0.06`, Damiao wrists `54`) | Firmware position-loop proportional gain (MyActuator 0x30 index 0x07, Damiao register KP_APR), written to the motor's ROM at enable when set (`null` leaves the motor's stored value). These five gains are the motor's own position PI → speed PI controller and only act under `wire_mode` `a4`; identify them with [`tune.a4`](/cli/tune-a4). Stock 0.008 on the X8-P20 shoulders stick-slips at creep speed; the stairs are gone by 0.2 and, with the 400 Hz stream the core runs, tracking keeps improving to 1.0 (0.17° / 12 ms at 12 deg/s) with 1.4 still clean. | +| --{prefix}left.elbow.firmware.position_ki | `null` | Firmware position-loop integral gain. Leave at the stock `0`: on top of the speed integrator it hunts around the target. | +| --{prefix}left.elbow.firmware.position_kd | `null` (shoulders and elbow `0.1`, shoulder_3 and wrist_1 `0.5`) | Firmware position-loop derivative gain (protocol V4.2+). The motor stores it, but the 0xA4 loop on the X8-P20 measured inert to it. | +| --{prefix}left.elbow.firmware.speed_kp | `null` (shoulder_1 `0.07`, elbow `0.05`; stock on the impedance joints: shoulder_2 `0.03`, shoulder_3 and wrist_1 `0.01`) | Firmware speed-loop proportional gain. Not a damper in practice — it did nothing for the loop's 5 Hz reversal mode — but the knob on the speed loop's own resonance (~100 Hz on the X8-P20, ~135 Hz on the X6-P20): the current tone grows with it until the loop goes unstable (0.2 on the shoulder, 0.1 at position_kp 1.5 on the elbow). Lower is quieter at no tracking cost. | +| --{prefix}left.elbow.firmware.profile_acc | `null` (Damiao wrists `2`, the stock ramp; `50` was the position-controller setting) | **Damiao wrists only:** the position-velocity profiler's ramp (rad/s²), written to ACC and, negated, DEC at enable. Every streamed target is approached along a trapezoid under this acceleration, so it caps how fast the wrist follows: the stock `2` (115 deg/s²) cannot keep up with the 200 Hz stream — the loop hunts at ~5 Hz — and would take half a second to reach teleop speed. `50` sits above the core tracker's 33 rad/s² limit while still rounding each 5 ms step; `50` and `200` scored alike in `tune.a4`. | +| --{prefix}left.elbow.firmware.speed_ki | `null` (shoulder_1 and elbow `1e-05`, shoulder_2 the stock `1e-4`; shoulder_3 / wrist_1 unset) | Firmware speed-loop integral gain. Lower than the stock `1e-4` is smoother on a geared joint: the integrator winds up while the joint is stuck and dumps it at release. | +| --{prefix}left.elbow.firmware.planner_accel | `null` (every MyActuator joint `0`) | **MyActuator only:** the 0xA4 position planner's acceleration and deceleration (dps/s), written at enable. Only `0` (direct PI tracking of each target) and `60000` (the planner finishing each step within the tick) follow a stream; anything between re-plans every target and the joint barely moves, and is refused. On the X6-P20 elbow `tune.a4` tracked a 3 deg/s triangle to 0.02° RMS with 4 ms lag at `60000`, against 0.23° / 74 ms direct. Pinned to `0` so a planner left on by a test run does not carry into the next session. `60000` wants `cap_track`. | +| --{prefix}left.elbow.firmware.cap_track | `null` | **0xA4 joints with the planner on:** each tick's speed cap as this multiple of the commanded speed (floor 1 dps, ceiling the tracker limit) instead of the fixed tracker limit — at a fixed cap the planner bursts through each step and idles the rest of the tick (4× the current spread on the elbow); `1.1`–`1.2` moved it continuously. Leave unset under direct tracking, where the cap is a hard limit on the PI output. Carried to the realtime core, not written to the motor. | +| --{prefix}left.elbow.firmware.tf_rated_current_a | `null` | **MyActuator a4 joints:** the motor's rated current (A — datasheet, or [`tune.a4 --tf-probe`](/cli/tune-a4)). Set, the realtime core sends the joint's position command as **0x73** (protocol V4.4 position control with torque feed-forward) carrying gravity, inertia and the cogging cancellation below, scaled by the joint's torque constant into the firmware's int8 1%-of-rated-current unit and faded in over a second. Only on firmware that implements 0x73 (VersionDate 2026042402 or later — the X8-P20 shoulders, not the X6-P20 elbow's 2025070202; elsewhere the joint stays on plain 0xA4 and the core logs why), and only in direct tracking (`planner_accel` `0`). Not written to the motor. | +| --{prefix}left.elbow.cogging | `null` (from calibration) | The joint's position-periodic torque (cogging / gear mesh) as a Fourier series in the joint angle — `period_deg` and `(k, a, b)` harmonics — cancelled by feed-forward on tracked ticks: added to the MIT `t_ff` on an impedance joint, carried by 0x73 on an a4 joint with `firmware.tf_rated_current_a` (a plain-0xA4 joint takes no feed-forward). Evaluated in the core at the measured angle. Fit it from a slow sweep with `axol tune.friction --raw-csv` then `scripts/cogging_map.py --fit --save`, which stores it in the calibration file. On the right shoulder_1 a 1.81° ripple (~0.5 Nm) put ~70% of the tool tip's slow-motion shake at 1–6 Hz. | +| --{prefix}left.elbow.cogging_gain | `1.0` | Share of `cogging` fed forward, for A/B runs (`0` off; `tune.motion --gain shoulder_1.cogging_gain=0.5`). | +| --{prefix}left.elbow.impedance_hz | `null` | This joint's impedance command rate: `480.0` commands it every tick of a 480 Hz core loop (its host feed-forward, damping and tracker stepped at 480), `240.0` keeps it on the verified 240 Hz lane, `null` follows `impedance_hz` above. Any joint at 480 runs the core at 480 Hz, the other impedance joints on alternate ticks. `tune.motion --fast-impedance right.shoulder_1` sets it for a run. | Per-arm and per-joint flags are shown for `left` / `elbow`. Swap in `right`, or any joint: `shoulder_1`, `shoulder_2`, `shoulder_3`, `elbow`, `wrist_1`, `wrist_2`, `wrist_3`. @@ -37,7 +63,7 @@ Per-arm and per-joint flags are shown for `left` / `elbow`. Swap in `right`, or | `shoulder_3` | 180 | 5.0 | 0 | 0.25 | 3.75 | 45 | 1.0 | | `elbow` | 130 | 5.0 | 0 | 0.6 | 0.25 | 40 | 2.22 | | `wrist_1` | 180 | 1.7 | 0 | 0 | 0.25 | 30 | 0.69 | - | `wrist_2` | 130 | 3.5 | 0 | 0 | 0.65 | 25 | 1.5 | + | `wrist_2` | 130 | 2.25 | 0 | 0 | 0.65 | 25 | 1.5 | | `wrist_3` | 130 | 2.0 | 0 | 0 | 0.75 | 25 | 0.9 | diff --git a/rust/axol-rt/README.md b/rust/axol-rt/README.md index 426284cc..99ec2999 100644 --- a/rust/axol-rt/README.md +++ b/rust/axol-rt/README.md @@ -242,7 +242,7 @@ AXOL_RT_TRACE=/tmp/axol-run axol teleop Each 240 Hz joint row includes the streamed target, wire position/velocity, measured position/velocity/torque, filter states, and separate gravity, -friction, inertia, and host-damping torque contributions. The bus threads +friction, inertia, host-damping, and stiction torque contributions. The bus threads enqueue fixed-size rows into bounded channels; background threads format and write them, and the regular five-second status line reports any trace drops. diff --git a/rust/axol-rt/src/bringup.rs b/rust/axol-rt/src/bringup.rs index 3eabed8d..7836f845 100644 --- a/rust/axol-rt/src/bringup.rs +++ b/rust/axol-rt/src/bringup.rs @@ -5,6 +5,7 @@ use std::io; use std::time::Duration; use crate::can::CanSock; +use crate::filter::CogTerm; use crate::proto; use crate::safety::purge_tx_queue; use crate::txn; @@ -40,6 +41,54 @@ pub struct MotorSpec { pub k: f64, pub fv: f64, pub fo: f64, + /// Error-sign stiction compensation (`filter::stiction`): gain as a + /// fraction of `fc`, and the error (rad) it saturates at. Zero gain is + /// the production law; zero for the gripper. + pub stiction_gain: f64, + pub stiction_err: f64, + /// Load-proportional stiction push, Nm per Nm of gravity feedforward. + pub stiction_load_gain: f64, + /// Torque dither (`filter::dither_step`): peak Nm (0 off) and frequency. + pub dither_nm: f64, + pub dither_hz: f64, + /// Command frame for tracked ticks (MyActuator joints only). + pub wire: WireMode, + /// Stribeck cancellation on measured velocity (`filter::stribeck_excess`): + /// gain, zero-load excess (Nm), excess per Nm of gravity, 1/e speed. + pub stribeck_gain: f64, + pub stribeck_dfs: f64, + pub stribeck_load_gain: f64, + pub stribeck_vs: f64, + /// Load-proportional Coulomb friction, Nm per Nm of gravity feedforward: + /// the tracked-mode friction term uses `fc + fl·|t_ff|`. + pub fl: f64, + /// Low-pass pole (rad/s) of the measured velocity the Stribeck term + /// follows; `<= 0` falls back to the control derivative pole. + pub stribeck_pole: f64, + /// 0xA4 joints with the firmware planner on (planner acceleration + /// 60000, written by the Python side at enable): each tick's speed cap + /// is this multiple of the commanded speed (floor `A4_CAP_FLOOR_DPS`), + /// so the planner moves continuously instead of bursting through each + /// step at a fixed cap. `<= 0` keeps the fixed cap (direct tracking). + pub cap_track: f64, + /// 0xA4 target lead (s): each command's target is sent this far ahead + /// along the tracker velocity, so the planner cruises through its step + /// instead of reaching the target and stopping for the rest of it. + pub lead_s: f64, + /// 0x73 torque feedforward scale for an a4 joint: output-shaft Nm per 1% + /// of the motor's rated current (the Python side's `kt × rated A / 100`). + /// `> 0` sends the position command as 0x73 carrying the host + /// feedforward, where the firmware supports it (protocol V4.4, + /// `proto::MA_FW_V44`); `0` keeps plain 0xA4. + pub tf_nm_per_pct: f64, + /// This joint's impedance command rate (Hz): `FAST_IMPEDANCE_HZ` (480) to + /// command it every tick of a 480 Hz loop, 240 for the half-rate lane, 0 + /// to follow the config-wide `impedance_hz`. Only meaningful on MIT. + pub mit_hz: f64, + /// Position-periodic torque to cancel (`filter::cogging`), motor frame, + /// already scaled by the joint's gain. Empty = none. Arrives on the + /// second configure, after the Python side has resolved joint offsets. + pub cogging: Vec, } #[derive(Clone, Copy, PartialEq)] @@ -48,6 +97,52 @@ pub enum Vendor { Damiao, } +/// Which frame a MyActuator arm joint is commanded with in tracked mode. +/// Damiao joints, the gripper, and every limp / gravity-comp tick (kp = 0) +/// use MIT regardless. An a4 joint's *holds* (bring-up, stalled stream) are +/// 0xA4 too: the X6-P20's 2025070202 firmware ignores 0xA4 after an MIT +/// frame until reset, so an a4 joint must see the position frame from its +/// first tick (see `serve::a4_wire`). +#[derive(Clone, Copy, PartialEq, Debug)] +pub enum WireMode { + /// The 0x400 impedance frame: the production control law. + Mit, + /// 0xA4 absolute position closed-loop: the firmware's own position PI + /// (and speed PI beneath it, at its kHz rate) tracks the streamed target + /// under a speed cap. No host feedforward reaches the motor; gravity and + /// friction are the firmware integrator's job. Paired with a 0x92 read + /// per tick for 0.01° position; the reply's torque channel is iq in + /// amps, so measured torque is reported as NaN on these joints. + A4, + /// Damiao position-velocity mode (0x100 + id, control-mode register 2): + /// the wrist firmware's own position → speed cascade (KP_APR/KP_ASR + /// registers, ACC/DEC ramps) tracks the streamed target under a speed + /// cap. The feedback frame is the MIT one, so position, velocity and + /// torque all come back with each command. Like `A4`, no host + /// feedforward reaches the motor. + Pv, +} + +impl WireMode { + pub fn parse(token: &str) -> Option { + match token { + "mit" => Some(Self::Mit), + "a4" => Some(Self::A4), + "pv" => Some(Self::Pv), + _ => None, + } + } + + /// The Damiao control-mode register value a wrist must be in for this + /// wire's command frame to be acted on. + pub fn dm_mode(self) -> u32 { + match self { + Self::Pv => proto::DM_MODE_POS_VEL, + Self::Mit | Self::A4 => proto::DM_MODE_MIT, + } + } +} + /// A motor that passed bring-up prep: identified, fault-free, ranges known. #[derive(Clone)] pub struct ReadyMotor { @@ -73,6 +168,41 @@ pub struct ReadyMotor { pub k: f64, pub fv: f64, pub fo: f64, + pub stiction_gain: f64, + pub stiction_err: f64, + pub stiction_load_gain: f64, + pub dither_nm: f64, + pub dither_hz: f64, + pub wire: WireMode, + pub stribeck_gain: f64, + pub stribeck_dfs: f64, + pub stribeck_load_gain: f64, + pub stribeck_vs: f64, + pub fl: f64, + /// See `MotorSpec::cap_track`. + pub cap_track: f64, + /// See `MotorSpec::lead_s`. + pub lead_s: f64, + /// The firmware VersionDate read at prep (MyActuator only). + pub fw_version: Option, + /// See `MotorSpec::mit_hz`. + pub mit_hz: f64, + /// `MotorSpec::tf_nm_per_pct` where the firmware takes 0x73, else 0 — + /// the bus loop sends 0x73 exactly when this is positive. + pub tf_nm_per_pct: f64, + /// See `MotorSpec::cogging`. + pub cogging: Vec, +} + +/// The 0x73 scale a joint actually runs: its configured one on firmware that +/// implements 0x73 (protocol V4.4, VersionDate `proto::MA_FW_V44` or later), +/// 0 otherwise (older firmware, an unread version, or none configured). +pub fn tf_scale(spec_scale: f64, version: Option) -> f64 { + if spec_scale > 0.0 && version.is_some_and(|v| v >= proto::MA_FW_V44) { + spec_scale + } else { + 0.0 + } } /// Status-probe attempts before a silent motor fails the bring-up. @@ -211,20 +341,56 @@ pub fn prepare(sock: &CanSock, iface: &str, specs: &[MotorSpec]) -> io::Result= 6) { let id = spec.motor_id as u16; let mode = read_dm_register(sock, id, proto::DM_REG_CTRL_MODE)?; - // Wrists run MIT (1); the gripper must already be in POSITION_FORCE - // (4), set by the Python side's calibration flow before arming. - let expected = if spec.gripper { 4.0 } else { 1.0 }; - if mode != expected { - return Err(err(format!( - "{} (0x{id:02X}): control mode {mode} (expected {expected}) — not enabling", - spec.joint - ))); + if spec.gripper { + // The gripper must already be in POSITION_FORCE (4), set by the + // Python side's calibration flow before arming. + let expected = proto::DM_MODE_POS_FORCE as f64; + if mode != expected { + return Err(err(format!( + "{} (0x{id:02X}): control mode {mode} (expected {expected}) — not enabling", + spec.joint + ))); + } + } else { + // A wrist runs in the mode its wire wants — MIT (1) or, for + // `wire_mode pv`, position-velocity (2). Put it there (RAM + // write, effective at once) rather than refusing: a wrist left + // in the other mode by the previous session is the normal case + // when the controller choice changes between runs. + let wanted = spec.wire.dm_mode(); + if mode != wanted as f64 { + write_dm_register(sock, id, proto::DM_REG_CTRL_MODE, wanted.to_le_bytes())?; + let now = read_dm_register(sock, id, proto::DM_REG_CTRL_MODE)?; + if now != wanted as f64 { + return Err(err(format!( + "{} (0x{id:02X}): control mode {now} after asking for {wanted} — not enabling", + spec.joint + ))); + } + } } let p_max = read_dm_register(sock, id, proto::DM_REG_PMAX)?; let v_max = read_dm_register(sock, id, proto::DM_REG_VMAX)?; @@ -260,6 +426,23 @@ pub fn prepare(sock: &CanSock, iface: &str, specs: &[MotorSpec]) -> io::Result io::Result io::Result<()> { + sock.send( + proto::DM_REG_ARB, + &proto::dm_write_register(motor_id, rid, value), + )?; + std::thread::sleep(Duration::from_millis(5)); + Ok(()) +} + /// Enable every cold motor. Motors found holding by [`prepare`] are left /// exactly as they are (no brake release / enable frame), and are excluded /// from the rollback: a failed cold bring-up must never drop a pre-existing @@ -367,3 +562,21 @@ fn disable_inner(sock: &CanSock, motors: &[ReadyMotor]) -> bool { } complete } + +#[cfg(test)] +mod tests { + use super::*; + + /// 0x73 only on protocol V4.4 firmware: the X8-P20 shoulders' 2026042402 + /// takes it, the X6-P20 elbow's 2025070202 does not (it falls back to + /// plain 0xA4), and an unread version or an unset scale never sends it. + #[test] + fn tf_runs_only_on_v44_firmware_with_a_scale() { + assert_eq!(tf_scale(0.2, Some(2026042402)), 0.2); + assert_eq!(tf_scale(0.2, Some(2026090101)), 0.2); + assert_eq!(tf_scale(0.2, Some(2025070202)), 0.0); + assert_eq!(tf_scale(0.2, None), 0.0); + assert_eq!(tf_scale(0.0, Some(2026042402)), 0.0); + assert_eq!(tf_scale(-1.0, Some(2026042402)), 0.0); + } +} diff --git a/rust/axol-rt/src/filter.rs b/rust/axol-rt/src/filter.rs index c7cc0094..6a2aef4c 100644 --- a/rust/axol-rt/src/filter.rs +++ b/rust/axol-rt/src/filter.rs @@ -105,7 +105,118 @@ impl BandPass { pub const FRICTION_FF_K_MAX: f64 = 100.0; pub fn friction(v: f64, fc: f64, k: f64, fv: f64, fo: f64) -> f64 { - fc * (0.1 * k.min(FRICTION_FF_K_MAX) * v).tanh() + fv * v + fo + fc * coulomb_unit(v, k) + fv * v + fo +} + +/// Saturation of the Coulomb feedforward in `[-1, 1]` — `coulomb_unit` in +/// `almond_axol.robot.control`, with the same `FRICTION_FF_K_MAX` cap. +pub fn coulomb_unit(v: f64, k: f64) -> f64 { + (0.1 * k.min(FRICTION_FF_K_MAX) * v).tanh() +} + +/// Peak stiction push (Nm) — `stiction_amplitude` in +/// `almond_axol.robot.control`: `gain·fc + load_gain·|gravity|`. Gear +/// friction follows the transmitted torque (right shoulder_1 broke away at +/// 0.66 Nm at rest, 2-3.3 Nm under 10-15 Nm of gravity), so the push tracks +/// the gravity feedforward the joint carries this tick. +pub fn stiction_amplitude(fc: f64, gain: f64, load_gain: f64, gravity: f64) -> f64 { + gain * fc + load_gain * gravity.abs() +} + +/// Measured speed (rad/s) the stiction push fades over — +/// `STICTION_FADE_VEL` in `almond_axol.robot.control` (~1.4 LSB of the +/// motor-reported velocity). +pub const STICTION_FADE_VEL: f64 = 0.03; + +/// Error-sign Coulomb compensation — `stiction_compensation` in +/// `almond_axol.robot.control`: +/// `amp·tanh(err/err_scale)·(1 − tanh(|v_meas|/STICTION_FADE_VEL))`. Pushes +/// toward the target (`err = q_des − q_meas`) while the joint is stuck, and +/// is gone as soon as the joint measurably slides — it must never drive the +/// slip phase (fading on the *commanded* velocity did, and turned the +/// stairs into a 2 Hz limit cycle). `amp == 0` is exactly zero. +pub fn stiction(err: f64, v_meas: f64, amp: f64, err_scale: f64) -> f64 { + if amp == 0.0 { + return 0.0; + } + let fade = 1.0 - (v_meas.abs() / STICTION_FADE_VEL).tanh(); + amp * (err / err_scale.max(1e-9)).tanh() * fade +} + +/// Speed (rad/s) over which the Stribeck term passes through zero — +/// `STRIBECK_V0` in `almond_axol.robot.control`. +pub const STRIBECK_V0: f64 = 0.02; + +/// Excess of low-speed over sliding friction this tick, Nm — +/// `stribeck_amplitude` in `almond_axol.robot.control`: +/// `gain·(dfs + load_gain·|gravity|)`. +pub fn stribeck_amplitude(gain: f64, dfs: f64, load_gain: f64, gravity: f64) -> f64 { + gain * (dfs + load_gain * gravity.abs()) +} + +/// Friction cancellation on *measured* velocity — `stribeck_excess` in +/// `almond_axol.robot.control`: `amp·exp(−(v/v_s)²)·tanh(v/STRIBECK_V0)`. +/// Follows the measured velocity with the measured friction curve's shape, +/// so a joint that speeds up sees the feedforward fall by what the real +/// friction falls and the velocity-weakening slope (negative damping — the +/// engine of the 2 Hz stick-slip) is flattened. Zero at rest. +pub fn stribeck_excess(v_meas: f64, amp: f64, v_s: f64) -> f64 { + if amp == 0.0 || v_s <= 0.0 { + return 0.0; + } + amp * (-(v_meas / v_s).powi(2)).exp() * (v_meas / STRIBECK_V0).tanh() +} + +/// Per-slot phase offset of the torque dither, the golden angle π(3 − √5) — +/// `DITHER_PHASE_STAGGER` in `almond_axol.robot.control`. +pub const DITHER_PHASE_STAGGER: f64 = 2.399_963_229_728_653; + +/// Advance a torque-dither oscillator one step — `dither_step` in +/// `almond_axol.robot.control`. Returns the torque; `phase` is advanced in +/// place. `nm == 0` is exactly zero and leaves the phase alone. +pub fn dither_step(phase: &mut f64, nm: f64, hz: f64, dt: f64) -> f64 { + if nm == 0.0 || hz <= 0.0 { + return 0.0; + } + *phase = (*phase + 2.0 * std::f64::consts::PI * hz * dt) % (2.0 * std::f64::consts::PI); + nm * phase.sin() +} + +/// One harmonic of a joint's position-periodic torque (cogging / gear mesh) +/// in the motor frame: `a·cos(w·q) + b·sin(w·q)` Nm, `w` in rad⁻¹ (2πk over +/// the period). The Python side fits the joint-frame series +/// (`almond_axol.tuning.cogging`), shifts it by the joint offset and scales +/// it by the joint's `cogging_gain` before it reaches the core, so the core +/// only evaluates it. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct CogTerm { + pub w: f64, + pub a: f64, + pub b: f64, +} + +/// The torque to add to cancel a joint's measured position-periodic torque +/// at motor-frame position `q` — the "osc cancellation" feedforward. The +/// right shoulder_1's 1.81° / 0.905° ripple is what it was built for: in slow +/// motion (2–5 deg/s) those bumps land at 1–6 Hz and were ~70% of the tool +/// tip's vertical shake (2026-09-23). Empty series → 0. +pub fn cogging(terms: &[CogTerm], q: f64) -> f64 { + terms + .iter() + .map(|t| t.a * (t.w * q).cos() + t.b * (t.w * q).sin()) + .sum() +} + +/// Seconds the 0x73 torque feedforward takes to fade in when a joint starts +/// taking it (and after every stretch without it). The firmware's speed +/// integrator was carrying gravity before; a step of the whole gravity +/// torque would kick the joint until the integrator unwound. Ramping it in +/// hands the load over gradually. +pub const TF_RAMP_S: f64 = 1.0; + +/// Advance a 0x73 feedforward fade-in: `ramp` rises by `dt / TF_RAMP_S` to 1. +pub fn tf_ramp_step(ramp: f64, dt: f64) -> f64 { + (ramp + dt.max(0.0) / TF_RAMP_S).min(1.0) } /// Velocity/acceleration-limited target tracker — the per-joint @@ -577,6 +688,143 @@ mod tests { /// Golden values from `almond_axol.robot.control.compute_friction` /// with fc=0.6, k=250 (above the cap), fv=0.15, fo=0.02. + /// Reference vectors from `almond_axol.robot.control.stribeck_excess` + /// (amp = 1, v_s = 0.1): `(v_meas, want)`. + #[test] + fn stribeck_matches_python() { + let golden = [ + (-0.3, -0.00012340980408665668), + (-0.1, -0.36784603928630505), + (-0.05, -0.7683759879897785), + (-0.02, -0.7317316219624262), + (0.0, 0.0), + (0.01, 0.4575190147179108), + (0.02, 0.7317316219624262), + (0.05, 0.7683759879897785), + (0.1, 0.36784603928630505), + (0.2, 0.018315638813231488), + ]; + for (v, want) in golden { + let got = stribeck_excess(v, 1.0, 0.1); + assert!( + (got - want).abs() < 1e-12, + "stribeck({v}): got {got:e}, want {want:e}" + ); + } + assert_eq!(stribeck_excess(0.05, 0.0, 0.1), 0.0); + assert!((stribeck_amplitude(1.0, 0.3, 0.1, -12.0) - 1.5).abs() < 1e-12); + assert_eq!(stribeck_amplitude(0.0, 0.3, 0.1, 12.0), 0.0); + } + + /// The cogging series is a plain Fourier sum in the motor frame: zero + /// with no terms, and each harmonic's cos/sin coefficient read back at + /// the quarter points of its period. + #[test] + fn cogging_sums_its_harmonics() { + assert_eq!(cogging(&[], 1.0), 0.0); + let period = 1.81_f64.to_radians(); + let terms = [ + CogTerm { + w: 2.0 * std::f64::consts::PI / period, + a: 0.3, + b: -0.1, + }, + CogTerm { + w: 4.0 * std::f64::consts::PI / period, + a: 0.0, + b: 0.2, + }, + ]; + assert!((cogging(&terms, 0.0) - 0.3).abs() < 1e-12); + // A quarter of the fundamental: cos → 0, sin → 1; the second harmonic + // is at half its period, sin → 0. + assert!((cogging(&terms, period / 4.0) + 0.1).abs() < 1e-9); + // Periodic in the fundamental. + let q = 0.37; + assert!((cogging(&terms, q) - cogging(&terms, q + 3.0 * period)).abs() < 1e-9); + } + + #[test] + fn tf_ramp_fades_in_over_its_time_and_saturates() { + let mut r = 0.0; + for _ in 0..240 { + r = tf_ramp_step(r, TF_RAMP_S / 480.0); + } + assert!((r - 0.5).abs() < 1e-9); + for _ in 0..1000 { + r = tf_ramp_step(r, 0.01); + } + assert_eq!(r, 1.0); + assert_eq!(tf_ramp_step(0.25, -1.0), 0.25); + } + + /// Reference vectors from `almond_axol.robot.control.dither_step`: + /// 1.5 Nm at 60 Hz stepped at 240 Hz, slots 0 and 1. + #[test] + fn dither_matches_python() { + let golden: [(usize, [f64; 4]); 2] = [ + ( + 0, + [1.5, 8.498308346471969e-16, -1.5, -1.6996616692943939e-15], + ), + ( + 1, + [ + -1.1060533171174791, + -1.0132354413922864, + 1.106053317117479, + 1.0132354413922875, + ], + ), + ]; + for (slot, want) in golden { + let mut phase = slot as f64 * DITHER_PHASE_STAGGER; + for (k, w) in want.iter().enumerate() { + let got = dither_step(&mut phase, 1.5, 60.0, 1.0 / 240.0); + assert!( + (got - w).abs() < 1e-12, + "slot {slot} k {k}: got {got:e}, want {w:e}" + ); + } + } + let mut phase = 1.0; + assert_eq!(dither_step(&mut phase, 0.0, 60.0, 0.01), 0.0); + assert_eq!(phase, 1.0); + } + + /// Reference vectors from `almond_axol.robot.control.stiction_compensation` + /// (amp = 0.36, err_scale = 0.1°): `(err, v_meas, want)`. + #[test] + fn stiction_matches_python() { + let scale = 0.0017453292519943296_f64; + let golden = [ + (-0.02, 0.0, -0.3599999999198255), + (-0.002, 0.0, -0.29390273095808195), + (-0.0005, 0.0, -0.10040068111546155), + (0.0, 0.0, 0.0), + (0.0005, 0.0, 0.10040068111546155), + (0.002, 0.0, 0.29390273095808195), + (0.02, 0.0, 0.3599999999198255), + (0.02, 0.022, 0.1349638509268763), + (0.02, -0.044, 0.03638171680139523), + (0.02, 0.15, 3.268646545848154e-05), + ]; + for (err, v, want) in golden { + let amp = stiction_amplitude(0.6, 0.6, 0.0, 0.0); + let got = stiction(err, v, amp, scale); + assert!( + (got - want).abs() < 1e-12, + "stiction({err}, v={v}): got {got:e}, want {want:e}" + ); + } + // Off by default: a zero amplitude is exactly 0. + assert_eq!(stiction_amplitude(0.6, 0.0, 0.0, 12.0), 0.0); + assert_eq!(stiction(0.02, 0.0, 0.0, scale), 0.0); + // The push follows |gravity|: 0.39 Nm at rest, 2.79 Nm under 12 Nm. + assert!((stiction_amplitude(1.3, 0.3, 0.2, -12.0) - 2.79).abs() < 1e-12); + assert!((stiction_amplitude(1.3, 0.3, 0.2, 0.0) - 0.39).abs() < 1e-12); + } + #[test] fn friction_matches_python() { let golden = [ diff --git a/rust/axol-rt/src/hold.rs b/rust/axol-rt/src/hold.rs index c0e50d27..8541b6d9 100644 --- a/rust/axol-rt/src/hold.rs +++ b/rust/axol-rt/src/hold.rs @@ -17,7 +17,7 @@ use std::sync::atomic::{AtomicBool, Ordering}; use std::sync::Arc; use std::time::{Duration, Instant}; -use crate::bringup::{self, MotorSpec, ReadyMotor, Vendor}; +use crate::bringup::{self, MotorSpec, ReadyMotor, Vendor, WireMode}; use crate::can::CanSock; use crate::proto; use crate::safety::{guarded_send, purge_tx_queue, SendOutcome, STALL_DETECT}; @@ -73,6 +73,23 @@ pub fn parse_params(path: &str) -> io::Result> { k: 0.0, fv: 0.0, fo: 0.0, + stiction_gain: 0.0, + stiction_err: 0.0, + stiction_load_gain: 0.0, + dither_nm: 0.0, + dither_hz: 0.0, + wire: WireMode::Mit, + stribeck_gain: 0.0, + stribeck_dfs: 0.0, + stribeck_load_gain: 0.0, + stribeck_vs: 0.0, + fl: 0.0, + stribeck_pole: 0.0, + cap_track: 0.0, + lead_s: 0.0, + tf_nm_per_pct: 0.0, + mit_hz: 0.0, + cogging: Vec::new(), }, t_ff: fields.get(5)?.parse().ok()?, }) diff --git a/rust/axol-rt/src/proto.rs b/rust/axol-rt/src/proto.rs index cddb15ad..6933437c 100644 --- a/rust/axol-rt/src/proto.rs +++ b/rust/axol-rt/src/proto.rs @@ -127,6 +127,50 @@ pub fn ma_decode_version(data: &[u8; 8]) -> u32 { u32::from_le_bytes([data[4], data[5], data[6], data[7]]) } +/// 0xA4 absolute position closed-loop command: `[0xA4, 0, cap_lo, cap_hi, +/// p0, p1, p2, p3]` — a uint16 output-shaft speed cap (dps) and the int32 +/// target (0.01 deg/LSB, motor frame). Sent to 0x140 + id; the reply comes +/// on 0x240 + id (see [`ma_decode_a4_reply`]). +pub fn ma_a4_encode(p_des: f64, cap_dps: f64) -> [u8; 8] { + let cap = cap_dps.clamp(0.0, u16::MAX as f64).round() as u16; + let pos = (p_des.to_degrees() * 100.0) + .round() + .clamp(i32::MIN as f64, i32::MAX as f64) as i32; + let c = cap.to_le_bytes(); + let p = pos.to_le_bytes(); + [0xA4, 0x00, c[0], c[1], p[0], p[1], p[2], p[3]] +} + +/// 0x73 (protocol V4.4, "TF"): the 0xA4 position command plus a feedforward +/// torque — `[0x73, ff, cap_lo, cap_hi, p0, p1, p2, p3]`, `ff` an int8 in 1% +/// of the motor's rated current. In direct tracking (planner acceleration +/// 0) the firmware adds it to the current command beneath its position and +/// speed PIs; with the planner on the protocol makes it plain 0xA4. The +/// reply has the 0xA4 layout (`ma_decode_a4_reply`) with 0x73 in byte 0. +pub const MA_TF_CMD: u8 = 0x73; + +/// Encode a 0x73 frame: [`ma_a4_encode`]'s target and cap plus `ff_pct` +/// (percent of rated current, rounded and clamped to the int8 range). +pub fn ma_tf_encode(p_des: f64, cap_dps: f64, ff_pct: f64) -> [u8; 8] { + let mut frame = ma_a4_encode(p_des, cap_dps); + frame[0] = MA_TF_CMD; + frame[1] = (ff_pct.round().clamp(-128.0, 127.0) as i8) as u8; + frame +} + +/// The 0x92 multi-turn angle request, paired with an 0xA4 command so the +/// host still gets 0.01 deg position (the 0xA4 reply's own angle is 1 deg). +pub const MA_MULTI_TURN_REQUEST: [u8; 8] = [MA_MULTI_TURN_ANGLE, 0, 0, 0, 0, 0, 0, 0]; + +/// Decode a 0xA4 (or 0xA2/0xA1 — same layout) control reply: `(iq A, +/// speed rad/s, angle rad)`. The angle is the reply's int16 whole degrees. +pub fn ma_decode_a4_reply(data: &[u8; 8]) -> (f64, f64, f64) { + let iq = i16::from_le_bytes([data[2], data[3]]) as f64 * 0.01; + let speed_dps = i16::from_le_bytes([data[4], data[5]]) as f64; + let angle_deg = i16::from_le_bytes([data[6], data[7]]) as f64; + (iq, speed_dps.to_radians(), angle_deg.to_radians()) +} + /// Decode a 0x92 reply: multi-turn angle in radians (0.01 deg/LSB). pub fn ma_decode_position(data: &[u8; 8]) -> f64 { let raw = i32::from_le_bytes([data[4], data[5], data[6], data[7]]); @@ -180,6 +224,36 @@ pub const DM_DISABLE: [u8; 8] = [0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD] pub const DM_CLEAR_ERRORS: [u8; 8] = [0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFB]; /// Damiao POSITION_FORCE command arbitration base (`0x300 + motor_id`). +/// Damiao control-mode register values (`DM_REG_CTRL_MODE`). The motor +/// only acts on the command frame of the mode it is in: an MIT frame is +/// ignored in POS_VEL and vice versa, so the core switches the register +/// (RAM write, immediate) whenever the frame it wants changes. +pub const DM_MODE_MIT: u32 = 1; +pub const DM_MODE_POS_VEL: u32 = 2; +pub const DM_MODE_POS_FORCE: u32 = 4; + +/// Register write (`0x55`, RAM only) for `motor_id`, sent to 0x7FF. The +/// motor does not acknowledge a write; read the register back to verify. +pub fn dm_write_register(motor_id: u16, rid: u8, value: [u8; 4]) -> [u8; 8] { + let (lo, hi) = ((motor_id & 0xFF) as u8, (motor_id >> 8) as u8); + [lo, hi, 0x55, rid, value[0], value[1], value[2], value[3]] +} + +/// Position-velocity command arbitration base (0x100 + motor id): the +/// firmware position loop (`wire_mode pv`), gains in the KP_APR/KI_APR and +/// KP_ASR/KI_ASR registers, ramps in ACC/DEC. Answered with the same +/// feedback frame as MIT, so no paired read is needed. +pub const DM_POS_VEL_ARB_BASE: u16 = 0x100; + +/// Encode a position-velocity command: `p_des` (rad) and the speed cap +/// (rad/s) as two little-endian f32. +pub fn dm_pos_vel_encode(position: f64, max_speed: f64) -> [u8; 8] { + let mut out = [0u8; 8]; + out[..4].copy_from_slice(&(position as f32).to_le_bytes()); + out[4..].copy_from_slice(&(max_speed.max(0.0) as f32).to_le_bytes()); + out +} + pub const DM_POS_FORCE_ARB_BASE: u16 = 0x300; /// Encode a Damiao POSITION_FORCE command (``): raw f32 target position, @@ -294,9 +368,70 @@ pub fn mit_encode(p_des: f64, v_des: f64, kp: f64, kd: f64, t_ff: f64, r: &MitRa mod tests { use super::*; + /// Mirrors `tune.a4`'s `dm_frame`: `struct.pack(" bool { matches!(err.raw_os_error(), Some(libc::ENOBUFS) | Some(libc::EAGAIN)) @@ -99,10 +108,14 @@ fn bringup_script() -> Option { bringup_script_in(Path::new(BRINGUP_SCRIPT), home.as_deref()) } -/// Drop frames queued behind a dead bus by flapping the CAN interface. +/// Drop frames queued behind a dead bus. /// -/// Prefer the installed bring-up script because the dual-channel adapter is -/// most reliable when both channels are flapped together. A purge performed +/// On an arm-hub channel, USB-reset the hub first (`RESET_SCRIPT`): a flap +/// clears the kernel's queue but not the frames the hub firmware already +/// holds, which it transmits on the next open. Otherwise, or when the reset +/// cannot run, flap the interface — preferring the installed bring-up script +/// because the dual-channel adapter is most reliable when both channels are +/// flapped together. A purge performed /// for the other arm within the last three seconds counts for this arm too. /// /// Returns false when the flap could not be run at all — most often because @@ -113,10 +126,19 @@ fn bringup_script() -> Option { /// (`almond_axol.cli.can.setup.purge_stale_tx`). pub fn purge_tx_queue(iface: &str) -> bool { let mut last = LAST_PURGE.lock().unwrap(); + let reset = reset_script_for(iface, Path::new(RESET_SCRIPT)); let script = bringup_script(); - if script.is_some() && last.is_some_and(|t| t.elapsed() < PURGE_DEDUPE) { + if (reset.is_some() || script.is_some()) && last.is_some_and(|t| t.elapsed() < PURGE_DEDUPE) { return true; } + if let Some(path) = &reset { + // Falls through to the flap when the reset cannot run — most often a + // robot whose sudo grant predates the script (`axol provision`). + if run_root(&["bash", &path.to_string_lossy()]).is_ok_and(|st| st.success()) { + *last = Some(Instant::now()); + return true; + } + } let result = match &script { Some(path) => run_root(&["bash", &path.to_string_lossy()]), None => run_root(&["ip", "link", "set", iface, "down"]).and_then(|st| { @@ -136,6 +158,12 @@ pub fn purge_tx_queue(iface: &str) -> bool { } } +/// The reset script to purge `iface` with: only for an arm-hub channel, and +/// only once `axol can.setup` has installed it. Split out for the tests. +fn reset_script_for(iface: &str, provisioned: &Path) -> Option { + (ARM_HUB_IFACES.contains(&iface) && provisioned.is_file()).then(|| provisioned.to_path_buf()) +} + #[cfg(test)] mod tests { use super::*; @@ -185,4 +213,29 @@ mod tests { assert_eq!(bringup_script_in(&dir.join("absent.sh"), Some(&dir)), None); assert_eq!(bringup_script_in(&dir.join("absent.sh"), None), None); } + + #[test] + fn arm_hub_channels_purge_with_the_usb_reset() { + let dir = scratch("reset"); + let reset = dir.join("reset_adapter.sh"); + std::fs::write(&reset, "#!/bin/bash\n").unwrap(); + for iface in ARM_HUB_IFACES { + assert_eq!(reset_script_for(iface, &reset), Some(reset.clone())); + } + } + + #[test] + fn other_buses_and_unset_up_hosts_keep_the_flap() { + // The wheel/chest adapters are single-channel and not the hub; a + // reset of the hub for their stall would drop a healthy arm session. + let dir = scratch("noreset"); + let reset = dir.join("reset_adapter.sh"); + std::fs::write(&reset, "#!/bin/bash\n").unwrap(); + assert_eq!(reset_script_for("can_alm_axol_b", &reset), None); + assert_eq!(reset_script_for("can0", &reset), None); + assert_eq!( + reset_script_for("can_alm_axol_l", &dir.join("absent.sh")), + None + ); + } } diff --git a/rust/axol-rt/src/serve.rs b/rust/axol-rt/src/serve.rs index 143fbac3..76410a16 100644 --- a/rust/axol-rt/src/serve.rs +++ b/rust/axol-rt/src/serve.rs @@ -152,6 +152,14 @@ //! by the Python torque-residual `ContactWatchdog` (limp gravity-comp //! hold, operator resets); a self-disabled motor just stops contributing //! while the rest of the arm keeps working, as in classic mode. +//! - **The firmware position loop is the one exception: a runaway goes +//! limp.** A joint on 0xA4 / 0x73 is stiff by design, so one more than +//! `A4_RUNAWAY_DEV_RAD` from the target it was just sent and moving +//! *away* faster than `A4_RUNAWAY_VEL_RAD_S`, on `A4_RUNAWAY_SAMPLES` +//! replies in a row, is being driven there by its own loop (right +//! shoulder_1 drove itself into its end stop, 2026-09-23). The session goes +//! limp as for a silent motor, with the runaway joint braked at the MIT +//! maximum kd (`A4_RUNAWAY_KD`) instead of coasting on `LIMP_KD`. //! - Every command batch accepts exactly one fresh reply per motor. A missed //! sample suppresses host damping for that tick; bursty loss (4 of the last //! 32 ticks) marks the joint *degraded* — host damping stays off until a @@ -198,7 +206,7 @@ use std::sync::{Arc, Condvar, Mutex}; use std::thread::JoinHandle; use std::time::{Duration, Instant}; -use crate::bringup::{self, MotorSpec, Vendor}; +use crate::bringup::{self, MotorSpec, ReadyMotor, Vendor, WireMode}; use crate::can::CanSock; use crate::filter::{self, BandPass, Cadence, Holdover, LpDiff, Trapezoid}; use crate::hold::sleep_until; @@ -238,7 +246,37 @@ const HOLDOVER_MAX: f64 = 0.080; /// - 1 (implicit; never declared): arm joints took slots in list order. /// - 2: slots come from the motor id (`slot = motor_id - 1`), so a bus may /// carry any subset of the arm. -const CONFIG_PROTO: u32 = 2; +/// - 3: each `joint` line carries two more fields, the stiction +/// compensation gain and its error scale (`filter::stiction`). +/// - 4: plus the load-proportional stiction gain (`filter::stiction_amplitude`). +/// - 5: plus the torque dither amplitude and frequency (`filter::dither_step`). +/// - 6: plus the wire mode token (`mit` | `a4`, `bringup::WireMode`). +/// - 7: plus the four Stribeck cancellation fields (`filter::stribeck_excess`). +/// - 8: plus the load-proportional Coulomb friction `fl` (Nm per Nm of gravity). +/// - 9: plus the Stribeck term's measured-velocity pole (rad/s). +/// - 10: the wire token gains `pv` (Damiao position-velocity, +/// `bringup::WireMode::Pv`); `loop_hz` above `THIN_ABOVE_HZ` thins the bus +/// schedule (`Thinning`). +/// - 11: impedance joints run at `IMPEDANCE_HZ` only — `loop_hz` 240, or +/// `MIXED_LOOP_HZ` with them on alternate ticks (`Thinning::mit_lane`) — +/// and anything else is refused. A proto-10 core given 480 would command +/// them at 480. +/// - 12: an optional trailing `cap_track` per joint line (`a4_speed_cap`); +/// a proto-11 core would ignore it and run a planner joint at a fixed cap. +/// - 13: a joint with `cap_track > 0` (the 0xA4 planner) rides the half-rate +/// lane — 240 Hz in a 480 Hz loop, 200 Hz at 400 — where the planner +/// follows; a proto-12 core would command it every tick, where it does not. +/// - 14: an optional `lead_ms` after `cap_track` (`a4_target`). +/// - 15: `impedance_hz` (240 | 480 — the MyActuator impedance joints every +/// tick of a 480 Hz loop, wrists on the 240 Hz lane), an optional +/// `tf_nm_per_pct` after `lead_ms` (0x73 torque feedforward), and +/// `cogging` lines (position-periodic torque cancellation, sent on a +/// second configure once joint offsets are known). A proto-14 core would +/// refuse the new lines — or worse, run 480 Hz impedance at 240. +/// - 16: an optional per-joint `impedance_hz` after `tf_nm_per_pct` (240 | +/// 480 | 0 = the config's): single impedance joints at 480 Hz, the rest on +/// the 240 Hz lane. A proto-15 core would run them all at one rate. +const CONFIG_PROTO: u32 = 16; /// Rolling feedback loss at or above this many misses in the last 32 ticks /// (12.5% over 133 ms at 240 Hz) marks a joint *degraded*: its host damping /// stays off until a full clean window has passed, and the transition is @@ -397,23 +435,42 @@ fn configure_bus_scheduling( /// start-to-start period gives up an unobservable amount of wall-clock phase /// instead: lateness can lower the average rate briefly, but can never produce /// a catch-up command faster than the configured rate. +/// `(p50, p95, max)` of the per-tick bus-busy fractions since the last stats +/// line; NaN when no tick had a reply. Sorts in place. +fn bus_busy_percentiles(busy: &mut [f64]) -> (f64, f64, f64) { + if busy.is_empty() { + return (f64::NAN, f64::NAN, f64::NAN); + } + busy.sort_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal)); + let at = |q: f64| busy[((busy.len() - 1) as f64 * q).round() as usize]; + (at(0.5), at(0.95), busy[busy.len() - 1]) +} + fn next_bus_deadline(began: Instant, period: Duration) -> Instant { began + period } -/// Accept at most one reply from each motor commanded in this tick. +/// Accept at most as many replies from each motor as it was sent commands +/// this tick (one for MIT, two for an 0xA4 joint: the command reply and the +/// 0x92 position read). /// /// CAN frames carry no command sequence number. The bus loop therefore drains -/// late frames before sending and uses this per-batch set to prevent duplicate -/// or unsolicited feedback from satisfying another motor's reply budget. -fn mark_unique_expected_reply(expected: &[bool], seen: &mut [bool], idx: usize) -> bool { - if idx >= expected.len() || idx >= seen.len() || !expected[idx] || seen[idx] { +/// late frames before sending and uses this per-batch budget to prevent +/// duplicate or unsolicited feedback from satisfying another motor's reply +/// budget. +fn mark_unique_expected_reply(expected: &[u8], seen: &mut [u8], idx: usize) -> bool { + if idx >= expected.len() || idx >= seen.len() || seen[idx] >= expected[idx] { return false; } - seen[idx] = true; + seen[idx] += 1; true } +/// Every reply the motor was budgeted for this tick arrived. +fn reply_complete(expected: &[u8], seen: &[u8], idx: usize) -> bool { + expected[idx] > 0 && seen[idx] >= expected[idx] +} + /// Outcome of one feedback opportunity for one arm joint. #[derive(Clone, Copy, Debug, PartialEq, Eq)] enum FeedbackVerdict { @@ -556,6 +613,430 @@ fn go_limp(limp: &AtomicBool, out_tx: &mpsc::Sender>, reason: &str) -> b true } +/// Whether this tick's frame for a joint is the 0xA4 position command. +/// +/// Only MyActuator joints on `wire_mode a4`, and for them on every tick that +/// commands a position: tracked ticks *and* passthrough holds (bring-up +/// hold, a stalled stream, Python's hold-at-measured-pose), which is any +/// tick with a position gain. The alternative — MIT for the hold, 0xA4 once +/// tracking starts — is what the classic design did, and it is exactly what +/// the X6-P20's 2025070202 firmware refuses: after an MIT frame, 0xA4 is +/// ignored until the motor is reset. The right elbow held its pose through +/// two whole replays that way (2026-09-21) while the X8-P20 shoulders, whose +/// 2026042402 firmware switches freely, tracked. Keeping an a4 joint on 0xA4 +/// from its first frame works on both. Limp and gravity comp (`kp == 0`) +/// stay MIT: those exist to make the joint compliant, which the firmware +/// position loop cannot be — so on the older firmware a joint that has been +/// hand-guided needs a re-enable before it will track on a4 again. +fn a4_wire(vendor: Vendor, wire: WireMode, tracked: bool, kp: f64) -> bool { + vendor == Vendor::MyActuator && wire == WireMode::A4 && (tracked || kp > 0.0) +} + +/// Whether this tick's frame for a joint is the Damiao position-velocity +/// command (0x100 + id): Damiao wrists on `wire_mode pv`, on every tick that +/// commands a position — the same rule as [`a4_wire`]. Limp and gravity +/// comp (`kp == 0`) fall back to MIT for compliance; the bus loop switches +/// the wrist's control-mode register along with the frame, because the +/// firmware ignores the frame of the mode it is not in. +fn pv_wire(vendor: Vendor, wire: WireMode, tracked: bool, kp: f64) -> bool { + vendor == Vendor::Damiao && wire == WireMode::Pv && (tracked || kp > 0.0) +} + +/// Above this loop rate the bus cannot carry every motor's frames every +/// tick and the schedule is thinned (`Thinning`). A 1 Mbps bus moves a +/// frame in ~0.13 ms with the USB adapters in the loop (measured: three a4 +/// joints at 240 Hz ran the bus 66-73% busy, 22 frames in ~2.9 ms). The +/// full eight-motor tick under the position controller — five 0xA4 +/// commands with echoes, five 0x92 reads, two wrist commands, the gripper — +/// is 26 frames, 3.4 ms: it fits a 240 Hz tick (4.17 ms), not a 400 Hz one +/// (2.5 ms, of which `REPLY_GUARD` is reserved). +const THIN_ABOVE_HZ: f64 = 300.0; + +/// The only rate an impedance (MIT) arm joint is commanded at. Its gains, +/// feedforward and damping filters were tuned and verified at 240 Hz; on a +/// loop mixing it with firmware-loop joints that run faster, it gets its own +/// 240 Hz cadence inside the faster loop (`Thinning::mit_lane`), not the +/// loop's rate — at 400 Hz with the firmware joints beside it, right +/// shoulder_3 / wrist_1 on impedance shook the arm (2026-09-22). +const IMPEDANCE_HZ: f64 = 240.0; + +/// The loop rate of a bus that mixes impedance arm joints with firmware-loop +/// ones: twice `IMPEDANCE_HZ`, so every impedance joint lands on alternate +/// ticks at exactly 240 Hz while the 0xA4 joints get 480 Hz (above the +/// position controller's 400, so no audible 200 Hz staircase either). +const MIXED_LOOP_HZ: f64 = 2.0 * IMPEDANCE_HZ; + +/// The fast impedance rate (`impedance_hz 480`, `tune.motion +/// --impedance-hz 480`): the MyActuator impedance joints commanded every +/// tick of a 480 Hz loop, their host pipeline stepped at 480, while the +/// Damiao wrists stay on the verified 240 Hz on alternate ticks +/// (`Thinning::mit_lane`). An experiment: the MyActuator gains, host +/// damping and feedforward were tuned at 240, and 400 Hz impedance on right +/// shoulder_3 / wrist_1 shook the arm once (2026-09-22) — it is opt-in and +/// never the config default. +const FAST_IMPEDANCE_HZ: f64 = 2.0 * IMPEDANCE_HZ; + +/// Whether an arm joint runs impedance at `FAST_IMPEDANCE_HZ` — every tick of +/// a 480 Hz loop — rather than on the 240 Hz half-rate lane: its own +/// `impedance_hz` 480, or (with none of its own) the config-wide 480, which +/// covers the MyActuator joints only (the Damiao wrists stay at 240). +fn fast_mit( + wire: WireMode, + gripper: bool, + myactuator: bool, + mit_hz: f64, + impedance_hz: f64, +) -> bool { + let is = |a: f64, b: f64| (a - b).abs() < 1e-6; + wire == WireMode::Mit + && !gripper + && (is(mit_hz, FAST_IMPEDANCE_HZ) + || (mit_hz <= 0.0 && myactuator && is(impedance_hz, FAST_IMPEDANCE_HZ))) +} + +/// Refuse a loop rate an impedance arm joint cannot run at. +/// +/// Every impedance joint runs at 240 Hz or at `FAST_IMPEDANCE_HZ` — per +/// joint (`impedance_hz` on its joint line: 240, 480, or 0 for the +/// config-wide `impedance_hz`). With any joint at 480 the loop must be 480 +/// (the rest on alternate ticks); otherwise 240, or `MIXED_LOOP_HZ` with the +/// impedance joints on alternate ticks. Anything else would command a joint +/// at a rate it was never meant to run at. +fn check_impedance_rate(loop_hz: f64, impedance_hz: f64, specs: &[MotorSpec]) -> io::Result<()> { + let is = |a: f64, b: f64| (a - b).abs() < 1e-6; + let rate_ok = |hz: f64| is(hz, IMPEDANCE_HZ) || is(hz, FAST_IMPEDANCE_HZ); + if !rate_ok(impedance_hz) { + return Err(io::Error::new( + io::ErrorKind::InvalidData, + format!( + "config: impedance_hz {impedance_hz} — impedance runs at {IMPEDANCE_HZ} Hz, or \ + {FAST_IMPEDANCE_HZ} Hz on the MyActuator joints (wrists staying at {IMPEDANCE_HZ})" + ), + )); + } + if let Some(s) = specs.iter().find(|s| s.mit_hz > 0.0 && !rate_ok(s.mit_hz)) { + return Err(io::Error::new( + io::ErrorKind::InvalidData, + format!( + "config: {} impedance_hz {} — {IMPEDANCE_HZ} or {FAST_IMPEDANCE_HZ} only", + s.joint, s.mit_hz + ), + )); + } + let fast = specs + .iter() + .find(|s| fast_mit(s.wire, s.gripper, s.motor_id <= 5, s.mit_hz, impedance_hz)); + let allowed: &[f64] = if fast.is_some() { + &[FAST_IMPEDANCE_HZ] + } else { + &[IMPEDANCE_HZ, MIXED_LOOP_HZ] + }; + if allowed.iter().any(|&hz| is(loop_hz, hz)) { + return Ok(()); + } + if let Some(s) = specs.iter().find(|s| !s.gripper && s.wire == WireMode::Mit) { + let rule = match fast { + Some(f) => format!( + "{} runs impedance at {FAST_IMPEDANCE_HZ} Hz, so the loop is {FAST_IMPEDANCE_HZ} Hz only", + f.joint + ), + None => format!( + "impedance runs at {IMPEDANCE_HZ} Hz only (loop_hz {IMPEDANCE_HZ}, or \ + {MIXED_LOOP_HZ} with it on alternate ticks)" + ), + }; + return Err(io::Error::new( + io::ErrorKind::InvalidData, + format!( + "config: loop_hz {loop_hz} with {} on the impedance frame — {rule}", + s.joint + ), + )); + } + Ok(()) +} + +/// Ticks between read-lane turns (the a4 0x92 reads and the gripper) when +/// impedance joints run every tick at 480 Hz (at most all five MyActuator +/// ones). Five MIT +/// request/reply pairs plus one wrist pair is 12 frames, ~1.6 ms of the +/// 2.08 ms tick; one more pair on every tick would leave no margin before +/// `REPLY_GUARD`, so the lane takes a turn on one tick in this many (60 Hz +/// for the gripper alone). An a4 joint's position is carried on its echo's +/// speed between reads (`a4_extrapolate`), as on the thinned 400 Hz bus. +const FAST_MIT_READ_DIV: u64 = 8; + +/// The bus schedule when the loop runs faster than the bus (`THIN_ABOVE_HZ`). +/// +/// Every MyActuator command still goes out every tick — that is the point +/// of the higher rate (the position loop's target staircase is audible at +/// 200 Hz and gone at 400). The rest rides in two round-robin lanes of one +/// request/reply pair per tick each, so a full tick is a fixed 14 frames +/// (~1.8 ms, ~72% of a 2.5 ms tick): +/// +/// - the Damiao wrists take turns being commanded (200 Hz each with two +/// wrists; their own profiler shapes the staircase); +/// - the 0xA4 joints take turns having their 0.01° position read (0x92) +/// and the gripper takes a turn in the same lane (about 67 Hz each on +/// the full arm). Between reads an a4 joint's position is carried +/// forward from the last read on the speed its command echo reports +/// every tick (`a4_extrapolate`). +/// +/// On a bus that mixes impedance (MIT) arm joints with firmware-loop ones +/// the loop runs at `MIXED_LOOP_HZ` and the impedance joints form their own +/// lane instead: each is commanded every other tick, at exactly 240 Hz, the +/// lane split across both phases so each tick carries half of them. Their +/// whole host pipeline runs on those ticks only (see `bus_loop`), so they +/// behave as the verified 240 Hz loop, interleaved. +/// +/// Below the threshold nothing is thinned: every motor is commanded, and +/// every a4 joint read, every tick. +struct Thinning { + enabled: bool, + /// The half-rate lane — impedance (MIT) arm joints and 0xA4 joints on + /// the firmware planner — as `(motor index, phase)`: commanded on ticks + /// where `tick % mit_div == phase`, their host pipeline stepped at that + /// rate (see `bus_loop`). + mit_lane: Vec<(usize, u64)>, + /// Ticks per impedance command: 2 on a mixed bus, 1 otherwise. + mit_div: u64, + /// Motor indices of the Damiao wrists not in `mit_lane`, one commanded + /// per tick. + dm_lane: Vec, + /// Motor indices of the a4 joints and the gripper, one served per turn. + read_lane: Vec, + /// Ticks per read-lane turn: 1, or `FAST_MIT_READ_DIV` when the + /// MyActuator impedance joints take every tick at 480 Hz. + read_div: u64, + /// The gripper's motor index, when the bus has one. + gripper: Option, +} + +impl Thinning { + fn plan(motors: &[ReadyMotor], loop_hz: f64, impedance_hz: f64) -> Self { + let enabled = loop_hz > THIN_ABOVE_HZ; + // Fast impedance joints (`fast_mit`) run at the loop's own 480 Hz, + // off the half-rate lane; every other impedance joint stays on it. + let at_480 = enabled && (loop_hz - FAST_IMPEDANCE_HZ).abs() < 1e-6; + let is_fast = |m: &ReadyMotor| { + at_480 + && fast_mit( + m.wire, + m.gripper, + m.vendor == Vendor::MyActuator, + m.mit_hz, + impedance_hz, + ) + }; + // The half-rate lane: impedance arm joints (the Damiao ones only at + // fast impedance), and 0xA4 joints on the firmware planner + // (`cap_track > 0`, planner acceleration 60000). The planner plans + // to each target and needs ~4 ms between them: on the right elbow a + // 50 deg/s sine tracked at 240 / 200 Hz (0.7° RMS, 13 ms) and fell + // 20° behind at 480 Hz (2026-09-22). + let mit: Vec = motors + .iter() + .enumerate() + .filter(|(_, m)| { + !m.gripper && ((m.wire == WireMode::Mit && !is_fast(m)) || is_planner(m)) + }) + .map(|(i, _)| i) + .collect(); + // `check_impedance_rate` has held a bus with impedance joints to 240 + // or 480 Hz, so above the threshold this is 2 (and 2 at 400 Hz, the + // position controller, for planner joints: 200 Hz). + let mit_div = if enabled && !mit.is_empty() { + (loop_hz / IMPEDANCE_HZ).round().max(1.0) as u64 + } else { + 1 + }; + let mit_lane: Vec<(usize, u64)> = if mit_div > 1 { + mit.iter() + .enumerate() + .map(|(k, &idx)| (idx, k as u64 % mit_div)) + .collect() + } else { + Vec::new() + }; + let dm_lane = motors + .iter() + .enumerate() + .filter(|(i, m)| { + m.vendor == Vendor::Damiao && !m.gripper && !mit_lane.iter().any(|(j, _)| j == i) + }) + .map(|(i, _)| i) + .collect(); + // Half-rate planner joints read their fine position on every one of + // their own commands (`a4_read`), so the round-robin read lane keeps + // only the every-tick a4 joints and the gripper. + let read_lane = motors + .iter() + .enumerate() + .filter(|(i, m)| { + m.gripper + || (m.vendor == Vendor::MyActuator + && m.wire == WireMode::A4 + && !mit_lane.iter().any(|(j, _)| j == i)) + }) + .map(|(i, _)| i) + .collect(); + let gripper = motors.iter().position(|m| m.gripper); + let read_div = if motors.iter().any(is_fast) { + FAST_MIT_READ_DIV + } else { + 1 + }; + Self { + enabled, + mit_lane, + mit_div, + dm_lane, + read_lane, + read_div, + gripper, + } + } + + /// The read-lane entry served on `tick`: the lane takes a turn on one + /// tick in `read_div`. + fn read_turn(&self, tick: u64) -> Option { + if !tick.is_multiple_of(self.read_div) { + return None; + } + Self::turn(&self.read_lane, tick / self.read_div) + } + + /// The phase of a half-rate-lane joint (impedance, or 0xA4 on the + /// planner) on its own cadence, or `None` for a motor commanded at the + /// loop's rate (or its round-robin lane). + fn mit_phase(&self, idx: usize) -> Option { + self.mit_lane + .iter() + .find(|(j, _)| *j == idx) + .map(|(_, phase)| *phase) + } + + fn turn(lane: &[usize], tick: u64) -> Option { + if lane.is_empty() { + None + } else { + Some(lane[(tick % lane.len() as u64) as usize]) + } + } + + /// Whether motor `idx` is commanded on `tick`. + fn commanded(&self, idx: usize, tick: u64) -> bool { + if !self.enabled { + return true; + } + if let Some(phase) = self.mit_phase(idx) { + return tick % self.mit_div == phase; + } + if self.dm_lane.contains(&idx) { + return Self::turn(&self.dm_lane, tick) == Some(idx); + } + if self.gripper == Some(idx) { + return self.read_turn(tick) == Some(idx); + } + true + } + + /// Whether an a4 joint's command on `tick` is followed by its 0x92 read. + /// A half-rate (planner) joint reads on each of its own commands. + fn a4_read(&self, idx: usize, tick: u64) -> bool { + !self.enabled || self.mit_phase(idx).is_some() || self.read_turn(tick) == Some(idx) + } +} + +/// An a4 joint's position between 0x92 reads: the last read (or the +/// previous carry) advanced along the speed its 0xA4 echo reports this +/// tick. The echo's speed is 1 dps resolution, so over a 15 ms read +/// interval the carry is within ~0.01° of the next read — the read's own +/// resolution — and a joint at rest (speed 0) never drifts. +fn a4_extrapolate(anchor: (f64, Instant), speed: f64, now: Instant) -> f64 { + anchor.0 + speed * now.saturating_duration_since(anchor.1).as_secs_f64() +} + +/// An 0xA4 joint on the firmware planner: speed-cap tracking is what the +/// planner route sets (`FirmwareGains.cap_track` alongside planner +/// acceleration 60000), and such a joint rides the half-rate lane. +fn is_planner(m: &ReadyMotor) -> bool { + m.vendor == Vendor::MyActuator && m.wire == WireMode::A4 && m.cap_track > 0.0 +} + +/// The 0xA4 target for one tick: the tracker position led `lead_s` along +/// its velocity. On the firmware planner a target reached within the step +/// stops the joint for the rest of it — a speed ripple at the step rate, +/// which put ~half the right elbow's speed error at 60-120 Hz on the 240 Hz +/// lane (2026-09-22); a target a few ms ahead keeps it cruising. 0 = none. +fn a4_target(p_cmd: f64, v_trk: f64, lead_s: f64) -> f64 { + p_cmd + v_trk * lead_s +} + +/// Lowest 0xA4 speed cap a tracking cap sets (dps), so a stationary target +/// still corrects (`tune.a4 --cap-floor`'s default). +const A4_CAP_FLOOR_DPS: f64 = 1.0; + +/// The 0xA4 frame's speed cap (dps) for one tick. +/// +/// Direct PI tracking (`cap_track <= 0`, planner acceleration 0): the fixed +/// tracker limit — there the cap is a hard limit on the PI output, and one +/// pinned near the commanded speed never lets the loop catch up (right +/// elbow, 2026-09-21: 1.8° RMS, 480 ms lag). With the firmware planner on +/// (60000), a fixed cap makes it finish each step at that speed and idle the +/// rest of the tick — 4x the current spread on the elbow — so the cap tracks +/// `cap_track` times the commanded speed instead (`tune.a4 --cap-track`, +/// 1.1-1.2 moved the joint continuously), never below `A4_CAP_FLOOR_DPS` +/// nor above the tracker limit. +fn a4_speed_cap(cap_track: f64, v_cmd: f64, max_vel: f64) -> f64 { + let fixed = max_vel.to_degrees(); + if cap_track <= 0.0 { + return fixed; + } + (cap_track * v_cmd.abs().to_degrees()).clamp(A4_CAP_FLOOR_DPS.min(fixed), fixed) +} + +/// Runaway guard on the firmware position loop (0xA4 / 0x73): position +/// error past which a joint moving *away* from its target is running away. +/// Normal lag is ~1° at the approach speed; right shoulder_1 on a4 with the +/// planner at 0 in ROM drove itself to its end stop (2026-09-23). +const A4_RUNAWAY_DEV_RAD: f64 = 5.0 * std::f64::consts::PI / 180.0; +/// ...at more than this speed away from the target (30°/s). +const A4_RUNAWAY_VEL_RAD_S: f64 = 30.0 * std::f64::consts::PI / 180.0; +/// ...on this many consecutive samples (~6 ms at 480 Hz), so one noisy +/// reply cannot trip it. +const A4_RUNAWAY_SAMPLES: u32 = 3; +/// Firmware damping (Nm·s/rad, the MIT maximum) on the joint that ran away, +/// once the session is limp: a limp joint's `LIMP_KD` would let a heavy +/// shoulder coast on into its stop; this brakes it and still hand-guides. +const A4_RUNAWAY_KD: f64 = 5.0; + +/// Per-joint runaway detector for the firmware position loop. Unlike the +/// deliberately absent deviation abort on impedance joints (see Safety), a +/// firmware-loop joint is stiff by design: moving fast *away* from its own +/// target means the loop is driving it there — a hand pushing it would be +/// fought, not followed — so the core takes it (and the session) limp. +#[derive(Clone, Copy, Debug, Default)] +struct RunawayGuard { + strikes: u32, +} + +impl RunawayGuard { + /// Feed one sample of an a4 joint; true once it has run away. + fn check(&mut self, meas_p: f64, meas_v: f64, target: f64) -> bool { + let err = meas_p - target; + let away = err.abs() > A4_RUNAWAY_DEV_RAD + && meas_v.abs() > A4_RUNAWAY_VEL_RAD_S + && meas_v.signum() == err.signum(); + self.strikes = if away { self.strikes + 1 } else { 0 }; + self.strikes >= A4_RUNAWAY_SAMPLES + } + + fn reset(&mut self) { + self.strikes = 0; + } +} + #[derive(Clone, Copy, Debug, Default)] pub struct JointCmd { pub p_des: f64, @@ -619,12 +1100,18 @@ struct TraceRow { friction_ff: f64, inertia_ff: f64, damping_ff: f64, + stiction_ff: f64, + dither_ff: f64, + stribeck_ff: f64, total_ff: f64, kd_host: f64, damp_w0: f64, damp_q: f64, tick_dt: f64, fb_dt: f64, + cogging_ff: f64, + /// The 0x73 feedforward sent, % of rated current (0 on other frames). + tf_pct: f64, } type TraceHandle = JoinHandle>; @@ -640,7 +1127,7 @@ fn trace_file(path: &PathBuf) -> io::Result> { let mut out = io::BufWriter::new(std::fs::File::create(path)?); writeln!( out, - "tick,time_s,seq,slot,motor_id,mode,target_p,cmd_p,cmd_v,cmd_a,cmd_v_fast,meas_p,motor_v,meas_v,meas_tau,gravity_ff,friction_ff,inertia_ff,damping_ff,total_ff,kd_host,damp_w0,damp_q,tick_dt,fb_dt" + "tick,time_s,seq,slot,motor_id,mode,target_p,cmd_p,cmd_v,cmd_a,cmd_v_fast,meas_p,motor_v,meas_v,meas_tau,gravity_ff,friction_ff,inertia_ff,damping_ff,stiction_ff,dither_ff,stribeck_ff,total_ff,kd_host,damp_w0,damp_q,tick_dt,fb_dt,cogging_ff,tf_pct" )?; Ok(out) } @@ -648,7 +1135,7 @@ fn trace_file(path: &PathBuf) -> io::Result> { fn write_trace_row(out: &mut io::BufWriter, r: TraceRow) -> io::Result<()> { writeln!( out, - "{},{:.9},{},{},{},{:.1},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.9},{:.9}", + "{},{:.9},{},{},{},{:.1},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.12},{:.9},{:.9},{:.12},{:.6}", r.tick, r.time_s, r.seq, @@ -668,12 +1155,17 @@ fn write_trace_row(out: &mut io::BufWriter, r: TraceRow) -> io::R r.friction_ff, r.inertia_ff, r.damping_ff, + r.stiction_ff, + r.dither_ff, + r.stribeck_ff, r.total_ff, r.kd_host, r.damp_w0, r.damp_q, r.tick_dt, r.fb_dt, + r.cogging_ff, + r.tf_pct, ) } @@ -812,6 +1304,10 @@ fn start_trace_writer( struct Config { loop_hz: f64, + /// The MyActuator impedance joints' command rate: `IMPEDANCE_HZ` (the + /// verified 240) or `FAST_IMPEDANCE_HZ` (480, the Damiao wrists staying + /// on a 240 Hz lane) — see `check_impedance_rate`. + impedance_hz: f64, watchdog_ms: f64, max_step_rad: f64, /// (side, iface, specs) — side 0 = left, 1 = right. @@ -820,6 +1316,7 @@ struct Config { fn parse_config(text: &str) -> io::Result { let mut loop_hz = 240.0; + let mut impedance_hz = IMPEDANCE_HZ; let mut watchdog_ms = 150.0; let mut max_step_rad = 0.35; let mut buses: Vec<(u8, String, Vec)> = Vec::new(); @@ -862,6 +1359,56 @@ fn parse_config(text: &str) -> io::Result { .and_then(|v| v.parse().ok()) .ok_or_else(|| bad(line))? } + "impedance_hz" => { + impedance_hz = f + .get(1) + .and_then(|v| v.parse().ok()) + .ok_or_else(|| bad(line))? + } + "cogging" => { + // cogging ( ) × n — + // after that joint's own line; motor frame, gain applied. + let side: u8 = f + .get(1) + .and_then(|v| v.parse().ok()) + .ok_or_else(|| bad(line))?; + let iface = f.get(2).ok_or_else(|| bad(line))?; + let motor_id: u8 = f + .get(3) + .and_then(|v| v.parse().ok()) + .ok_or_else(|| bad(line))?; + let n: usize = f + .get(4) + .and_then(|v| v.parse().ok()) + .ok_or_else(|| bad(line))?; + if f.len() != 5 + 3 * n { + return Err(bad(line)); + } + let num = |i: usize| -> io::Result { + f.get(i) + .and_then(|v| v.parse::().ok()) + .filter(|v| v.is_finite()) + .ok_or_else(|| bad(line)) + }; + let mut terms = Vec::with_capacity(n); + for k in 0..n { + terms.push(filter::CogTerm { + w: num(5 + 3 * k)?, + a: num(6 + 3 * k)?, + b: num(7 + 3 * k)?, + }); + } + let spec = buses + .iter_mut() + .find(|(s, i, _)| *s == side && i == iface) + .and_then(|(_, _, specs)| { + specs + .iter_mut() + .find(|s| !s.gripper && s.motor_id == motor_id) + }) + .ok_or_else(|| bad(line))?; + spec.cogging = terms; + } "watchdog_ms" => { watchdog_ms = f .get(1) @@ -877,6 +1424,11 @@ fn parse_config(text: &str) -> io::Result { "joint" | "gripper" => { // joint // + // + // + // + // [ [ + // [ []]]] // gripper let gripper = f[0] == "gripper"; let side: u8 = f @@ -913,6 +1465,23 @@ fn parse_config(text: &str) -> io::Result { k: 0.0, fv: 0.0, fo: 0.0, + stiction_gain: 0.0, + stiction_err: 0.0, + stiction_load_gain: 0.0, + dither_nm: 0.0, + dither_hz: 0.0, + wire: WireMode::Mit, + stribeck_gain: 0.0, + stribeck_dfs: 0.0, + stribeck_load_gain: 0.0, + stribeck_vs: 0.0, + fl: 0.0, + stribeck_pole: 0.0, + cap_track: 0.0, + lead_s: 0.0, + tf_nm_per_pct: 0.0, + mit_hz: 0.0, + cogging: Vec::new(), } } else { let motor_id: u8 = f @@ -940,6 +1509,42 @@ fn parse_config(text: &str) -> io::Result { k: num(10)?, fv: num(11)?, fo: num(12)?, + stiction_gain: num(13)?, + stiction_err: num(14)?, + stiction_load_gain: num(15)?, + dither_nm: num(16)?, + dither_hz: num(17)?, + wire: f + .get(18) + .and_then(|t| WireMode::parse(t)) + .ok_or_else(|| bad(line))?, + stribeck_gain: num(19)?, + stribeck_dfs: num(20)?, + stribeck_load_gain: num(21)?, + stribeck_vs: num(22)?, + fl: num(23)?, + stribeck_pole: num(24)?, + // Optional: absent = fixed 0xA4 cap (direct tracking). + cap_track: match f.get(25) { + Some(_) => num(25)?, + None => 0.0, + }, + // Optional: the 0xA4 target lead, ms on the wire. + lead_s: match f.get(26) { + Some(_) => num(26)? / 1e3, + None => 0.0, + }, + // Optional: the 0x73 feedforward scale (0 = 0xA4). + tf_nm_per_pct: match f.get(27) { + Some(_) => num(27)?, + None => 0.0, + }, + // Optional: this joint's impedance rate (0 = config's). + mit_hz: match f.get(28) { + Some(_) => num(28)?, + None => 0.0, + }, + cogging: Vec::new(), } }; if spec.slot >= N_SLOTS || bus.2.iter().any(|s| s.slot == spec.slot) { @@ -969,8 +1574,12 @@ fn parse_config(text: &str) -> io::Result { "config: no joints", )); } + for (_, _, specs) in &buses { + check_impedance_rate(loop_hz, impedance_hz, specs)?; + } Ok(Config { loop_hz, + impedance_hz, watchdog_ms, max_step_rad, buses, @@ -1085,13 +1694,21 @@ mod tests { #[test] fn replies_must_be_expected_and_unique() { - let expected = [true, true, false]; - let mut seen = [false; 3]; + let expected = [1u8, 2, 0]; + let mut seen = [0u8; 3]; assert!(mark_unique_expected_reply(&expected, &mut seen, 0)); assert!(!mark_unique_expected_reply(&expected, &mut seen, 0)); assert!(!mark_unique_expected_reply(&expected, &mut seen, 2)); + // An 0xA4 joint is budgeted two replies (command echo + 0x92 read) + // and is only complete once both are in. assert!(mark_unique_expected_reply(&expected, &mut seen, 1)); - assert_eq!(seen, [true, true, false]); + assert!(!reply_complete(&expected, &seen, 1)); + assert!(mark_unique_expected_reply(&expected, &mut seen, 1)); + assert!(reply_complete(&expected, &seen, 1)); + assert!(!mark_unique_expected_reply(&expected, &mut seen, 1)); + assert!(reply_complete(&expected, &seen, 0)); + assert!(!reply_complete(&expected, &seen, 2)); + assert_eq!(seen, [1, 2, 0]); } #[test] @@ -1175,6 +1792,420 @@ mod tests { assert_eq!(consecutive.record(LATE, PERIOD), TimingVerdict::Degraded); } + #[test] + fn bus_busy_percentiles_report_median_tail_and_peak() { + let mut busy = vec![0.5, 0.7, 0.6, 0.9, 0.55, 0.65, 0.6, 0.62, 0.58, 0.61, 0.95]; + let (p50, p95, max) = bus_busy_percentiles(&mut busy); + assert_eq!(p50, 0.61); + assert_eq!(p95, 0.95); + assert_eq!(max, 0.95); + let (a, b, c) = bus_busy_percentiles(&mut []); + assert!(a.is_nan() && b.is_nan() && c.is_nan()); + } + + #[test] + fn a4_joints_stay_on_the_position_frame_through_holds_but_not_limp() { + use crate::bringup::{Vendor, WireMode}; + // Tracked and holding (kp > 0) both take 0xA4 on an a4 MyActuator … + assert!(a4_wire(Vendor::MyActuator, WireMode::A4, true, 250.0)); + assert!(a4_wire(Vendor::MyActuator, WireMode::A4, false, 250.0)); + // … limp / gravity comp (kp = 0) fall back to MIT for compliance … + assert!(!a4_wire(Vendor::MyActuator, WireMode::A4, false, 0.0)); + // … and nothing else ever does, whatever the tick. + assert!(!a4_wire(Vendor::MyActuator, WireMode::Mit, true, 250.0)); + assert!(!a4_wire(Vendor::Damiao, WireMode::A4, true, 250.0)); + } + + #[test] + fn runaway_guard_trips_on_sustained_fast_motion_away_from_the_target() { + let d = |deg: f64| deg.to_radians(); + let mut g = RunawayGuard::default(); + // Shoulder_1 on a4, 2026-09-23: 8° past its target and accelerating + // away — trips on the third consecutive sample, not before. + assert!(!g.check(d(8.0), d(60.0), 0.0)); + assert!(!g.check(d(8.5), d(80.0), 0.0)); + assert!(g.check(d(9.0), d(100.0), 0.0)); + // Same in the negative direction. + let mut g = RunawayGuard::default(); + for _ in 0..A4_RUNAWAY_SAMPLES - 1 { + assert!(!g.check(d(-6.0), d(-40.0), 0.0)); + } + assert!(g.check(d(-6.0), d(-40.0), 0.0)); + } + + #[test] + fn runaway_guard_ignores_lag_slow_drift_and_single_samples() { + let d = |deg: f64| deg.to_radians(); + let mut g = RunawayGuard::default(); + for _ in 0..10 { + // Lagging a fast move: far behind but moving *toward* the target. + assert!(!g.check(d(-10.0), d(90.0), 0.0)); + // Past the target but slow (a settling overshoot, a hand's push). + assert!(!g.check(d(7.0), d(10.0), 0.0)); + // Fast away but inside the normal-lag band. + assert!(!g.check(d(2.0), d(90.0), 0.0)); + } + // An isolated bad reply between good ones never accumulates. + for _ in 0..10 { + assert!(!g.check(d(8.0), d(90.0), 0.0)); + assert!(!g.check(d(0.5), d(5.0), 0.0)); + } + // reset() (a tick off the position frame) clears the count. + g.check(d(8.0), d(90.0), 0.0); + g.check(d(8.0), d(90.0), 0.0); + g.reset(); + assert!(!g.check(d(8.0), d(90.0), 0.0)); + } + + #[test] + fn pv_joints_follow_the_same_hold_rule_on_damiao_only() { + use crate::bringup::{Vendor, WireMode}; + assert!(pv_wire(Vendor::Damiao, WireMode::Pv, true, 130.0)); + assert!(pv_wire(Vendor::Damiao, WireMode::Pv, false, 130.0)); + assert!(!pv_wire(Vendor::Damiao, WireMode::Pv, false, 0.0)); + assert!(!pv_wire(Vendor::Damiao, WireMode::Mit, true, 130.0)); + assert!(!pv_wire(Vendor::MyActuator, WireMode::Pv, true, 130.0)); + assert_eq!(WireMode::Pv.dm_mode(), proto::DM_MODE_POS_VEL); + assert_eq!(WireMode::Mit.dm_mode(), proto::DM_MODE_MIT); + } + + fn ready(id: u8, vendor: Vendor, wire: WireMode) -> ReadyMotor { + ReadyMotor { + id, + joint: format!("m{id}"), + vendor, + ranges: proto::MitRanges { + p_max: 12.5, + v_max: 30.0, + kp_max: 500.0, + kd_max: 5.0, + t_max: 10.0, + }, + hold_pos: 0.0, + holding: false, + kp: 100.0, + kd: 1.0, + gripper: id == 8, + slot: id as usize - 1, + max_vel: 9.4, + max_accel: 33.0, + fc: 0.0, + k: 0.0, + fv: 0.0, + fo: 0.0, + stiction_gain: 0.0, + stiction_err: 0.0, + stiction_load_gain: 0.0, + dither_nm: 0.0, + dither_hz: 0.0, + wire, + stribeck_gain: 0.0, + stribeck_dfs: 0.0, + stribeck_load_gain: 0.0, + stribeck_vs: 0.0, + fl: 0.0, + cap_track: 0.0, + lead_s: 0.0, + fw_version: None, + tf_nm_per_pct: 0.0, + mit_hz: 0.0, + cogging: Vec::new(), + } + } + + fn full_arm_position_controller() -> Vec { + let mut v: Vec = (1..=5) + .map(|id| ready(id, Vendor::MyActuator, WireMode::A4)) + .collect(); + v.push(ready(6, Vendor::Damiao, WireMode::Pv)); + v.push(ready(7, Vendor::Damiao, WireMode::Pv)); + v.push(ready(8, Vendor::Damiao, WireMode::Mit)); + v + } + + #[test] + fn thinning_is_off_at_240_hz() { + let motors = full_arm_position_controller(); + let sched = Thinning::plan(&motors, 240.0, IMPEDANCE_HZ); + assert!(!sched.enabled); + for tick in 0..20 { + for idx in 0..motors.len() { + assert!(sched.commanded(idx, tick)); + assert!(sched.a4_read(idx, tick)); + } + } + } + + #[test] + fn thinning_at_400_hz_is_a_fixed_fourteen_frame_tick() { + let motors = full_arm_position_controller(); + let sched = Thinning::plan(&motors, 400.0, IMPEDANCE_HZ); + assert!(sched.enabled); + let mut wrist_turns = [0u32; 2]; + let mut gripper_turns = 0u32; + let mut reads = [0u32; 5]; + for tick in 0..60u64 { + // Frames this tick: 2 per MyActuator command (echo), 2 per 0x92 + // read, 2 per Damiao command. + let mut frames = 0; + for idx in 0..motors.len() { + if !sched.commanded(idx, tick) { + continue; + } + frames += 2; + match idx { + 0..=4 => { + if sched.a4_read(idx, tick) { + frames += 2; + reads[idx] += 1; + } + } + 5 | 6 => wrist_turns[idx - 5] += 1, + _ => gripper_turns += 1, + } + } + assert_eq!(frames, 14, "tick {tick}"); + // MyActuator joints are never thinned. + for idx in 0..5 { + assert!(sched.commanded(idx, tick)); + } + } + // Two wrists alternate: 200 Hz each. Five reads and the gripper share + // the other lane: 400/6 Hz each. + assert_eq!(wrist_turns, [30, 30]); + assert_eq!(gripper_turns, 10); + assert_eq!(reads, [10; 5]); + } + + #[test] + fn thinning_only_reads_a4_joints_and_puts_mit_joints_on_their_own_lane() { + let motors = vec![ + ready(1, Vendor::MyActuator, WireMode::Mit), + ready(2, Vendor::MyActuator, WireMode::A4), + ready(6, Vendor::Damiao, WireMode::Pv), + ]; + let sched = Thinning::plan(&motors, MIXED_LOOP_HZ, IMPEDANCE_HZ); + assert_eq!(sched.read_lane, vec![1]); + assert_eq!(sched.dm_lane, vec![2]); + assert_eq!(sched.mit_lane, vec![(0, 0)]); + for tick in 0..8 { + // The impedance joint alternates (240 Hz); the a4 joint and the + // only pv wrist go every tick, and the a4 joint is read every tick. + assert_eq!(sched.commanded(0, tick), tick % 2 == 0); + assert!(sched.commanded(1, tick)); + assert!(sched.commanded(2, tick)); + assert!(sched.a4_read(1, tick)); + } + } + + /// shoulder_1 + elbow on 0xA4, the rest on impedance: the split that + /// shook the arm when it all ran at 400 Hz. + fn mixed_arm() -> Vec { + vec![ + ready(1, Vendor::MyActuator, WireMode::A4), + ready(2, Vendor::MyActuator, WireMode::Mit), + ready(3, Vendor::MyActuator, WireMode::Mit), + ready(4, Vendor::MyActuator, WireMode::A4), + ready(5, Vendor::MyActuator, WireMode::Mit), + ready(6, Vendor::Damiao, WireMode::Mit), + ready(7, Vendor::Damiao, WireMode::Mit), + ready(8, Vendor::Damiao, WireMode::Mit), + ] + } + + #[test] + fn a_mixed_bus_commands_every_impedance_joint_at_exactly_240_hz() { + let motors = mixed_arm(); + let sched = Thinning::plan(&motors, MIXED_LOOP_HZ, IMPEDANCE_HZ); + assert!(sched.enabled); + assert_eq!(sched.mit_div, 2); + // The wrists are impedance joints here, so they ride the impedance + // lane rather than taking turns with each other. + assert!(sched.dm_lane.is_empty()); + let mit: Vec = sched.mit_lane.iter().map(|(i, _)| *i).collect(); + assert_eq!(mit, vec![1, 2, 4, 5, 6]); + let mut last: [Option; 8] = [None; 8]; + let mut worst_frames = 0; + for tick in 0..240u64 { + let mut frames = 0; + let mut mit_this_tick = 0; + for idx in 0..motors.len() { + if !sched.commanded(idx, tick) { + continue; + } + frames += 2; + if let Some(prev) = last[idx] { + if sched.mit_phase(idx).is_some() { + // Evenly spaced: exactly every other tick, never 1 or 3. + assert_eq!(tick - prev, 2, "motor {idx}"); + } + } + last[idx] = Some(tick); + if sched.mit_phase(idx).is_some() { + mit_this_tick += 1; + } + if motors[idx].wire == WireMode::A4 { + assert_eq!(tick - last[idx].unwrap(), 0); + if sched.a4_read(idx, tick) { + frames += 2; + } + } + } + // Five impedance joints split 3 / 2 across the two phases. + assert!(mit_this_tick == 2 || mit_this_tick == 3); + // The 0xA4 joints go every tick. + assert!(sched.commanded(0, tick) && sched.commanded(3, tick)); + worst_frames = worst_frames.max(frames); + } + // Fits the 480 Hz tick: 2.08 ms less the reply guard at ~0.13 ms a + // frame is ~14 frames. + assert!(worst_frames <= 14, "{worst_frames} frames"); + } + + #[test] + fn planner_joints_ride_the_half_rate_lane_and_read_every_own_command() { + // shoulder_1 on direct tracking, the elbow on the planner, the rest + // on impedance: the elbow alternates with the impedance joints. + let mut motors = mixed_arm(); + motors[3].cap_track = 1.2; // elbow + let sched = Thinning::plan(&motors, MIXED_LOOP_HZ, IMPEDANCE_HZ); + assert!(sched.mit_phase(3).is_some()); + assert!(sched.mit_phase(0).is_none()); // shoulder_1: every tick + assert_eq!(sched.read_lane, vec![0, 7]); // shoulder_1 + gripper only + let mut worst = 0; + let mut last = None; + for tick in 0..96u64 { + assert!(sched.commanded(0, tick)); + let mut frames = 0; + for idx in 0..motors.len() { + if !sched.commanded(idx, tick) { + continue; + } + frames += 2; + if motors[idx].wire == WireMode::A4 && sched.a4_read(idx, tick) { + frames += 2; + } + } + if sched.commanded(3, tick) { + // Every elbow command carries its fine-position read. + assert!(sched.a4_read(3, tick)); + if let Some(prev) = last { + assert_eq!(tick - prev, 2); // 240 Hz, evenly spaced + } + last = Some(tick); + } + worst = worst.max(frames); + } + assert!(worst <= 14, "{worst} frames"); + // Direct-tracking a4 joints stay out of the lane, as before. + assert!(Thinning::plan(&mixed_arm(), MIXED_LOOP_HZ, IMPEDANCE_HZ) + .mit_phase(3) + .is_none()); + } + + #[test] + fn the_position_controller_has_no_impedance_lane() { + let sched = Thinning::plan(&full_arm_position_controller(), 400.0, IMPEDANCE_HZ); + assert!(sched.mit_lane.is_empty()); + assert_eq!(sched.mit_div, 1); + // An all-impedance arm at 240 Hz is not thinned at all. + let all_mit: Vec = (1..=7) + .map(|id| { + ready( + id, + if id <= 5 { + Vendor::MyActuator + } else { + Vendor::Damiao + }, + WireMode::Mit, + ) + }) + .collect(); + let sched = Thinning::plan(&all_mit, IMPEDANCE_HZ, IMPEDANCE_HZ); + assert!(!sched.enabled && sched.mit_lane.is_empty()); + assert!((0..7).all(|i| sched.commanded(i, 1))); + } + + #[test] + fn impedance_joints_run_at_240_hz_only() { + let spec = |wire: &str, gripper: bool| { + let text = if gripper { + "proto 16\ngripper 0 canL 8\n".to_string() + } else { + format!( + "proto 16\njoint 0 canL shoulder_1 1 250 3.5 9.4 33.0 0.6 250 0.15 0.02 0 0 0 0 60 {wire} 0 0.3 0.1 0.1 0 20\n" + ) + }; + text + }; + for hz in [240.0, 480.0] { + assert!(parse_config(&format!("loop_hz {hz}\n{}", spec("mit", false))).is_ok()); + } + for hz in [200.0, 300.0, 400.0, 960.0] { + let err = parse_config(&format!("loop_hz {hz}\n{}", spec("mit", false))) + .err() + .expect("refused") + .to_string(); + assert!(err.contains("240 Hz only"), "{err}"); + } + // Firmware-loop joints, and the always-MIT gripper, are not held to it. + assert!(parse_config(&format!("loop_hz 400\n{}", spec("a4", false))).is_ok()); + assert!(parse_config(&format!("loop_hz 400\n{}", spec("", true))).is_ok()); + } + + #[test] + fn a4_cap_tracks_commanded_speed_only_with_the_planner_on() { + let max_vel = 9.4; // rad/s: the tracker limit, ~539 dps + // Direct tracking: always the fixed limit, whatever the speed. + assert_eq!(a4_speed_cap(0.0, 0.1, max_vel), max_vel.to_degrees()); + // Planner: 1.2x the commanded speed, sign-independent ... + let v = 3f64.to_radians(); + assert!((a4_speed_cap(1.2, v, max_vel) - 3.6).abs() < 1e-9); + assert!((a4_speed_cap(1.2, -v, max_vel) - 3.6).abs() < 1e-9); + // ... never below the floor (a hold still corrects) ... + assert_eq!(a4_speed_cap(1.2, 0.0, max_vel), A4_CAP_FLOOR_DPS); + // ... nor above the tracker limit. + assert_eq!(a4_speed_cap(1.2, 100.0, max_vel), max_vel.to_degrees()); + } + + #[test] + fn a4_target_leads_along_the_tracker_velocity() { + assert_eq!(a4_target(1.0, 2.0, 0.0), 1.0); + assert!((a4_target(1.0, 2.0, 0.005) - 1.01).abs() < 1e-12); + assert!((a4_target(1.0, -2.0, 0.005) - 0.99).abs() < 1e-12); + let line = "joint 0 canL elbow 4 130 5.0 9.4 33.0 0 0 0 0 0 0 0 0 60 a4 0 0.3 0.1 0.1 0 20"; + let cfg = parse_config(&format!( + "proto {CONFIG_PROTO}\nloop_hz 400\n{line} 1.05 5\n" + )) + .unwrap(); + assert_eq!(cfg.buses[0].2[0].lead_s, 0.005); + let cfg = + parse_config(&format!("proto {CONFIG_PROTO}\nloop_hz 400\n{line} 1.05\n")).unwrap(); + assert_eq!(cfg.buses[0].2[0].lead_s, 0.0); + } + + #[test] + fn a4_cap_track_is_an_optional_trailing_joint_field() { + let line = "joint 0 canL shoulder_1 1 250 3.5 9.4 33.0 0.6 250 0.15 0.02 0 0 0 0 60 a4 0 0.3 0.1 0.1 0 20"; + let cfg = + parse_config(&format!("proto {CONFIG_PROTO}\nloop_hz 400\n{line} 1.2\n")).unwrap(); + assert_eq!(cfg.buses[0].2[0].cap_track, 1.2); + let cfg = parse_config(&format!("proto {CONFIG_PROTO}\nloop_hz 400\n{line}\n")).unwrap(); + assert_eq!(cfg.buses[0].2[0].cap_track, 0.0); + } + + #[test] + fn a4_carry_follows_the_echo_speed_and_holds_at_rest() { + let t0 = Instant::now(); + let t1 = t0 + Duration::from_millis(10); + let p = a4_extrapolate((1.0, t0), 0.5, t1); + assert!((p - 1.005).abs() < 1e-9); + assert_eq!(a4_extrapolate((1.0, t0), 0.0, t1), 1.0); + // A clock that has not advanced (or ran backwards) adds nothing. + assert_eq!(a4_extrapolate((1.0, t1), 3.0, t0), 1.0); + } + #[test] fn timing_health_isolated_overrun_degrades_not_limps() { // The field record: one 60 ms stall in an otherwise perfect stream. @@ -1340,12 +2371,12 @@ mod tests { #[test] fn parse_config_assigns_slots() { let cfg = parse_config( - "proto 2\n\ + "proto 16\n\ loop_hz 240\n\ - joint 0 canL shoulder_1 1 250 3.5 9.4 33.0 0.6 250 0.15 0.02\n\ - joint 0 canL shoulder_2 2 250 3.5 9.4 33.0 0.5 250 0.10 0.0\n\ + joint 0 canL shoulder_1 1 250 3.5 9.4 33.0 0.6 250 0.15 0.02 0 0 0 0 60 mit 0 0.3 0.1 0.1 0 20\n\ + joint 0 canL shoulder_2 2 250 3.5 9.4 33.0 0.5 250 0.10 0.0 0 0 0 0 60 mit 0 0.3 0.1 0.1 0 20\n\ gripper 0 canL 8\n\ - joint 0 canL shoulder_3 3 180 2.0 9.4 33.0 0.4 250 0.08 0.0\n", + joint 0 canL shoulder_3 3 180 2.0 9.4 33.0 0.4 250 0.08 0.0 0.6 0.0017 0.2 1.5 60 a4 0.7 0.3 0.1 0.1 0.08 40\n", ) .unwrap(); let specs = &cfg.buses[0].2; @@ -1360,9 +2391,73 @@ mod tests { (specs[0].fc, specs[0].k, specs[0].fv, specs[0].fo), (0.6, 250.0, 0.15, 0.02) ); + assert_eq!((specs[0].stiction_gain, specs[0].stiction_err), (0.0, 0.0)); + assert_eq!( + (specs[3].stiction_gain, specs[3].stiction_err), + (0.6, 0.0017) + ); + assert_eq!( + (specs[0].stiction_load_gain, specs[3].stiction_load_gain), + (0.0, 0.2) + ); + assert_eq!((specs[0].dither_nm, specs[0].dither_hz), (0.0, 60.0)); + assert_eq!((specs[3].dither_nm, specs[3].dither_hz), (1.5, 60.0)); + assert_eq!( + (specs[0].wire, specs[3].wire), + (WireMode::Mit, WireMode::A4) + ); + assert_eq!(specs[0].stribeck_gain, 0.0); + assert_eq!( + ( + specs[3].stribeck_gain, + specs[3].stribeck_dfs, + specs[3].stribeck_load_gain, + specs[3].stribeck_vs + ), + (0.7, 0.3, 0.1, 0.1) + ); + assert_eq!((specs[0].fl, specs[3].fl), (0.0, 0.08)); + assert_eq!( + (specs[0].stribeck_pole, specs[3].stribeck_pole), + (20.0, 40.0) + ); + // An unknown wire token is a bad line, not a silent MIT. + assert!(parse_config( + "proto 16\njoint 0 canL shoulder_1 1 250 3.5 9.4 33.0 0.6 250 0.15 0.02 0 0 0 0 60 a9 0 0.3 0.1 0.1 0 20\n" + ) + .is_err()); // A joint line missing the tracker/friction params (the previous // 7-field layout) must be rejected, not defaulted. - assert!(parse_config("proto 2\njoint 0 canL shoulder_1 1 250 3.5\n").is_err()); + assert!(parse_config("proto 16\njoint 0 canL shoulder_1 1 250 3.5\n").is_err()); + // ... and so must the proto-2 … 8 layouts (13 … 24 fields). + assert!(parse_config( + "proto 16\njoint 0 canL shoulder_1 1 250 3.5 9.4 33.0 0.6 250 0.15 0.02\n" + ) + .is_err()); + assert!(parse_config( + "proto 16\njoint 0 canL shoulder_1 1 250 3.5 9.4 33.0 0.6 250 0.15 0.02 0 0\n" + ) + .is_err()); + assert!(parse_config( + "proto 16\njoint 0 canL shoulder_1 1 250 3.5 9.4 33.0 0.6 250 0.15 0.02 0 0 0\n" + ) + .is_err()); + assert!(parse_config( + "proto 16\njoint 0 canL shoulder_1 1 250 3.5 9.4 33.0 0.6 250 0.15 0.02 0 0 0 0 60\n" + ) + .is_err()); + assert!(parse_config( + "proto 16\njoint 0 canL shoulder_1 1 250 3.5 9.4 33.0 0.6 250 0.15 0.02 0 0 0 0 60 mit\n" + ) + .is_err()); + assert!(parse_config( + "proto 16\njoint 0 canL shoulder_1 1 250 3.5 9.4 33.0 0.6 250 0.15 0.02 0 0 0 0 60 mit 0 0.3 0.1 0.1\n" + ) + .is_err()); + assert!(parse_config( + "proto 16\njoint 0 canL shoulder_1 1 250 3.5 9.4 33.0 0.6 250 0.15 0.02 0 0 0 0 60 mit 0 0.3 0.1 0.1 0\n" + ) + .is_err()); } /// A bus carrying only some of the arm joints (a bench wrist assembly) @@ -1371,9 +2466,9 @@ mod tests { #[test] fn parse_config_subset_keeps_joint_slots() { let cfg = parse_config( - "proto 2\n\ - joint 0 can0 wrist_2 6 40 1.0 9.4 33.0 0.0 0.0 0.0 0.0\n\ - joint 0 can0 wrist_3 7 40 1.0 9.4 33.0 0.0 0.0 0.0 0.0\n\ + "proto 16\n\ + joint 0 can0 wrist_2 6 40 1.0 9.4 33.0 0.0 0.0 0.0 0.0 0 0 0 0 60 mit 0 0.3 0.1 0.1 0 20\n\ + joint 0 can0 wrist_3 7 40 1.0 9.4 33.0 0.0 0.0 0.0 0.0 0 0 0 0 60 mit 0 0.3 0.1 0.1 0 20\n\ gripper 0 can0 8\n", ) .unwrap(); @@ -1384,12 +2479,18 @@ mod tests { ); // Arm joint ids outside 1..=7 have no slot; a repeated id would // double-book one. - assert!(parse_config("proto 2\njoint 0 can0 wrist_3 8 40 1.0 9.4 33.0 0 0 0 0\n").is_err()); - assert!(parse_config("proto 2\njoint 0 can0 bogus 0 40 1.0 9.4 33.0 0 0 0 0\n").is_err()); assert!(parse_config( - "proto 2\n\ - joint 0 can0 wrist_2 6 40 1.0 9.4 33.0 0 0 0 0\n\ - joint 0 can0 wrist_2 6 40 1.0 9.4 33.0 0 0 0 0\n" + "proto 16\njoint 0 can0 wrist_3 8 40 1.0 9.4 33.0 0 0 0 0 0 0 0 0 60 mit 0 0.3 0.1 0.1 0 20\n" + ) + .is_err()); + assert!(parse_config( + "proto 16\njoint 0 can0 bogus 0 40 1.0 9.4 33.0 0 0 0 0 0 0 0 0 60 mit 0 0.3 0.1 0.1 0 20\n" + ) + .is_err()); + assert!(parse_config( + "proto 16\n\ + joint 0 can0 wrist_2 6 40 1.0 9.4 33.0 0 0 0 0 0 0 0 0 60 mit 0 0.3 0.1 0.1 0 20\n\ + joint 0 can0 wrist_2 6 40 1.0 9.4 33.0 0 0 0 0 0 0 0 0 60 mit 0 0.3 0.1 0.1 0 20\n" ) .is_err()); } @@ -1400,7 +2501,8 @@ mod tests { /// target on the max-step gate — the arms enabled and never moved. #[test] fn parse_config_requires_matching_proto() { - let joint = "joint 0 can0 wrist_2 6 40 1.0 9.4 33.0 0 0 0 0\n"; + let joint = + "joint 0 can0 wrist_2 6 40 1.0 9.4 33.0 0 0 0 0 0 0 0 0 60 mit 0 0.3 0.1 0.1 0 20\n"; let error_of = |text: &str| match parse_config(text) { Ok(_) => panic!("accepted a skewed config: {text:?}"), Err(err) => err.to_string(), @@ -1410,14 +2512,185 @@ mod tests { assert!(err.contains("no `proto` line"), "{err}"); assert!(err.contains("axol rt.install"), "{err}"); // A future client generation this core does not understand. - let err = error_of(&format!("proto 3\n{joint}")); - assert!(err.contains("proto 3"), "{err}"); - assert!(err.contains("proto 2"), "{err}"); + let err = error_of(&format!("proto 99\n{joint}")); + assert!(err.contains("proto 99"), "{err}"); + assert!(err.contains("proto 16"), "{err}"); // Malformed declarations are bad lines, not silently accepted. assert!(parse_config(&format!("proto\n{joint}")).is_err()); assert!(parse_config(&format!("proto two\n{joint}")).is_err()); // Order does not matter; the line just has to be there. - assert!(parse_config(&format!("{joint}proto 2\n")).is_ok()); + assert!(parse_config(&format!("{joint}proto 16\n")).is_ok()); + } + + const S1_A4: &str = "joint 1 canR shoulder_1 1 250 3.5 9.4 33.0 0.6 250 0.15 0.02 0 0 0 0 60 a4 0 0.3 0.1 0.1 0 20"; + + /// The 0x73 scale is an optional field after `lead_ms`; `cogging` lines + /// attach their harmonics to the joint already declared on that bus. + #[test] + fn parse_config_takes_tf_scale_and_cogging_series() { + let cfg = parse_config(&format!( + "proto 16\nloop_hz 480\n{S1_A4} 0 0 0.24\n\ + joint 1 canR elbow 4 130 5 9.4 33.0 0 0 0 0 0 0 0 0 60 mit 0 0.3 0.1 0.1 0 20\n\ + cogging 1 canR 1 2 198.9 0.3 -0.1 397.8 0 0.2\n" + )) + .unwrap(); + let specs = &cfg.buses[0].2; + assert_eq!(specs[0].tf_nm_per_pct, 0.24); + assert_eq!(specs[1].tf_nm_per_pct, 0.0); + assert_eq!( + specs[0].cogging, + vec![ + filter::CogTerm { + w: 198.9, + a: 0.3, + b: -0.1 + }, + filter::CogTerm { + w: 397.8, + a: 0.0, + b: 0.2 + }, + ] + ); + assert!(specs[1].cogging.is_empty()); + // A cogging line for a joint the bus does not carry, a count that + // does not match its terms, or a non-finite coefficient is refused. + for bad in [ + "cogging 1 canR 3 1 198.9 0.3 0\n", + "cogging 1 canR 1 2 198.9 0.3 0\n", + "cogging 1 canR 1 1 198.9 NaN 0\n", + "cogging 0 canR 1 1 198.9 0.3 0\n", + ] { + assert!( + parse_config(&format!("proto 16\nloop_hz 480\n{S1_A4}\n{bad}")).is_err(), + "{bad}" + ); + } + } + + /// `impedance_hz 480` runs the MyActuator impedance joints at a 480 Hz + /// loop and nothing else; 240 keeps the verified rules; any other value + /// is refused. + #[test] + fn fast_impedance_needs_a_480_hz_loop() { + let mit = "joint 0 canL shoulder_1 1 250 3.5 9.4 33.0 0.6 250 0.15 0.02 0 0 0 0 60 mit 0 0.3 0.1 0.1 0 20\n"; + let cfg = parse_config(&format!("proto 16\nloop_hz 480\nimpedance_hz 480\n{mit}")).unwrap(); + assert_eq!(cfg.impedance_hz, FAST_IMPEDANCE_HZ); + assert_eq!( + parse_config(&format!("proto 16\n{mit}")) + .unwrap() + .impedance_hz, + IMPEDANCE_HZ + ); + let err = parse_config(&format!("proto 16\nloop_hz 240\nimpedance_hz 480\n{mit}")) + .err() + .expect("refused") + .to_string(); + assert!(err.contains("480 Hz only"), "{err}"); + let err = parse_config(&format!("proto 16\nloop_hz 480\nimpedance_hz 400\n{mit}")) + .err() + .expect("refused") + .to_string(); + assert!(err.contains("impedance_hz 400"), "{err}"); + // A bus with no impedance joint is not held to the rule. + assert!(parse_config(&format!( + "proto 16\nloop_hz 400\nimpedance_hz 480\n{S1_A4}\n" + )) + .is_ok()); + } + + /// Per-joint fast impedance: shoulder_1 and the elbow at 480 Hz (joint + /// field 28), every other impedance joint on the 240 Hz lane, the loop + /// held to 480 by the rule. + #[test] + fn single_joints_can_run_impedance_at_480() { + let line = |name: &str, id: u8, hz: f64| { + format!( + "joint 1 canR {name} {id} 250 3.5 9.4 33.0 0 0 0 0 0 0 0 0 60 mit 0 0.3 0.1 0.1 0 20 0 0 0 {hz}\n" + ) + }; + let text = format!( + "proto 16\nloop_hz 480\n{}{}{}{}", + line("shoulder_1", 1, 480.0), + line("shoulder_2", 2, 0.0), + line("elbow", 4, 480.0), + line("wrist_2", 6, 0.0), + ); + let cfg = parse_config(&text).unwrap(); + let specs = &cfg.buses[0].2; + assert_eq!( + specs.iter().map(|s| s.mit_hz).collect::>(), + vec![480.0, 0.0, 480.0, 0.0] + ); + // The rule: a 480 Hz joint needs the 480 Hz loop. + let err = parse_config(&text.replace("loop_hz 480", "loop_hz 240")) + .err() + .expect("refused") + .to_string(); + assert!(err.contains("shoulder_1 runs impedance at 480"), "{err}"); + assert!(parse_config(&text.replace(" 480\n", " 400\n")).is_err()); + // The schedule: shoulder_1 (0) and the elbow (2) every tick, the rest + // on the lane. + let mut motors = vec![ + ready(1, Vendor::MyActuator, WireMode::Mit), + ready(2, Vendor::MyActuator, WireMode::Mit), + ready(4, Vendor::MyActuator, WireMode::Mit), + ready(6, Vendor::Damiao, WireMode::Mit), + ]; + motors[0].mit_hz = 480.0; + motors[2].mit_hz = 480.0; + let sched = Thinning::plan(&motors, FAST_IMPEDANCE_HZ, IMPEDANCE_HZ); + assert_eq!(sched.mit_lane, vec![(1, 0), (3, 1)]); + for tick in 0..8u64 { + assert!(sched.commanded(0, tick) && sched.commanded(2, tick)); + assert_ne!(sched.commanded(1, tick), sched.commanded(3, tick)); + } + assert_eq!(sched.read_div, FAST_MIT_READ_DIV); + // Without a per-joint rate nothing is fast at the default 240. + motors[0].mit_hz = 0.0; + motors[2].mit_hz = 0.0; + let slow = Thinning::plan(&motors, MIXED_LOOP_HZ, IMPEDANCE_HZ); + assert_eq!(slow.mit_lane.len(), 4); + assert_eq!(slow.read_div, 1); + } + + /// Fast impedance: the MyActuator impedance joints go every tick, the + /// Damiao wrists alternate at 240 Hz, and the read lane (gripper, a4 + /// reads) takes one tick in `FAST_MIT_READ_DIV` — 12 frames a tick, 14 + /// on the read ticks. + #[test] + fn fast_impedance_commands_myactuator_every_tick_and_wrists_at_240() { + let mut motors: Vec = (1..=4) + .map(|id| ready(id, Vendor::MyActuator, WireMode::Mit)) + .collect(); + motors.push(ready(5, Vendor::MyActuator, WireMode::A4)); + motors.push(ready(6, Vendor::Damiao, WireMode::Mit)); + motors.push(ready(7, Vendor::Damiao, WireMode::Mit)); + motors.push(ready(8, Vendor::Damiao, WireMode::Mit)); + let sched = Thinning::plan(&motors, FAST_IMPEDANCE_HZ, FAST_IMPEDANCE_HZ); + assert_eq!(sched.mit_div, 2); + assert_eq!(sched.read_div, FAST_MIT_READ_DIV); + assert_eq!(sched.mit_lane, vec![(5, 0), (6, 1)]); + let mut gripper_ticks = 0; + let mut reads = 0; + for tick in 0..480u64 { + for idx in 0..5 { + assert!(sched.commanded(idx, tick), "MyActuator {idx} tick {tick}"); + } + // Exactly one wrist per tick, each on alternate ticks. + assert_ne!(sched.commanded(5, tick), sched.commanded(6, tick)); + gripper_ticks += sched.commanded(7, tick) as u32; + reads += sched.a4_read(4, tick) as u32; + // Never more than one read-lane pair on a tick. + assert!(!(sched.commanded(7, tick) && sched.a4_read(4, tick))); + } + // 480 / 8 = 60 turns a second, shared by the gripper and one a4 read. + assert_eq!(gripper_ticks + reads, 60); + assert_eq!(gripper_ticks, 30); + // The default rate keeps every impedance joint on the lane. + let slow = Thinning::plan(&motors, MIXED_LOOP_HZ, IMPEDANCE_HZ); + assert_eq!(slow.read_div, 1); + assert_eq!(slow.mit_lane.len(), 6); } } @@ -1919,20 +3192,43 @@ fn bus_loop( v_meas: LpDiff, bp: BandPass, vel_meas: f64, + /// Slower (20 rad/s) measured-velocity estimate for the Stribeck + /// term: smooth enough at 0.05 rad/s that the feedforward does not + /// step with the encoder, ~30° behind at the 2 Hz cycle. + v_meas_slow: LpDiff, + vel_meas_slow: f64, last_fb: Option, + /// Torque-dither oscillator phase (`filter::dither_step`), started a + /// golden angle apart per slot. + dither_phase: f64, } let mut damp: Vec = (0..N_SLOTS) - .map(|_| Damp { + .map(|slot| Damp { v_cmd: LpDiff::new(CONTROL_CUTOFF), a_cmd: LpDiff::new(CONTROL_CUTOFF), v_cmd_fast: LpDiff::new(VEL_CUTOFF), v_meas: LpDiff::new(VEL_CUTOFF), bp: BandPass::new(), vel_meas: 0.0, + v_meas_slow: LpDiff::new( + specs + .iter() + .find(|s| s.slot == slot && !s.gripper && s.stribeck_pole > 0.0) + .map(|s| s.stribeck_pole) + .unwrap_or(CONTROL_CUTOFF), + ), + vel_meas_slow: 0.0, last_fb: None, + dither_phase: slot as f64 * filter::DITHER_PHASE_STAGGER, }) .collect(); let mut prev_tick: Option = None; + // Impedance joints on their own 240 Hz cadence (`Thinning::mit_lane`): + // when each last ran, for a time step and overrun of its own. + let mut own_prev: [Option; N_SLOTS] = [None; N_SLOTS]; + // 0x73 joints: the torque feedforward's fade-in (`filter::tf_ramp_step`), + // 0 whenever a tick goes out without it (limp, gravity comp). + let mut tf_ramp = [0.0f64; N_SLOTS]; // Latest decoded feedback per slot, shipped to Python once per tick as // an `F` packet — the core is the only CAN consumer; Python fills its // Motor caches from these instead of passively reading the bus. @@ -1961,6 +3257,13 @@ fn bus_loop( let mut ticks: u64 = 0; let mut watchdog_frozen = false; let mut next_stats = Instant::now() + Duration::from_secs(5); + // Bus occupancy per tick: tick start → last reply received, as a + // fraction of the period. This is what bounds the loop rate — at 240 Hz + // with three a4 joints (two request/reply pairs each) the right arm is + // estimated near three quarters of a 1 Mbps bus — so it is reported in + // the 5 s stats line to size any rate change against a measurement. + let mut bus_busy: Vec = Vec::with_capacity(2048); + let mut bus_last_reply: Option = None; // TX-stall (e-stop) tracking — see `guarded_send`. A dead bus skips the // motor disable on the way out (nothing is powered to hear it, and the // freshly purged queue should stay empty). @@ -1983,8 +3286,135 @@ fn bus_loop( let mut stall_probe = stall::StallProbe::open(); // Per-tick reply bookkeeping, allocated once: the loop must not grow // the heap (a fresh page is a fault, see `stall::lock_memory`). - let mut expected = vec![false; motors.len()]; - let mut seen = vec![false; motors.len()]; + let mut expected = vec![0u8; motors.len()]; + let mut seen = vec![0u8; motors.len()]; + // 0xA4 joints: the command reply arrives before the 0x92 position read; + // its speed and iq are staged here until the fine position completes + // the sample. + let mut a4_stage: [(f64, f64); N_SLOTS] = [(0.0, 0.0); N_SLOTS]; + let mut a4_follow = vec![false; motors.len()]; + // Which motors this tick tried to command (a thinned motor's off-tick + // is not a missed reply; a dropped send still is). + let mut attempted = vec![false; motors.len()]; + // 0xA4 joints: the last 0.01° position and when it was taken, carried + // forward on the echo's speed between reads (`a4_extrapolate`). Seeded + // from bring-up's own 0x92 read so the first echo-only tick has a base. + let mut a4_anchor: [Option<(f64, Instant)>; N_SLOTS] = [None; N_SLOTS]; + // 0xA4 joints: the target this tick's position frame carried (None on + // a tick it went out as MIT), the runaway guard checking replies + // against it, and which joint (if any) tripped it. + let mut a4_sent: [Option; N_SLOTS] = [None; N_SLOTS]; + let mut runaway_guard = [RunawayGuard::default(); N_SLOTS]; + let mut ran_away = [false; N_SLOTS]; + for m in motors.iter() { + if m.vendor == Vendor::MyActuator && m.wire == WireMode::A4 { + a4_anchor[m.slot] = Some((m.hold_pos, Instant::now())); + } + } + // Damiao wrists: the control-mode register the motor is in (bring-up + // put it in the wire's mode). The frame the core wants can change + // (pv ↔ MIT across limp / gravity comp), and the register must follow + // it or the firmware ignores the frame. + let mut dm_mode: Vec = motors + .iter() + .map(|m| { + if m.vendor == Vendor::Damiao && !m.gripper { + m.wire.dm_mode() + } else { + 0 + } + }) + .collect(); + let sched = Thinning::plan(&motors, cfg.loop_hz, cfg.impedance_hz); + if !sched.mit_lane.is_empty() { + send_text( + out_tx, + b'L', + &format!( + "{iface}: {:.0} Hz loop — {} joint(s) on alternate ticks at {:.0} Hz each ({} impedance, {} on the 0xA4 planner), other firmware-loop joints every tick", + cfg.loop_hz, + sched.mit_lane.len(), + cfg.loop_hz / sched.mit_div as f64, + sched.mit_lane.iter().filter(|(i, _)| motors[*i].wire == WireMode::Mit).count(), + sched.mit_lane.iter().filter(|(i, _)| is_planner(&motors[*i])).count(), + ), + ); + } + if sched.read_div > 1 { + send_text( + out_tx, + b'L', + &format!( + "{iface}: fast impedance — {} every tick at {:.0} Hz; the other impedance joints at {:.0} Hz on alternate ticks; read lane one tick in {}", + motors + .iter() + .enumerate() + .filter(|(i, m)| !m.gripper + && m.wire == WireMode::Mit + && sched.mit_phase(*i).is_none()) + .map(|(_, m)| m.joint.as_str()) + .collect::>() + .join(", "), + cfg.loop_hz, + cfg.loop_hz / sched.mit_div as f64, + sched.read_div, + ), + ); + } + for (m, spec) in motors.iter().filter_map(|m| { + specs + .iter() + .find(|s| s.slot == m.slot && !s.gripper) + .map(|s| (m, s)) + }) { + if spec.tf_nm_per_pct > 0.0 && m.wire == WireMode::A4 { + let text = if m.tf_nm_per_pct > 0.0 { + format!( + "{iface}: {} on 0x73 — torque feedforward (gravity + inertia + cogging) at {:.4} Nm per % rated current, faded in over {:.1} s", + m.joint, + m.tf_nm_per_pct, + filter::TF_RAMP_S, + ) + } else { + format!( + "{iface}: {} stays on plain 0xA4 — 0x73 needs protocol V4.4 firmware (VersionDate {} or later), this motor reports {}", + m.joint, + proto::MA_FW_V44, + m.fw_version.map_or("no version".to_string(), |v| v.to_string()), + ) + }; + send_text(out_tx, b'L', &text); + } + if !m.cogging.is_empty() { + let cancels = m.wire == WireMode::Mit || m.tf_nm_per_pct > 0.0; + send_text( + out_tx, + b'L', + &format!( + "{iface}: {} cogging cancellation: {} harmonic(s){}", + m.joint, + m.cogging.len(), + if cancels { + "" + } else { + " — NOT applied: a plain-0xA4 joint takes no feedforward (needs 0x73)" + }, + ), + ); + } + } + if sched.enabled { + send_text( + out_tx, + b'L', + &format!( + "{iface}: {:.0} Hz loop — bus schedule thinned: {} wrist(s) take turns, {} read-lane entries (a4 position reads + gripper) take turns", + cfg.loop_hz, + sched.dm_lane.len(), + sched.read_lane.len(), + ), + ); + } // Belt-and-braces: sends on a dead bus normally fail fast with ENOBUFS, // but if the socket sndbuf fills first a blocking write would hang the // loop; the timeout turns that into EAGAIN (treated as TX-full). @@ -2260,9 +3690,30 @@ fn bus_loop( // Send all commands back-to-back and remember exactly which // motors were successfully queued in this tick. - expected.fill(false); + expected.fill(0); + attempted.fill(false); let mut trace_pending: [Option; N_SLOTS] = [None; N_SLOTS]; for (motor_index, m) in motors.iter().enumerate() { + // An impedance joint on its own 240 Hz cadence runs nothing on + // its off-ticks — not the tracker, derivatives, band-pass or + // dither — and on its own ticks steps them over its own + // interval, so it is the verified 240 Hz loop, interleaved. + // Every other motor steps at the loop's rate as before. + let (tick_dt, cmd_dt, overrun) = if sched.mit_phase(motor_index).is_some() { + if !sched.commanded(motor_index, ticks) { + continue; + } + let nominal = period.as_secs_f64() * sched.mit_div as f64; + let dt = + own_prev[m.slot].map_or(0.0, |p| began.duration_since(p).as_secs_f64()); + own_prev[m.slot] = Some(began); + // Its own cycle slipped a whole period: the same rule the + // loop applies to itself, at the joint's own rate. + let own_overrun = dt >= 2.0 * nominal; + (dt, if own_overrun { nominal } else { dt }, own_overrun) + } else { + (tick_dt, cmd_dt, overrun) + }; let c = if is_limp && !m.gripper { // Limp is enforced here, whatever the target says: no // stiffness, firmware damping only, no host damping or @@ -2274,7 +3725,11 @@ fn bus_loop( JointCmd { mode: 0.0, kp: 0.0, - kd: LIMP_KD, + kd: if ran_away[m.slot] { + A4_RUNAWAY_KD + } else { + LIMP_KD + }, kd_host: 0.0, j_eff: 0.0, ..play[m.slot] @@ -2283,6 +3738,7 @@ fn bus_loop( play[m.slot] }; let c = &c; + a4_sent[m.slot] = None; let (arb, frame) = if m.gripper { // Idle until the first target (classic mode leaves the // gripper uncommanded until motion_control too). Slot @@ -2317,65 +3773,170 @@ fn bus_loop( } else { c.p_des }; - let p_cmd = if tracked { - let (p, _, _) = trk[m.slot].update(p_tgt, cmd_dt); - p + // The tracker's own velocity: the rate of the trajectory + // actually sent, for the 0xA4 cap below (0 on a hold). + let (p_cmd, v_trk) = if tracked { + let (p, v, _) = trk[m.slot].update(p_tgt, cmd_dt); + (p, v) } else { trk[m.slot].seed(c.p_des); - c.p_des + (c.p_des, 0.0) }; let d = &mut damp[m.slot]; - let (v_wire, a_cmd, v_cmd_fast, friction_ff, inertia_ff, v_damp) = - if tracked && overrun { - // The gap since the last command is not a trajectory - // segment the motor followed — it held. Re-prime the - // derivative chains at rest here so the first tick - // back carries no fictitious velocity, acceleration - // (inertia torque), or band-pass energy; they ramp - // in again from the next tick as tracking resumes. - d.v_cmd.seed(p_cmd); - d.a_cmd.seed(0.0); - d.v_cmd_fast.seed(p_cmd); - d.bp.reset(); - (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - } else if tracked { - // Match classic AxolArm.motion_control: friction uses - // the 20 rad/s low-pass position derivative, inertia - // uses a second identical derivative, and damping - // uses its independent 80 rad/s desired-velocity - // derivative. Only the source position/rate differ: - // the core can use the trajectory it really sends. - let v_cmd = d.v_cmd.update(p_cmd, tick_dt); - let a_cmd = d.a_cmd.update(v_cmd, tick_dt); - let v_cmd_fast = d.v_cmd_fast.update(p_cmd, tick_dt); - let friction_ff = filter::friction(v_cmd, m.fc, m.k, m.fv, m.fo); - let inertia_ff = c.j_eff * a_cmd; - let damp_ok = feedback_fresh[m.slot] - && timing_on_time - && !timing_health.degraded - && !feedback_health[m.slot].degraded; - let v_damp = if damp_ok { - d.bp.update(v_cmd_fast - d.vel_meas, c.damp_w0, c.damp_q, tick_dt) - } else { - // A missing frame makes measured velocity stale. - // Reset rather than carrying band-pass energy into - // the first tick after feedback recovers. While the - // joint is degraded, damping stays off for the whole - // stretch: re-engaging a freshly reset band-pass - // every few ticks is a torque transient, not damping. - d.bp.reset(); - 0.0 - }; - (v_cmd, a_cmd, v_cmd_fast, friction_ff, inertia_ff, v_damp) + let ( + v_wire, + a_cmd, + v_cmd_fast, + friction_ff, + inertia_ff, + v_damp, + stiction_ff, + dither_ff, + stribeck_ff, + ) = if tracked && overrun { + // The gap since the last command is not a trajectory + // segment the motor followed — it held. Re-prime the + // derivative chains at rest here so the first tick + // back carries no fictitious velocity, acceleration + // (inertia torque), or band-pass energy; they ramp + // in again from the next tick as tracking resumes. + d.v_cmd.seed(p_cmd); + d.a_cmd.seed(0.0); + d.v_cmd_fast.seed(p_cmd); + d.bp.reset(); + (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + } else if tracked { + // Match classic AxolArm.motion_control: friction uses + // the 20 rad/s low-pass position derivative, inertia + // uses a second identical derivative, and damping + // uses its independent 80 rad/s desired-velocity + // derivative. Only the source position/rate differ: + // the core can use the trajectory it really sends. + let v_cmd = d.v_cmd.update(p_cmd, tick_dt); + let a_cmd = d.a_cmd.update(v_cmd, tick_dt); + let v_cmd_fast = d.v_cmd_fast.update(p_cmd, tick_dt); + // Sliding friction grows with the torque the gear + // meshes carry (`fl`, Nm per Nm of gravity). + let fc_eff = m.fc + m.fl * c.t_ff.abs(); + let friction_ff = filter::friction(v_cmd, fc_eff, m.k, m.fv, m.fo); + // Stiction compensation acts on the measured error + // against the latest accepted feedback. It is a slow + // term (it resolves a stuck joint over tens of ms), + // so a one-tick-old sample is fine, but a joint with + // no fresh reply this tick gets none rather than a + // push computed from a stale position. + let stiction_ff = match latest[m.slot] { + Some((pos, vel, _, _)) if feedback_fresh[m.slot] => filter::stiction( + p_cmd - pos, + vel, + filter::stiction_amplitude( + m.fc, + m.stiction_gain, + m.stiction_load_gain, + c.t_ff, + ), + m.stiction_err, + ), + _ => 0.0, + }; + let dither_ff = filter::dither_step( + &mut d.dither_phase, + m.dither_nm, + m.dither_hz, + tick_dt, + ); + // Stribeck cancellation on the measured velocity — + // only with a fresh sample behind it, like the + // stiction push. + let stribeck_ff = if feedback_fresh[m.slot] { + filter::stribeck_excess( + d.vel_meas_slow, + filter::stribeck_amplitude( + m.stribeck_gain, + m.stribeck_dfs, + m.stribeck_load_gain, + c.t_ff, + ), + m.stribeck_vs, + ) + } else { + 0.0 + }; + let inertia_ff = c.j_eff * a_cmd; + let damp_ok = feedback_fresh[m.slot] + && timing_on_time + && !timing_health.degraded + && !feedback_health[m.slot].degraded; + let v_damp = if damp_ok { + d.bp.update(v_cmd_fast - d.vel_meas, c.damp_w0, c.damp_q, tick_dt) } else { - d.v_cmd.seed(p_cmd); - d.a_cmd.seed(0.0); - d.v_cmd_fast.seed(p_cmd); + // A missing frame makes measured velocity stale. + // Reset rather than carrying band-pass energy into + // the first tick after feedback recovers. While the + // joint is degraded, damping stays off for the whole + // stretch: re-engaging a freshly reset band-pass + // every few ticks is a torque transient, not damping. d.bp.reset(); - (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + 0.0 }; + ( + v_cmd, + a_cmd, + v_cmd_fast, + friction_ff, + inertia_ff, + v_damp, + stiction_ff, + dither_ff, + stribeck_ff, + ) + } else { + d.v_cmd.seed(p_cmd); + d.a_cmd.seed(0.0); + d.v_cmd_fast.seed(p_cmd); + d.bp.reset(); + (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + }; let damping_ff = c.kd_host * v_damp; - let t_ff = c.t_ff + friction_ff + inertia_ff + damping_ff; + // Position-periodic torque cancellation, on tracked ticks + // only, at where the joint is: the latest reply carried + // one tick along its velocity. Using the tracker position + // instead would put it ~70 ms behind on an a4 joint — a + // third of the 0.905° ripple period at 5 deg/s. + let cogging_ff = if tracked && !m.cogging.is_empty() { + let q = match latest[m.slot] { + Some((pos, vel, _, _)) => pos + vel * tick_dt, + None => p_cmd, + }; + filter::cogging(&m.cogging, q) + } else { + 0.0 + }; + let t_ff = c.t_ff + + friction_ff + + stiction_ff + + dither_ff + + stribeck_ff + + inertia_ff + + damping_ff + + cogging_ff; + // The 0x73 feedforward for a firmware-loop joint that + // takes it: gravity, inertia and the cogging term — not + // the host damping band-pass or the friction family, + // which exist for the MIT frame (the firmware's speed PI + // is the damping and carries friction on this path) — + // faded in over `filter::TF_RAMP_S`. + let tf_on = m.tf_nm_per_pct > 0.0 && a4_wire(m.vendor, m.wire, tracked, c.kp); + tf_ramp[m.slot] = if tf_on { + filter::tf_ramp_step(tf_ramp[m.slot], tick_dt) + } else { + 0.0 + }; + let tf_pct = if tf_on { + tf_ramp[m.slot] * (c.t_ff + inertia_ff + cogging_ff) / m.tf_nm_per_pct + } else { + 0.0 + }; if trace_this_tick && trace_tx.is_some() { trace_pending[m.slot] = Some(TraceRow { tick: ticks, @@ -2398,23 +3959,101 @@ fn bus_loop( friction_ff, inertia_ff, damping_ff, + stiction_ff, + dither_ff, + stribeck_ff, total_ff: t_ff, kd_host: c.kd_host, damp_w0: c.damp_w0, damp_q: c.damp_q, tick_dt, fb_dt: f64::NAN, + cogging_ff, + tf_pct, }); } - let frame = proto::mit_encode(p_cmd, v_wire, c.kp, c.kd, t_ff, &m.ranges); - let arb = match m.vendor { - Vendor::MyActuator => proto::MA_MC_REQ + m.id as u16, - Vendor::Damiao => m.id as u16, - }; - (arb, frame) + if a4_wire(m.vendor, m.wire, tracked, c.kp) { + // Firmware position loop: the streamed trajectory (or + // the hold pose) as an absolute 0.01° target under the + // tracker's own velocity limit as the speed cap — as + // 0x73 with the torque feedforward where the joint + // takes it, plain 0xA4 (no feedforward) otherwise. The + // 0x92 read below restores fine position to the host. + a4_follow[motor_index] = true; + let target = a4_target(p_cmd, v_trk, m.lead_s); + a4_sent[m.slot] = Some(target); + let cap = a4_speed_cap(m.cap_track, v_trk, m.max_vel); + ( + proto::MA_REQ + m.id as u16, + if tf_on { + proto::ma_tf_encode(target, cap, tf_pct) + } else { + proto::ma_a4_encode(target, cap) + }, + ) + } else if pv_wire(m.vendor, m.wire, tracked, c.kp) { + // Damiao firmware position loop: same target, the + // tracker's velocity limit as the speed cap; the + // wrist's profiler (ACC/DEC) shapes it further. + ( + proto::DM_POS_VEL_ARB_BASE + m.id as u16, + proto::dm_pos_vel_encode(p_cmd, m.max_vel), + ) + } else { + let frame = proto::mit_encode(p_cmd, v_wire, c.kp, c.kd, t_ff, &m.ranges); + let arb = match m.vendor { + Vendor::MyActuator => proto::MA_MC_REQ + m.id as u16, + Vendor::Damiao => m.id as u16, + }; + (arb, frame) + } }; + // The bus cannot carry every motor every tick at the + // higher loop rate: a thinned motor's off-tick still ran + // its tracker (above), it just sends nothing. + if !sched.commanded(motor_index, ticks) { + continue; + } + // A Damiao wrist acts only on the frame of the control mode + // it is in. Switch the register (RAM write, immediate) on + // the tick the wanted frame changes — pv ↔ MIT across limp + // or gravity comp — before that frame goes out. + if dm_mode[motor_index] != 0 { + let wanted = if arb == proto::DM_POS_VEL_ARB_BASE + m.id as u16 { + proto::DM_MODE_POS_VEL + } else { + proto::DM_MODE_MIT + }; + if wanted != dm_mode[motor_index] { + let reg = proto::dm_write_register( + m.id as u16, + proto::DM_REG_CTRL_MODE, + wanted.to_le_bytes(), + ); + if let SendOutcome::Sent = + guarded_send(&sock, proto::DM_REG_ARB, ®, &mut enobufs_since)? + { + dm_mode[motor_index] = wanted; + send_text( + out_tx, + b'L', + &format!( + "{iface}: {} control mode → {} ({})", + m.joint, + wanted, + if wanted == proto::DM_MODE_POS_VEL { + "position-velocity, firmware loop" + } else { + "MIT, compliant" + }, + ), + ); + } + } + } + attempted[motor_index] = true; match guarded_send(&sock, arb, &frame, &mut enobufs_since)? { - SendOutcome::Sent => expected[motor_index] = true, + SendOutcome::Sent => expected[motor_index] = 1, SendOutcome::Dropped => {} SendOutcome::Stalled => { // The e-stop path: nothing has ACKed for >1 s. Stop @@ -2453,6 +4092,26 @@ fn bus_loop( } } + // 0xA4 joints: ask for the 0.01° multi-turn angle right behind the + // command so the reply pair lands inside this tick's window. + for (motor_index, m) in motors.iter().enumerate() { + if !a4_follow[motor_index] { + continue; + } + a4_follow[motor_index] = false; + if expected[motor_index] == 0 || !sched.a4_read(motor_index, ticks) { + continue; + } + if let SendOutcome::Sent = guarded_send( + &sock, + proto::MA_REQ + m.id as u16, + &proto::MA_MULTI_TURN_REQUEST, + &mut enobufs_since, + )? { + expected[motor_index] = 2; + } + } + // Collect replies. The // window begins when this tick actually began, not at its nominal // schedule point: a late wake must not discard shoulder feedback @@ -2461,8 +4120,9 @@ fn bus_loop( // must end the window at `reply_deadline`, never a jiffy or two // later, or the overrun lands on the next tick as lateness. let reply_deadline = began + period.saturating_sub(REPLY_GUARD); - seen.fill(false); - let mut pending = expected.iter().filter(|&&value| value).count(); + seen.fill(0); + bus_last_reply = None; + let mut pending: usize = expected.iter().map(|&n| n as usize).sum(); while pending > 0 { let now = Instant::now(); if now >= reply_deadline { @@ -2471,6 +4131,7 @@ fn bus_loop( let Some(frame) = sock.recv_timeout(reply_deadline - now)? else { break; }; + bus_last_reply = Some(Instant::now()); let (idx, pos, vel, tau) = match frame.id { id if (0x501..=0x505).contains(&id) => { let motor_id = (id - 0x500) as u8; @@ -2484,6 +4145,48 @@ fn bus_loop( ); (idx, pos, vel, tau) } + id if (0x241..=0x245).contains(&id) => { + // 0xA4 joints answer twice: the command echo (iq, + // speed, whole-degree angle) is staged; the 0x92 + // multi-turn read completes the sample. Torque is + // not available in Nm on this path (the echo carries + // q-axis current), so it is reported as NaN and the + // contact watchdog is blind on the joint. + let motor_id = (id - 0x240) as u8; + let Some(idx) = motors.iter().position(|m| m.id == motor_id) else { + continue; + }; + let slot = motors[idx].slot; + match frame.data[0] { + 0xA4 | proto::MA_TF_CMD => { + let (iq, speed, _) = proto::ma_decode_a4_reply(&frame.data); + if expected[idx] >= 2 { + // A 0x92 read follows: stage, let it + // complete the sample. + if mark_unique_expected_reply(&expected, &mut seen, idx) { + pending -= 1; + a4_stage[slot] = (speed, iq); + } + continue; + } + // No read this tick (thinned schedule): the + // echo completes the sample with the last + // fine position carried on its speed. + let now = Instant::now(); + let Some(anchor) = a4_anchor[slot] else { + continue; + }; + let pos = a4_extrapolate(anchor, speed, now); + (idx, pos, speed, f64::NAN) + } + proto::MA_MULTI_TURN_ANGLE => { + let pos = proto::ma_decode_position(&frame.data); + let (vel, _iq) = a4_stage[slot]; + (idx, pos, vel, f64::NAN) + } + _ => continue, + } + } id if (0x16..=0x18).contains(&id) => { let motor_id = (id - 0x10) as u8; let Some(idx) = motors.iter().position(|m| m.id == motor_id) else { @@ -2506,6 +4209,11 @@ fn bus_loop( pending -= 1; let recv_time = Instant::now(); latest[motors[idx].slot] = Some((pos, vel, tau, recv_time)); + // Every accepted sample re-anchors the a4 carry — the 0x92 + // read, the carried echo itself, and the MIT reply of an a4 + // joint that is limp right now, so the carry resumes from + // where the joint really is when it goes back on 0xA4. + a4_anchor[motors[idx].slot] = Some((pos, recv_time)); // The gripper has no damping chain or trace row: its // POSITION_FORCE reply only feeds the telemetry cache. if motors[idx].gripper { @@ -2521,6 +4229,7 @@ fn bus_loop( .map_or(0.0, |p| recv_time.duration_since(p).as_secs_f64()); d.last_fb = Some(recv_time); d.vel_meas = d.v_meas.update(pos, dt); + d.vel_meas_slow = d.v_meas_slow.update(pos, dt); (d.vel_meas, dt) }; if let (Some(tx), Some(mut row)) = @@ -2546,6 +4255,32 @@ fn bus_loop( // watchdog's job (limp gravity-comp hold, as in classic // mode); a self-disabled motor simply stops contributing, // as it did in classic mode. + // + // The firmware position loop is the exception: stiff by + // design, so a joint on it moving fast *away* from the + // target it was just sent is being driven there by its own + // loop (`RunawayGuard`). Limp takes every joint off the + // position frame; the runaway joint gets the MIT maximum kd + // to brake instead of coasting into its stop. + let slot = motors[idx].slot; + match a4_sent[slot] { + Some(target) => { + if runaway_guard[slot].check(pos, vel, target) && !ran_away[slot] { + ran_away[slot] = true; + go_limp( + limp, + out_tx, + &format!( + "{iface}: {} ran away on the firmware position loop ({:+.1}° from its target, moving away at {:+.0}°/s) — going limp, that joint braked at kd {A4_RUNAWAY_KD}", + motors[idx].joint, + (pos - target).to_degrees(), + vel.to_degrees(), + ), + ); + } + } + None => runaway_guard[slot].reset(), + } } // Replies still outstanding at the window's end. Counted into the @@ -2566,9 +4301,27 @@ fn bus_loop( if motor.gripper { continue; } - feedback_fresh[motor.slot] = seen[idx]; + if !attempted[idx] { + // Not this motor's tick on the thinned schedule: no + // reply was owed, so none is missing, and the latest + // sample stays "fresh" — it is the newest the schedule + // can produce. Clearing it here would leave the + // feedforwards that need a sample (stiction, Stribeck, + // host damping) permanently off on a thinned MIT joint, + // since every commanded tick follows an off-tick. + continue; + } + let complete = reply_complete(&expected, &seen, idx); + feedback_fresh[motor.slot] = complete; let health = &mut feedback_health[motor.slot]; - match health.record(seen[idx], silent_limit) { + // Counted in the joint's own commands, so the silent limit + // stays `SILENT_FEEDBACK_FAULT` of wall time on the 240 Hz lane. + let limit = if sched.mit_phase(idx).is_some() { + silent_limit.div_ceil(sched.mit_div as u32) + } else { + silent_limit + }; + match health.record(complete, limit) { FeedbackVerdict::Steady => {} FeedbackVerdict::Degraded => { degraded_episodes += 1; @@ -2608,7 +4361,7 @@ fn bus_loop( "{iface}: {} silent for {} consecutive ticks ({:.1} s) — motor unreachable; going limp rather than commanding it stiff and blind", motor.joint, health.consecutive_misses, - health.consecutive_misses as f64 / cfg.loop_hz, + health.consecutive_misses as f64 * sched.mit_phase(idx).map_or(1.0, |_| sched.mit_div as f64) / cfg.loop_hz, ), ); } @@ -2626,16 +4379,24 @@ fn bus_loop( let _ = out_tx.send(build_feedback(side, &latest, Instant::now())); } + if let Some(t) = bus_last_reply { + bus_busy.push(t.duration_since(began).as_secs_f64() / period.as_secs_f64()); + } if began >= next_stats { next_stats = began + Duration::from_secs(5); + let (busy_p50, busy_p95, busy_max) = bus_busy_percentiles(&mut bus_busy); + bus_busy.clear(); send_text( out_tx, b'L', &format!( - "{iface}: {ticks} ticks, {late} late ({:.2}%), {overruns} overruns, {timing_degraded_ticks} timing-degraded ticks in {timing_degraded_episodes} episodes, {missed} missed replies, {degraded_ticks} feedback-degraded ticks in {degraded_episodes} episodes, {rejected} rejected targets, {held_ticks} held-over ticks (oldest target {:.1} ms, cadence {:.1} ms), {trace_dropped} trace drops, seq {:?}", + "{iface}: {ticks} ticks, {late} late ({:.2}%), {overruns} overruns, {timing_degraded_ticks} timing-degraded ticks in {timing_degraded_episodes} episodes, {missed} missed replies, {degraded_ticks} feedback-degraded ticks in {degraded_episodes} episodes, {rejected} rejected targets, {held_ticks} held-over ticks (oldest target {:.1} ms, cadence {:.1} ms), bus busy p50 {:.0}% p95 {:.0}% max {:.0}% of the tick, {trace_dropped} trace drops, seq {:?}", late as f64 / ticks as f64 * 100.0, worst_target_age * 1e3, cadence.get().unwrap_or(f64::NAN) * 1e3, + busy_p50 * 100.0, + busy_p95 * 100.0, + busy_max * 100.0, last_seq, ), ); diff --git a/rust/axol-rt/src/timing.rs b/rust/axol-rt/src/timing.rs index ccbaa08a..b4fd7e67 100644 --- a/rust/axol-rt/src/timing.rs +++ b/rust/axol-rt/src/timing.rs @@ -9,7 +9,25 @@ use std::time::{Duration, Instant}; const JOINTS: usize = 8; const WINDOW: Duration = Duration::from_secs(2); -const TARGET_HZ: f64 = 240.0; +/// The loop rates the core runs at: 240 Hz on the impedance controller, +/// 400 Hz on the firmware position controller (`AxolConfig.controller`). +/// The passive observer is not told which; it takes the one nearest the +/// measured command rate, so deadline misses are counted against the loop +/// that is actually running. +const LOOP_RATES_HZ: [f64; 2] = [240.0, 400.0]; + +/// The nominal loop rate behind a measured command rate (240 Hz until the +/// measurement exists). +fn target_hz(command_hz: Option) -> f64 { + let Some(hz) = command_hz else { + return LOOP_RATES_HZ[0]; + }; + LOOP_RATES_HZ + .iter() + .copied() + .min_by(|a, b| (a - hz).abs().total_cmp(&(b - hz).abs())) + .unwrap_or(LOOP_RATES_HZ[0]) +} #[derive(Default)] struct JointEvents { @@ -133,7 +151,8 @@ impl TimingAggregator { headroom.push(period - cycle); } } - let nominal = 1.0 / TARGET_HZ; + let target = target_hz(rate(&commands)); + let nominal = 1.0 / target; let deadline_misses: usize = command_dt .iter() .map(|dt| ((*dt / nominal + 0.5) as usize).saturating_sub(1)) @@ -162,8 +181,8 @@ impl TimingAggregator { .map(|t| now.duration_since(*t).as_secs_f64() * 1e3) }; Some(format!( - "{{\"sourceJoint\":\"{}\",\"targetHz\":240.0,\"commandHz\":{},\"feedbackHz\":{},\"commandPeriodMs\":{},\"feedbackPeriodMs\":{},\"commandJitterP95Ms\":{},\"feedbackJitterP95Ms\":{},\"commandGapMaxMs\":{},\"feedbackGapMaxMs\":{},\"commandBatchP50Ms\":{},\"commandBatchP95Ms\":{},\"feedbackBatchP95Ms\":{},\"canCycleP50Ms\":{},\"canCycleP95Ms\":{},\"canUtilizationP95Pct\":{},\"canHeadroomP05Ms\":{},\"roundTripP50Ms\":{},\"roundTripP95Ms\":{},\"deadlineMisses\":{},\"missedFeedback\":{},\"commandAgeMs\":{},\"feedbackAgeMs\":{}}}", - names[slot], js(rate(&commands)), js(rate(&feedback)), js(median(&command_dt).map(ms)), + "{{\"sourceJoint\":\"{}\",\"targetHz\":{},\"commandHz\":{},\"feedbackHz\":{},\"commandPeriodMs\":{},\"feedbackPeriodMs\":{},\"commandJitterP95Ms\":{},\"feedbackJitterP95Ms\":{},\"commandGapMaxMs\":{},\"feedbackGapMaxMs\":{},\"commandBatchP50Ms\":{},\"commandBatchP95Ms\":{},\"feedbackBatchP95Ms\":{},\"canCycleP50Ms\":{},\"canCycleP95Ms\":{},\"canUtilizationP95Pct\":{},\"canHeadroomP05Ms\":{},\"roundTripP50Ms\":{},\"roundTripP95Ms\":{},\"deadlineMisses\":{},\"missedFeedback\":{},\"commandAgeMs\":{},\"feedbackAgeMs\":{}}}", + names[slot], js(Some(target)), js(rate(&commands)), js(rate(&feedback)), js(median(&command_dt).map(ms)), js(median(&feedback_dt).map(ms)), js(jitter(&command_dt)), js(jitter(&feedback_dt)), js(command_dt.iter().copied().reduce(f64::max).map(ms)), js(feedback_dt.iter().copied().reduce(f64::max).map(ms)), @@ -277,7 +296,28 @@ mod tests { .snapshot_json(start + Duration::from_millis(14)) .unwrap(); assert!(json.contains("\"sourceJoint\":\"SHOULDER_1\"")); + assert!(json.contains("\"targetHz\":240.000000000")); assert!(json.contains("\"commandHz\":240.000000000")); assert!(json.contains("\"roundTripP95Ms\":1.000000000")); } + + #[test] + fn a_400_hz_stream_is_judged_against_400_hz() { + assert_eq!(target_hz(None), 240.0); + assert_eq!(target_hz(Some(238.0)), 240.0); + assert_eq!(target_hz(Some(396.0)), 400.0); + let mut timing = TimingAggregator::new(); + let start = Instant::now(); + for tick in 0..8 { + let at = start + Duration::from_secs_f64(tick as f64 / 400.0); + timing.observe(0x141, &[0xA4, 0, 0, 0, 0, 0, 0, 0], at); + timing.observe(0x501, &[0; 8], at + Duration::from_micros(500)); + } + let json = timing + .snapshot_json(start + Duration::from_millis(19)) + .unwrap(); + assert!(json.contains("\"targetHz\":400.000000000"), "{json}"); + // 400 Hz ticks are not 240 Hz deadline misses. + assert!(json.contains("\"deadlineMisses\":0"), "{json}"); + } } diff --git a/rust/axol-rt/tools/rt_proto_check.py b/rust/axol-rt/tools/rt_proto_check.py index 8bd1ff03..83d506c9 100644 --- a/rust/axol-rt/tools/rt_proto_check.py +++ b/rust/axol-rt/tools/rt_proto_check.py @@ -12,6 +12,10 @@ import os import struct import subprocess +import sys + +sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..", "..", "..")) +from almond_axol.rt.link import CONFIG_PROTO # noqa: E402 BIN = os.environ.get( "AXOL_RT_BIN", @@ -57,8 +61,20 @@ async def recv(): return proc.returncode, out.decode(), result -joint_line = b"joint 0 can_alm_axol_l shoulder_1 1 250 3.5 9.4 33.0 0.6 250 0.15 0.02\n" -cfg = b"C" + b"proto 2\n" + b"loop_hz 240\n" + joint_line +# One joint per wire token, in the current `joint` line layout (see the +# `parse_config` tests in serve.rs): the package's CONFIG_PROTO must be the +# binary's, or the clean check fails with the core's version-skew message. +joint_line = ( + b"joint 0 can_alm_axol_l shoulder_1 1 250 3.5 9.4 33.0 0.6 250 0.15 0.02" + b" 0 0 0 0 60 a4 0 0.3 0.1 0.1 0 20\n" + b"joint 0 can_alm_axol_l elbow 4 130 5.0 9.4 33.0 0.6 250 0.15 0.02" + b" 0 0 0 0 60 mit 0 0.3 0.1 0.1 0 20\n" + b"joint 0 can_alm_axol_l wrist_2 6 130 3.5 9.4 33.0 0 0 0 0" + b" 0 0 0 0 60 pv 0 0.3 0.1 0.1 0 20\n" +) +# A mixed bus (a4 + mit + pv): the core runs it at 480 Hz with the impedance +# joint on alternate ticks; 400 would be refused (impedance is 240 Hz only). +cfg = b"C" + f"proto {CONFIG_PROTO}\n".encode() + b"loop_hz 480\n" + joint_line async def clean(send, recv, w): @@ -93,10 +109,8 @@ def check_feedback_parse(): its `feedback_packet_layout` unit test) and asserts `RtLink._parse_feedback` recovers the values, including the age -> timestamp reconstruction. """ - import sys import time - sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..", "..", "..")) from almond_axol.rt.link import RtLink slots_in = {0: (1.5, -0.25, 3.0, 1200), 7: (0.5, 0.0, 0.1, 0)} diff --git a/scripts/cogging_map.py b/scripts/cogging_map.py new file mode 100644 index 00000000..1a64bd0d --- /dev/null +++ b/scripts/cogging_map.py @@ -0,0 +1,299 @@ +"""Look for position-periodic torque (cogging / gear mesh) in a friction sweep +and turn it into a feedforward table. + +Input is the ``--raw-csv`` of ``axol tune.friction``: every cruise sample of a +bidirectional constant-speed sweep, ``(joint, side, v_rad_s, direction, +q_rad, tau_nm)``. The forward and backward passes at one speed are averaged +on a fine angle grid, which cancels the speed-dependent friction and leaves +gravity plus anything that depends on *position*. A smooth trend (the gravity +model's residual) is removed, and the remainder is analysed in the **angle** +domain: cogging and gear mesh have a fixed period in degrees of travel and +line up across speeds, whereas stick-slip and structural ringing have a fixed +period in *time* and therefore change spatial period with speed. + +Usage: + uv run python scripts/cogging_map.py ~/fric-s1-raw.csv + uv run python scripts/cogging_map.py ~/fric-s1-raw.csv --grid-deg 0.1 --table ~/cog-s1.json + uv run python scripts/cogging_map.py ~/fric-s1-raw.csv --fit # Fourier series + uv run python scripts/cogging_map.py ~/fric-s1-raw.csv --fit --save # → calibration + +``--fit`` fits the Fourier series the realtime core cancels +(``almond_axol.tuning.cogging``: a ``--period`` fundamental, default 3.62°, +with ``--harmonics``, default 1 2 4 — the right shoulder_1's 3.62° / 1.81° / +0.905° ripple), reports each pass's fit and how well it predicts the *other* +passes (the out-of-sample test — a series that only fits its own pass cancels +nothing), and with ``--save`` writes it to this robot's calibration file as +the joint's ``cogging`` entry. It then applies on every bring-up: added to the +MIT feedforward on an impedance joint, carried by 0x73 on a firmware-loop joint +with ``firmware.tf_rated_current_a`` set. + +The table (``--table``) is ``{"joint", "side", "grid_deg", "q_deg": [...], +"tau_nm": [...]}``: the periodic torque to *add* to the feedforward at each +angle so the motor cancels it. Only worth wiring in if the report shows a +peak that is consistent across speeds and well above the noise floor. +""" + +from __future__ import annotations + +import argparse +import csv +import json +import math +from collections import defaultdict +from pathlib import Path + +import numpy as np + + +def grid_average(q: np.ndarray, tau: np.ndarray, grid: np.ndarray) -> np.ndarray: + """Mean torque per grid cell (NaN where empty).""" + idx = np.clip(np.searchsorted(grid, q) - 1, 0, len(grid) - 2) + sums = np.zeros(len(grid) - 1) + counts = np.zeros(len(grid) - 1) + np.add.at(sums, idx, tau) + np.add.at(counts, idx, 1) + with np.errstate(invalid="ignore"): + return np.where(counts > 0, sums / np.maximum(counts, 1), np.nan) + + +def detrend(x: np.ndarray, y: np.ndarray, degree: int = 5) -> np.ndarray: + """Remove a low-order polynomial (the gravity residual and any drift).""" + ok = np.isfinite(y) + coef = np.polyfit(x[ok], y[ok], degree) + return y - np.polyval(coef, x) + + +def angle_spectrum(y: np.ndarray, grid_deg: float) -> tuple[np.ndarray, np.ndarray]: + """Power spectrum against spatial frequency (cycles per degree).""" + ok = np.isfinite(y) + z = np.where(ok, y, 0.0) + z = z - z[ok].mean() + window = np.hanning(len(z)) + power = np.abs(np.fft.rfft(z * window)) ** 2 + freq = np.fft.rfftfreq(len(z), grid_deg) + return freq, power + + +def analyse(path: Path, grid_deg: float, table: Path | None) -> None: + # Keyed by (pass index, speed) so repeated passes at one speed stay apart — + # pass-to-pass repeatability at the same speed is the strictest test of + # whether a position table could cancel anything. + by_speed: dict[tuple[int, float], dict[str, list[tuple[float, float]]]] = ( + defaultdict(lambda: {"+": [], "-": []}) + ) + joint = side = "" + with open(path, newline="") as f: + for row in csv.DictReader(f): + joint, side = row["joint"], row["side"] + key = (int(row.get("pass", 0) or 0), round(float(row["v_rad_s"]), 4)) + by_speed[key][row["direction"]].append( + (math.degrees(float(row["q_rad"])), float(row["tau_nm"])) + ) + if not by_speed: + raise SystemExit("no samples in the CSV") + all_q = np.array( + [q for d in by_speed.values() for rows in d.values() for q, _ in rows] + ) + lo, hi = np.floor(all_q.min()), np.ceil(all_q.max()) + grid = np.arange(lo, hi + grid_deg, grid_deg) + centres = grid[:-1] + grid_deg / 2 + + print( + f"{side} {joint}: {len(all_q)} samples over {lo:.0f}..{hi:.0f}°, {grid_deg}° grid" + ) + print( + f"{'speed':>8s} {'periodic RMS':>13s} {'noise floor':>12s} {'top spatial peaks (° per cycle : Nm)':>40s}" + ) + residuals: dict[tuple[int, float], np.ndarray] = {} + for key in sorted(by_speed): + _pass, v = key + fwd = np.array(by_speed[key]["+"]) if by_speed[key]["+"] else np.empty((0, 2)) + bwd = np.array(by_speed[key]["-"]) if by_speed[key]["-"] else np.empty((0, 2)) + if len(fwd) < 50 or len(bwd) < 50: + continue + f_avg = grid_average(fwd[:, 0], fwd[:, 1], grid) + b_avg = grid_average(bwd[:, 0], bwd[:, 1], grid) + both = np.isfinite(f_avg) & np.isfinite(b_avg) + avg = np.where(both, (f_avg + b_avg) / 2.0, np.nan) + # Restrict to the span both directions covered. + ok = np.flatnonzero(both) + if len(ok) < 20: + continue + span = slice(ok[0], ok[-1] + 1) + x = centres[span] + y = avg[span] + # Fill small gaps by interpolation so the spectrum is not spiked. + good = np.isfinite(y) + y = np.interp(x, x[good], y[good]) + resid = detrend(x, y) + residuals[key] = np.interp(centres, x, resid, left=np.nan, right=np.nan) + freq, power = angle_spectrum(resid, grid_deg) + # Ignore anything slower than one cycle per 10° (that is the trend). + sel = freq > 0.1 + order = np.argsort(power[sel])[::-1][:3] + amps = ( + np.sqrt(power[sel][order] / power[sel].sum()) * resid.std() * math.sqrt(2) + ) + peaks = ", ".join( + f"{1 / freq[sel][i]:.2f}° : {a:.3f}" for i, a in zip(order, amps) + ) + # Noise floor: median spectral amplitude. + floor = ( + math.sqrt(np.median(power[sel]) / power[sel].sum()) + * resid.std() + * math.sqrt(2) + ) + print( + f"{math.degrees(v):5.1f}°/s #{_pass} {resid.std():8.3f} Nm {floor:9.3f} Nm {peaks}" + ) + + if len(residuals) >= 2: + keys = sorted(residuals) + stack = np.array([residuals[k] for k in keys]) + common = np.all(np.isfinite(stack), axis=0) + if common.sum() > 20: + c = np.corrcoef(stack[:, common]) + label = lambda k: f"{math.degrees(k[1]):.1f}°/s #{k[0]}" # noqa: E731 + pairs = [ + (label(keys[i]), label(keys[j]), c[i, j]) + for i in range(len(keys)) + for j in range(i + 1, len(keys)) + ] + print( + "\ncorrelation of the position residual between passes (cogging repeats, stick-slip does not):" + ) + for a, b, r in pairs: + print(f" {a} vs {b}: r = {r:+.2f}") + mean_resid = stack[:, common].mean(axis=0) + print( + f"speed-averaged periodic torque: {mean_resid.std():.3f} Nm RMS, {np.ptp(mean_resid):.3f} Nm peak-to-peak" + ) + if table is not None: + out = { + "joint": joint, + "side": side, + "grid_deg": grid_deg, + "q_deg": [float(q) for q in centres[common]], + # The torque the motor supplied through each bump is what + # it has to be given ahead of time: the residual as-is. + "tau_nm": [float(t) for t in mean_resid], + "note": "feedforward to ADD at each joint-frame angle to cancel the measured position-periodic torque", + } + table.write_text(json.dumps(out, indent=1)) + print(f"table → {table}") + print( + "\nRead it as: a peak that sits at the same ° per cycle at every speed with r above ~0.7 " + "between speeds is cogging or gear mesh and can be cancelled from a table; peaks that " + "move with speed are time-domain (stick-slip, structural) and cannot." + ) + + +def fit_series( + path: Path, period_deg: float, harmonics: tuple[int, ...], save: bool +) -> None: + """Fit the Fourier series per pass and pooled; optionally save it.""" + from almond_axol.robot.calibration import update_joint_calibration + from almond_axol.tuning.cogging import fit_cogging, prediction_r + + passes: dict[int, list[tuple[float, float, str]]] = defaultdict(list) + joint = side = "" + with open(path, newline="") as f: + for row in csv.DictReader(f): + joint, side = row["joint"], row["side"] + passes[int(row.get("pass", 0) or 0)].append( + (float(row["q_rad"]), float(row["tau_nm"]), row["direction"]) + ) + if not passes: + raise SystemExit("no samples in the CSV") + + def cols(rows: list[tuple[float, float, str]]): + q, t, d = zip(*rows) + return np.array(q), np.array(t), np.array(d) + + names = ", ".join(f"{period_deg / k:.3g}°" for k in harmonics) + print(f"\n{side} {joint}: Fourier fit, {period_deg}° fundamental ({names})") + fits = {} + for k in sorted(passes): + try: + fits[k] = fit_cogging( + *cols(passes[k]), period_deg=period_deg, harmonics=harmonics + ) + except ValueError as exc: + print(f" pass {k}: {exc}") + for k, fit in fits.items(): + others = {o: prediction_r(fit.model, *cols(passes[o])) for o in fits if o != k} + amps = ", ".join( + f"{period_deg / h:.3g}°: {a:.3f} Nm" + for h, a in zip(harmonics, fit.amplitudes) + ) + pred = ", ".join(f"#{o} r={r:+.2f}" for o, r in others.items()) + print( + f" pass {k}: R² {fit.r2:.2f} of {fit.ripple_rms:.3f} Nm ripple — {amps}" + + (f" | predicts {pred}" if pred else "") + ) + pooled = fit_cogging( + *cols([r for rows in passes.values() for r in rows]), + period_deg=period_deg, + harmonics=harmonics, + ) + amps = ", ".join( + f"{period_deg / h:.3g}°: {a:.3f} Nm" + for h, a in zip(harmonics, pooled.amplitudes) + ) + print(f" pooled: R² {pooled.r2:.2f} — {amps}") + print( + " Worth cancelling when every pass shows the same amplitudes and each " + "predicts the others at r ≳ 0.4." + ) + if save: + if side not in ("left", "right") or not joint: + raise SystemExit("the CSV names no side/joint — cannot save") + out = update_joint_calibration(side, joint, cogging=pooled.model.as_dict()) + print(f" saved {side}.{joint} cogging → {out}") + + +def main() -> None: + p = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + p.add_argument("csv", type=Path, help="--raw-csv output of axol tune.friction") + p.add_argument( + "--grid-deg", type=float, default=0.1, help="Angle grid (default: 0.1°)" + ) + p.add_argument( + "--table", + type=Path, + default=None, + help="Write the cancellation table here (JSON)", + ) + p.add_argument( + "--fit", + action="store_true", + help="Fit the Fourier series the realtime core cancels (see the docstring)", + ) + p.add_argument( + "--period", + type=float, + default=3.62, + help="Fundamental period of the fit, degrees (default: 3.62)", + ) + p.add_argument( + "--harmonics", + type=int, + nargs="+", + default=[1, 2, 4], + help="Harmonic numbers to fit (default: 1 2 4)", + ) + p.add_argument( + "--save", + action="store_true", + help="With --fit: write the pooled series to this robot's calibration file", + ) + args = p.parse_args() + analyse(args.csv, args.grid_deg, args.table) + if args.fit or args.save: + fit_series(args.csv, args.period, tuple(args.harmonics), args.save) + + +if __name__ == "__main__": + main() diff --git a/scripts/creep_test.py b/scripts/creep_test.py new file mode 100644 index 00000000..9348f261 --- /dev/null +++ b/scripts/creep_test.py @@ -0,0 +1,378 @@ +"""Creep test: does a MyActuator joint move smoothly at slow speed under its +*own* position loop (0xA4 absolute position closed-loop), where the MIT +impedance frame stick-slips? + +Feasibility probe for moving the X8-P20 shoulders onto the firmware loop. +Streams a constant-velocity 0xA4 target at ``--rate`` Hz while reading the +fine multi-turn position (0x92, 0.01 deg/LSB) every cycle, then scores each +constant-speed segment: velocity ripple, lag, 1-4 Hz error band, and the +fraction of 50 ms windows in which the joint did not move at all (the +stick phases). No realtime core, no impedance control: the other joints +are parked in their firmware position holds exactly as the tuning probes +park them, and the arm is homed before and after. + +The comparable MIT numbers (right shoulder_1 at ~0.05 rad/s, extended, +2026-09-18): velocity swinging 0 to 0.15 rad/s at ~2 Hz, 1-4 Hz error band +0.05-0.11 deg, stairs 0.1-0.6 deg. + +Usage: + uv run python scripts/creep_test.py --r --joint shoulder_1 + uv run python scripts/creep_test.py --r --joint shoulder_1 --center -35 --amp 10 --speeds 2 3 6 --csv ~/creep-s1.csv + +The joint under test runs the whole time in firmware position control with +a speed cap (``--cap``, dps): it holds position stiffly and will push back +against contact up to the motor's limit. Keep the workspace clear. +""" + +from __future__ import annotations + +import argparse +import asyncio +import csv +import math +import struct +import time +from pathlib import Path + +import numpy as np + +from almond_axol.cli.motor import add_side_and_channel_arguments, resolve_channel +from almond_axol.cli.tune.friction import _home_all, _ramp_verified +from almond_axol.constants import ARM_JOINTS, Joint +from almond_axol.motor import CanBus, ControlMode, Motor +from almond_axol.motor.myactuator import MyActuatorMotor +from almond_axol.tuning import joint_frame_motors, safe_limits + +_MA_POS_CONTROL = 0xA4 +_MA_MULTI_TURN_ANGLE = 0x92 +_MA_READ_ACCEL = 0x42 +_MA_WRITE_ACCEL = 0x43 # RAM + ROM; 0x00 = position-plan accel, 0x01 = decel +#: The firmware needs a moment after a 0x43 flash write before it answers again. +_ACCEL_WRITE_SETTLE_S = 0.3 + + +async def _read_plan_accel(driver: MyActuatorMotor) -> tuple[int, int]: + """(acceleration, deceleration) of the position planner, dps/s.""" + out = [] + for kind in (0x00, 0x01): + resp = await driver._request(bytes([_MA_READ_ACCEL, kind, 0, 0, 0, 0, 0, 0])) + out.append(struct.unpack_from(" tuple[int, int]: + """Write the position planner's accel and decel (raw, so 0 = direct + tracking can be asked for) and return what the motor reads back.""" + for kind in (0x00, 0x01): + await driver._request( + bytes([_MA_WRITE_ACCEL, kind, 0, 0]) + struct.pack(" bytes: + """0xA4: uint16 speed cap (dps) + int32 target (0.01 deg).""" + return ( + bytes([_MA_POS_CONTROL, 0x00]) + + struct.pack(" tuple[int, float, float, float]: + """(temp C, iq A, speed rad/s, position rad) from a 0x240 control reply. + Position is the coarse int16 degrees the reply carries (1 deg/LSB).""" + temp = struct.unpack_from(" dict: + """Score one constant-velocity cruise (joint frame, rad).""" + if len(t) < 20: + return {} + dt = float(np.median(np.diff(t))) + err = pos - target + # Velocity from fine position, 50 ms box-averaged. + win = max(1, int(round(0.05 / dt))) + kern = np.ones(win) / win + v = np.convolve(np.gradient(pos, t), kern, mode="same") + core = slice(win, -win) if len(t) > 3 * win else slice(0, len(t)) + v_core = v[core] + # Stick fraction: windows with < 0.03 deg of travel while the command moves. + n_win = len(pos) // win + moved = np.abs( + np.diff(pos[: n_win * win].reshape(n_win, win)[:, [0, -1]], axis=1) + ).ravel() + stuck = float(np.mean(moved < math.radians(0.03))) if n_win else math.nan + expected = abs(v_cmd) * win * dt + stairs = float(np.degrees(np.max(moved) - expected)) if n_win else math.nan + e = err - err.mean() + n = len(e) + F = np.abs(np.fft.rfft(e * np.hanning(n))) ** 2 + f = np.fft.rfftfreq(n, dt) + band = math.sqrt(F[(f >= 1) & (f <= 4)].sum() / max(F.sum(), 1e-30)) * float( + e.std() + ) + return { + "v_cmd_dps": math.degrees(abs(v_cmd)), + "lag_ms": float(-err.mean() / v_cmd * 1000.0) if v_cmd else math.nan, + "err_rms_deg": float(np.degrees(err.std())), + "band_1_4_deg": float(np.degrees(band)), + "v_ripple": float(v_core.std() / max(abs(v_cmd), 1e-9)), + "v_min_frac": float(np.min(v_core * np.sign(v_cmd)) / max(abs(v_cmd), 1e-9)), + "stuck_frac": stuck, + "max_stair_deg": stairs, + } + + +async def _stream( + driver: MyActuatorMotor, + offset: float, + start: float, + end: float, + v: float, + cap_dps: float, + rate: float, + log: list, + seg: int, +) -> None: + """Stream a constant-velocity 0xA4 trajectory (joint frame) with a fine + position read every cycle.""" + period = 1.0 / rate + duration = abs(end - start) / v + sign = 1.0 if end > start else -1.0 + t0 = time.perf_counter() + deadline = t0 + while True: + t = time.perf_counter() - t0 + if t > duration + 0.5: + return + deadline += period + target = start + sign * v * min(t, duration) + resp = await driver._request(_a4_frame(target - offset, cap_dps)) + temp, iq, speed, pos_coarse = _decode_a4_reply(resp) + fine = await driver._request(bytes([_MA_MULTI_TURN_ANGLE, 0, 0, 0, 0, 0, 0, 0])) + pos = struct.unpack_from(" None: + joint = Joint(args.joint) + is_left = args.l + lo, hi = safe_limits(joint, is_left) + center = math.radians(args.center) + half = math.radians(args.amp) / 2.0 + margin = math.radians(3.0) + if not (lo + margin <= center - half and center + half <= hi - margin): + raise SystemExit( + f"range {args.center - args.amp / 2:+.1f}..{args.center + args.amp / 2:+.1f}° " + f"is outside the safe range [{math.degrees(lo) + 3:.1f}, {math.degrees(hi) - 3:.1f}]° " + f"for {joint.value}" + ) + speeds = [math.radians(s) for s in args.speeds] + channel = resolve_channel(args) + log: list = [] + print( + f"\nCreep test — {'left' if is_left else 'right'} {joint.value}: 0xA4 direct tracking" + ) + print( + f" range {args.center - args.amp / 2:+.1f}..{args.center + args.amp / 2:+.1f}°, " + f"speeds {args.speeds} deg/s, cap {args.cap:g} dps, {args.rate:g} Hz" + ) + async with CanBus(channel) as bus: + raw = {j: Motor(bus, j) for j in ARM_JOINTS} + await asyncio.gather(*[m.enable() for m in raw.values()]) + motors = await joint_frame_motors(raw, is_left) + await asyncio.gather( + *[ + m.set_control_mode(ControlMode.POSITION_VELOCITY) + for m in motors.values() + ] + ) + driver = motors[joint].motor._driver + if not isinstance(driver, MyActuatorMotor): + raise SystemExit(f"{joint.value} is not a MyActuator joint") + offset = motors[joint].offset + original_accel: tuple[int, int] | None = None + try: + acc, dec = await _read_plan_accel(driver) + print(f" position planner: accel {acc} dps/s, decel {dec} dps/s (stored)") + if args.accel is not None and (acc, dec) != (args.accel, args.accel): + original_accel = (acc, dec) + got = await _write_plan_accel(driver, args.accel) + mode = "direct PI tracking" if got[0] == 0 else "velocity-profiled hops" + print( + f" position planner now: accel {got[0]}, decel {got[1]} dps/s → {mode}" + ) + if got[0] != args.accel: + print( + f" ! the motor did not accept {args.accel}; running with {got[0]}" + ) + elif acc == 0: + print(" mode: direct PI tracking") + else: + print(" mode: velocity-profiled hops between streamed targets") + print(" Homing all joints to rest ...") + await _home_all(motors) + start = center - half + print(f" Ramping {joint.value} to {math.degrees(start):+.1f}° ...") + await _ramp_verified(motors, {joint: start}) + await asyncio.sleep(0.5) + seg = 0 + here = start + for v in speeds: + for _ in range(args.passes): + there = center + half if here < center else center - half + print( + f" segment {seg}: {math.degrees(here):+.1f} → {math.degrees(there):+.1f}° at {math.degrees(v):.1f} deg/s" + ) + await _stream( + driver, offset, here, there, v, args.cap, args.rate, log, seg + ) + here = there + seg += 1 + except KeyboardInterrupt: + print("\n Interrupted.") + finally: + print(" Returning to rest and disabling ...") + try: + await _ramp_verified(motors, {joint: 0.0}) + await _home_all(motors) + except Exception: # noqa: BLE001 - best-effort teardown + pass + if original_accel is not None: + try: + got = await _write_plan_accel(driver, original_accel[0]) + print( + f" position planner restored: accel {got[0]}, decel {got[1]} dps/s" + ) + except Exception: # noqa: BLE001 - report, the value is in ROM + print( + f" ! could not restore the planner acceleration ({original_accel[0]} " + "dps/s) — set it with `axol motor.set-config` before the next session" + ) + await asyncio.gather( + *[m.set_control_mode(ControlMode.IMPEDANCE) for m in motors.values()] + ) + await asyncio.gather(*[m.disable() for m in raw.values()]) + + if not log: + print("No samples.") + return + arr = np.array(log, dtype=float) + if args.csv is not None: + with open(args.csv, "w", newline="") as f: + w = csv.writer(f) + w.writerow( + [ + "t", + "seg", + "v_cmd", + "target", + "pos_fine", + "pos_coarse", + "speed_motor", + "iq_a", + "temp_c", + ] + ) + w.writerows(arr.tolist()) + print(f" samples → {args.csv}") + print( + f"\n{'seg':>3s} {'v dps':>6s} {'lag ms':>7s} {'err°':>6s} {'band1-4°':>8s} {'v ripple':>8s} {'v min':>6s} {'stuck':>6s} {'stair°':>7s}" + ) + for seg in np.unique(arr[:, 1]).astype(int): + s = (arr[:, 1] == seg) & (arr[:, 2] != 0.0) + t = arr[s, 0] + if len(t) < 20: + continue + cruise = t > t[0] + 0.3 + m = segment_metrics( + t[cruise], arr[s, 3][cruise], arr[s, 4][cruise], float(arr[s, 2][0]) + ) + if m: + print( + f"{seg:3d} {m['v_cmd_dps']:6.1f} {m['lag_ms']:7.1f} {m['err_rms_deg']:6.3f} " + f"{m['band_1_4_deg']:8.3f} {m['v_ripple']:8.2f} {m['v_min_frac']:6.2f} " + f"{m['stuck_frac']:6.2f} {m['max_stair_deg']:7.3f}" + ) + print( + "\n v ripple = std(velocity)/|v_cmd| (MIT stick-slip ≈ 1.0, smooth < 0.2); " + "v min = slowest 50 ms window over |v_cmd| (0 = it stopped);\n" + " stuck = fraction of 50 ms windows with < 0.03° travel; stair = largest window travel beyond the commanded amount." + ) + + +def main() -> None: + p = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + add_side_and_channel_arguments(p) + p.add_argument( + "--joint", default="shoulder_1", choices=[j.value for j in ARM_JOINTS] + ) + p.add_argument( + "--center", + type=float, + default=-35.0, + help="Centre of the creep range, joint-frame degrees (default: -35)", + ) + p.add_argument( + "--amp", + type=float, + default=10.0, + help="Total travel per pass, degrees (default: 10)", + ) + p.add_argument( + "--speeds", + type=float, + nargs="+", + default=[2.0, 3.0, 6.0], + help="Creep speeds, deg/s (default: 2 3 6)", + ) + p.add_argument( + "--passes", + type=int, + default=2, + help="Passes per speed, alternating direction (default: 2)", + ) + p.add_argument( + "--cap", type=float, default=45.0, help="0xA4 speed cap, dps (default: 45)" + ) + p.add_argument( + "--rate", type=float, default=200.0, help="Command rate, Hz (default: 200)" + ) + p.add_argument( + "--accel", + type=int, + default=None, + help="Position-planner acceleration (dps/s) to run with: 0 asks for direct PI " + "tracking, otherwise velocity-profiled hops. Written to the motor for the run " + "and restored afterwards (default: leave the stored value)", + ) + p.add_argument("--csv", type=Path, default=None, help="Write every sample here") + args = p.parse_args() + asyncio.run(_run(args)) + + +if __name__ == "__main__": + main() diff --git a/scripts/fw_gains.py b/scripts/fw_gains.py new file mode 100644 index 00000000..994d7f5e --- /dev/null +++ b/scripts/fw_gains.py @@ -0,0 +1,149 @@ +"""Read or set a MyActuator joint's firmware loop gains, in RAM by default. + +The X8-P20 shoulders ship with a position loop of ~0.3 Hz bandwidth +(position_kp 0.008) and a speed loop (speed_kp 0.03, speed_ki 0.0001) that +cannot hold creep speed against the gearbox's velocity-weakening friction — +an 0xA4 replay of `slow_osc` tracked with a 4-7° lag and the same 2 Hz +velocity cycle the MIT frame shows. This is the sweep tool for that: write a +few gains, replay, read the trace, repeat. + +Writes go to RAM (0x31) unless ``--persist`` is given (0x32, ROM), so an +experiment is undone by a power cycle and a good set is committed +deliberately. Every write is read back (0x30) and printed. + +Usage: + uv run python scripts/fw_gains.py --r --id 1 # read + uv run python scripts/fw_gains.py --r --id 1 speed_kp=0.1 speed_ki=0.001 + uv run python scripts/fw_gains.py --r --id 1 position_kp=0.05 --persist + +Gains: current_kp current_ki speed_kp speed_ki position_kp position_ki position_kd +""" + +from __future__ import annotations + +import argparse +import asyncio +import struct + +from almond_axol.cli.motor import add_side_and_channel_arguments, resolve_channel +from almond_axol.motor.bus import CanBus +from almond_axol.motor.motor import make_driver +from almond_axol.motor.myactuator import _MA_PID_IDX, MyActuatorMotor + +_READ = 0x30 +_WRITE_RAM = 0x31 +_WRITE_ROM = 0x32 +_READ_ACCEL = 0x42 +_WRITE_ACCEL = 0x43 # RAM + ROM + + +async def _read_accel(driver: MyActuatorMotor) -> tuple[int, int]: + out = [] + for kind in (0x00, 0x01): + resp = await driver._request(bytes([_READ_ACCEL, kind, 0, 0, 0, 0, 0, 0])) + out.append(int(struct.unpack_from(" tuple[int, int]: + """Position-planner accel and decel, written raw (0 = direct tracking of a + streamed 0xA4 target, which wire_mode a4 needs). Persists in ROM.""" + for kind in (0x00, 0x01): + await driver._request( + bytes([_WRITE_ACCEL, kind, 0, 0]) + struct.pack(" float: + resp = await driver._request(bytes([_READ, _MA_PID_IDX[name], 0, 0, 0, 0, 0, 0])) + return float(struct.unpack_from(" None: + cmd = _WRITE_ROM if persist else _WRITE_RAM + await driver._request( + bytes([cmd, _MA_PID_IDX[name], 0, 0]) + struct.pack(" None: + writes: dict[str, float] = {} + for spec in args.gains: + name, _, raw = spec.partition("=") + if name not in _MA_PID_IDX or not raw: + raise SystemExit( + f"bad gain {spec!r}; want NAME=VALUE with NAME in {', '.join(_MA_PID_IDX)}" + ) + writes[name] = float(raw) + async with CanBus(resolve_channel(args)) as bus: + driver = make_driver(bus, args.id, kt=1.0) + if not isinstance(driver, MyActuatorMotor): + raise SystemExit(f"motor {args.id:#04x} is not a MyActuator") + before = {n: await _read(driver, n) for n in _MA_PID_IDX} + acc = await _read_accel(driver) + print(f"motor {args.id:#04x} gains now:") + for n, v in before.items(): + print(f" {n:12s} {v:.6g}") + print( + f" planner accel/decel {acc[0]}/{acc[1]} dps/s" + + ( + " (direct tracking)" + if acc[0] == 0 + else " (profiled — a streamed 0xA4 will not follow)" + ) + ) + if args.accel is not None and acc != (args.accel, args.accel): + got = await _write_accel(driver, args.accel) + print( + f" planner accel/decel {acc[0]}/{acc[1]} -> {got[0]}/{got[1]} dps/s (ROM)" + ) + if got[0] == 0: + print( + " ! this joint now executes a stored 0xA4 target at its speed cap the moment it wakes — set it back (--accel 5000) when done with wire_mode a4" + ) + if not writes: + return + for n, v in writes.items(): + await _write(driver, n, v, args.persist) + after = {n: await _read(driver, n) for n in _MA_PID_IDX} + print( + f"\nafter writing to {'ROM (persistent)' if args.persist else 'RAM (until power cycle)'}:" + ) + for n in writes: + flag = ( + "" + if abs(after[n] - writes[n]) < 1e-6 * max(1.0, abs(writes[n])) + else " ! not accepted" + ) + print(f" {n:12s} {before[n]:.6g} -> {after[n]:.6g}{flag}") + + +def main() -> None: + p = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + add_side_and_channel_arguments(p) + p.add_argument("--id", type=lambda x: int(x, 0), required=True, help="Motor CAN ID") + p.add_argument("gains", nargs="*", metavar="NAME=VALUE", help="Gains to write") + p.add_argument( + "--accel", + type=int, + default=None, + help="Also set the position-planner accel/decel (dps/s, ROM): 0 for the direct " + "tracking wire_mode a4 needs, 5000 to put a joint back", + ) + p.add_argument( + "--persist", + action="store_true", + help="Write to ROM (0x32) instead of RAM (0x31); survives a power cycle", + ) + asyncio.run(_run(p.parse_args())) + + +if __name__ == "__main__": + main() diff --git a/tests/test_axol_construction.py b/tests/test_axol_construction.py index 699fe958..70ecc9bc 100644 --- a/tests/test_axol_construction.py +++ b/tests/test_axol_construction.py @@ -347,6 +347,28 @@ def setUp(self) -> None: joint: self.enterContext(patch.object(motor, "disable", AsyncMock())) for joint, motor in self.arm.motors.items() } + # The enable-time firmware-gain provisioning talks to the motors over + # the (mocked) bus like the other bring-up writes stubbed below. + self.enterContext( + patch( + "almond_axol.motor.myactuator.MyActuatorMotor.ensure_rom_gains", + AsyncMock(return_value={}), + ) + ) + self.enterContext( + patch( + "almond_axol.motor.damiao.DamiaoMotor.ensure_rom_gains", + AsyncMock(return_value={}), + ) + ) + # ... and so does the held-joint check that precedes it (reads only). + for cls in ("myactuator.MyActuatorMotor", "damiao.DamiaoMotor"): + self.enterContext( + patch( + f"almond_axol.motor.{cls}.firmware_gain_mismatches", + AsyncMock(return_value={}), + ) + ) def _assert_only_cold_joints_torqued_off(self) -> None: for joint, disable in self.disables.items(): diff --git a/tests/test_can_purge.py b/tests/test_can_purge.py index 8bd9d014..8de2a7bb 100644 --- a/tests/test_can_purge.py +++ b/tests/test_can_purge.py @@ -19,7 +19,13 @@ from unittest.mock import AsyncMock, patch from almond_axol.cli.can import setup as can_setup -from almond_axol.constants import CAN_BRINGUP_SCRIPT, CAN_LEFT, CAN_RIGHT +from almond_axol.constants import ( + CAN_BASE, + CAN_BRINGUP_SCRIPT, + CAN_LEFT, + CAN_RESET_SCRIPT, + CAN_RIGHT, +) from almond_axol.utils import can_purge @@ -51,6 +57,15 @@ def test_grants_the_exact_commands_the_core_runs(self) -> None: for command in commands: self.assertTrue(command.startswith("/"), command) + def test_grants_the_arm_hub_usb_reset(self) -> None: + # safety.rs prefers the reset script on the arm channels: the hub + # firmware keeps frames a flap cannot reach. + commands = can_purge.purge_commands() + self.assertTrue( + any(command.endswith(f"bash {CAN_RESET_SCRIPT}") for command in commands), + commands, + ) + def test_grants_every_form_the_fallback_flap_issues(self) -> None: # bring_up_interfaces configures between the down and the up. A grant # covering only down/up lets a non-root backstop take the interfaces @@ -176,6 +191,16 @@ def test_unknown_when_tc_is_missing_or_fails(self) -> None: class PurgeStaleTxTest(unittest.TestCase): """What a bring-up does about frames the last session left queued.""" + def setUp(self) -> None: + # Neither generated script is installed, whatever this host (a robot + # included) has in /etc: the flap goes through bring_up_interfaces. + scratch = tempfile.TemporaryDirectory() + self.addCleanup(scratch.cleanup) + for name in ("CAN_BRINGUP_SCRIPT", "CAN_RESET_SCRIPT"): + patcher = patch.object(can_setup, name, Path(scratch.name) / name) + patcher.start() + self.addCleanup(patcher.stop) + def test_clean_queues_flap_nothing(self) -> None: with ( patch.object(can_setup, "_iface_present", return_value=True), @@ -311,11 +336,46 @@ def setUp(self) -> None: scratch = tempfile.TemporaryDirectory() self.addCleanup(scratch.cleanup) self.script = Path(scratch.name) / "startup.sh" + self.reset = Path(scratch.name) / "reset_adapter.sh" + # Absent unless a test installs it, whatever this host has in /etc. + patcher = patch.object(can_setup, "CAN_RESET_SCRIPT", self.reset) + patcher.start() + self.addCleanup(patcher.stop) def _installed(self) -> Path: self.script.write_text("#!/bin/bash\n") return self.script + def test_arm_channels_prefer_the_usb_reset_once_installed(self) -> None: + self.reset.write_text("#!/bin/bash\n") + runs: list[list[str]] = [] + with ( + patch.object(can_setup, "CAN_BRINGUP_SCRIPT", self._installed()), + patch.object( + can_setup, + "run_root", + lambda argv, **_kw: runs.append(argv) or _completed(0), + ), + patch.object(can_setup, "bring_up_interfaces", side_effect=AssertionError), + ): + can_setup._flap_for_purge([CAN_LEFT, CAN_RIGHT]) + self.assertEqual(runs, [["bash", str(self.reset)]]) + + def test_a_single_bus_keeps_the_bring_up_flap(self) -> None: + # The reset covers only the arm hub; the wheel bus is its own adapter. + self.reset.write_text("#!/bin/bash\n") + runs: list[list[str]] = [] + with ( + patch.object(can_setup, "CAN_BRINGUP_SCRIPT", self._installed()), + patch.object( + can_setup, + "run_root", + lambda argv, **_kw: runs.append(argv) or _completed(0), + ), + ): + can_setup._flap_for_purge([CAN_BASE]) + self.assertEqual(runs, [["bash", str(self.script)]]) + def test_prefers_the_bring_up_script_for_managed_channels(self) -> None: # One granted command, and the only ordering that takes the dual # adapter's two channels down and up together. diff --git a/tests/test_can_setup.py b/tests/test_can_setup.py index 211fe9f7..cd9107f2 100644 --- a/tests/test_can_setup.py +++ b/tests/test_can_setup.py @@ -3,6 +3,7 @@ import contextlib import io import os +import subprocess import tempfile import unittest from contextlib import redirect_stderr, redirect_stdout @@ -405,12 +406,16 @@ def test_paired_recovery_takes_both_channels_down_before_either_up(self) -> None ) def test_axol_rx_retry_does_not_reset_wheel_and_lift_buses_again(self) -> None: + # A host without the reset script: the retry is the plain pair cycle. + no_reset = Mock() + no_reset.exists.return_value = False with ( patch.object( setup, "_global_setup_lock", return_value=contextlib.nullcontext() ), patch.object(setup._LOCK_LOCAL, "depth", 1, create=True), patch.object(setup.Path, "exists", return_value=True), + patch.object(setup, "CAN_RESET_SCRIPT", no_reset), patch.object(setup, "run_root") as run_root, patch.object( setup, @@ -435,6 +440,80 @@ def test_axol_rx_retry_does_not_reset_wheel_and_lift_buses_again(self) -> None: [setup.CAN_LEFT, setup.CAN_RIGHT], force_cycle=True ) + def test_axol_rx_retry_usb_resets_the_hub_once_installed(self) -> None: + # The hub firmware can release frames it held from a stalled session + # on any pair cycle; the retry resets the device instead. + with ( + patch.object( + setup, "_global_setup_lock", return_value=contextlib.nullcontext() + ), + patch.object(setup._LOCK_LOCAL, "depth", 1, create=True), + patch.object(setup.Path, "exists", return_value=True), + patch.object(setup, "run_root") as run_root, + patch.object( + setup, + "rx_alive_per_arm", + side_effect=[(False, False), (True, True)], + ), + patch.object(setup, "bring_up_interfaces", side_effect=AssertionError), + contextlib.redirect_stdout(io.StringIO()), + ): + setup.bring_up_can(setup._AXOL_PROFILE) + + flagged = ["env", f"{setup._GLOBAL_LOCK_ENV}=1", "bash"] + self.assertEqual( + run_root.call_args_list, + [ + call([*flagged, str(setup._AXOL_PROFILE.cron_script)], check=True), + call([*flagged, str(setup.CAN_RESET_SCRIPT)], check=True), + ], + ) + + def test_only_the_arm_hub_pair_is_usb_reset(self) -> None: + with ( + patch.object(setup.Path, "exists", return_value=True), + patch.object(setup, "run_root", side_effect=AssertionError), + patch.object(setup, "bring_up_interfaces") as bring_up, + contextlib.redirect_stdout(io.StringIO()), + ): + setup._recover_hub_pair([setup.CAN_MANTIS_LEFT, setup.CAN_MANTIS_RIGHT]) + bring_up.assert_called_once_with( + [setup.CAN_MANTIS_LEFT, setup.CAN_MANTIS_RIGHT], force_cycle=True + ) + + def test_reset_script_resets_the_hub_then_runs_the_bring_up(self) -> None: + text = setup._reset_script_text() + self.assertTrue(text.startswith("#!/bin/bash\n")) + self.assertIn("set -euo pipefail", text) + # The hub behind the arm channels, found through sysfs, reset once. + self.assertIn("/sys/class/net/${IFACE}/device/..", text) + self.assertIn(f"for IFACE in {setup.CAN_LEFT} {setup.CAN_RIGHT}; do", text) + self.assertIn('usbreset "$(printf "%03d/%03d"', text) + # The sysfs fallback where usbreset is missing or fails. + self.assertIn('echo 0 > "${HUB}/authorized"', text) + self.assertIn('echo 1 > "${HUB}/authorized"', text) + # Lock handling matches the bring-up script, which it hands over to + # with the global lock still held. + self.assertIn(f'exec 8<"{setup._GLOBAL_LOCK_FILE}"', text) + self.assertTrue( + text.rstrip().endswith( + f"exec env {setup._GLOBAL_LOCK_ENV}=1 bash " + f'"{setup._AXOL_PROFILE.cron_script}"' + ) + ) + # The single-channel wheel/chest adapters are never reset. + self.assertNotIn(setup.CAN_BASE, text) + self.assertNotIn(setup.CAN_CHEST, text) + + def test_reset_script_passes_bash_syntax_check(self) -> None: + with tempfile.NamedTemporaryFile("w", suffix=".sh") as script: + script.write(setup._reset_script_text()) + script.flush() + checked = subprocess.run( + ["bash", "-n", script.name], capture_output=True, text=True + ) + self.assertEqual(checked.returncode, 0, checked.stderr) + def test_root_executed_scripts_install_root_owned_outside_operator_state( self, ) -> None: @@ -612,6 +691,7 @@ def test_apply_setup_does_not_swallow_rp1_security_cleanup_failure(self) -> None with ( patch.object(setup, "_write_udev_rules"), patch.object(setup, "_write_cron_script"), + patch.object(setup, "_write_reset_script"), patch.object(setup, "_write_hotplug_unit"), patch.object(setup, "_reload_udev"), patch.object(setup, "_rename_interfaces"), diff --git a/tests/test_cogging.py b/tests/test_cogging.py new file mode 100644 index 00000000..87673adb --- /dev/null +++ b/tests/test_cogging.py @@ -0,0 +1,148 @@ +"""Cogging ("osc") cancellation: the Fourier fit from a friction sweep, its +calibration-file round trip, and the motor-frame series the core evaluates.""" + +from __future__ import annotations + +import json +import math +import tempfile +import unittest +from pathlib import Path + +import numpy as np + +from almond_axol.robot.calibration import ( + clean_cogging, + load_calibration, + update_joint_calibration, +) +from almond_axol.robot.config import ( + CoggingModel, + FrictionParams, + JointConfig, + _calibrated_joint, +) +from almond_axol.tuning.cogging import angle_highpass, fit_cogging, prediction_r + + +def _sweep(model: CoggingModel, seed: int, noise: float = 0.2): + """A bidirectional constant-speed sweep over -50..-20°: gravity (a slow + sine), Coulomb friction flipping with direction, the ripple, noise.""" + rng = np.random.default_rng(seed) + q_fwd = np.radians(np.linspace(-50, -20, 3000)) + q = np.concatenate([q_fwd, q_fwd[::-1]]) + direction = np.array(["+"] * len(q_fwd) + ["-"] * len(q_fwd)) + gravity = -14.0 * np.cos(q) + friction = np.where(direction == "+", 1.3, -1.3) + tau = gravity + friction + model.torque(q) + noise * rng.standard_normal(len(q)) + return q, tau, direction + + +class CoggingModelTest(unittest.TestCase): + MODEL = CoggingModel(3.62, ((1, 0.03, -0.02), (2, 0.45, -0.25), (4, 0.05, 0.1))) + + def test_motor_terms_reproduce_the_joint_series_at_joint_equals_motor_plus_offset( + self, + ) -> None: + for offset in (0.0, 0.37, -1.2): + terms = self.MODEL.motor_terms(offset, gain=0.8) + for q_motor in np.linspace(-1.0, 1.0, 7): + got = sum( + a * math.cos(w * q_motor) + b * math.sin(w * q_motor) + for w, a, b in terms + ) + self.assertAlmostEqual( + got, 0.8 * float(self.MODEL.torque(q_motor + offset)), places=10 + ) + + def test_dict_round_trip_and_calibration_overlay(self) -> None: + self.assertEqual(CoggingModel.from_dict(self.MODEL.as_dict()), self.MODEL) + jc = JointConfig( + kp=1.0, + kd=0.1, + friction=FrictionParams(fc=0.0, k=0.0, fv=0.0, fo=0.0), + mass=1.0, + com=(0.0, 0.0, 0.0), + ) + got = _calibrated_joint(jc, {"cogging": self.MODEL.as_dict()}) + self.assertEqual(got.cogging, self.MODEL) + self.assertEqual(got.cogging_gain, 1.0) + + +class FitTest(unittest.TestCase): + TRUE = CoggingModel(3.62, ((1, 0.02, 0.0), (2, 0.5, -0.2), (4, 0.0, 0.1))) + + def test_the_fit_recovers_the_ripple_through_gravity_and_friction(self) -> None: + fit = fit_cogging(*_sweep(self.TRUE, 0)) + for (k, a, b), (k0, a0, b0) in zip(fit.model.harmonics, self.TRUE.harmonics): + self.assertEqual(k, k0) + self.assertAlmostEqual(a, a0, delta=0.03) + self.assertAlmostEqual(b, b0, delta=0.03) + self.assertGreater(fit.r2, 0.7) + # Another pass (other noise) is predicted out of sample. + self.assertGreater(prediction_r(fit.model, *_sweep(self.TRUE, 1)), 0.8) + + def test_pure_noise_predicts_nothing(self) -> None: + empty = CoggingModel(3.62, ((1, 0.0, 0.0),)) + fit = fit_cogging(*_sweep(empty, 2)) + self.assertLess(fit.r2, 0.05) + self.assertLess(abs(prediction_r(fit.model, *_sweep(empty, 3))), 0.1) + + def test_too_little_travel_is_refused(self) -> None: + q = np.radians(np.linspace(0, 5, 200)) + with self.assertRaisesRegex(ValueError, "too little travel"): + fit_cogging(q, np.zeros_like(q), np.array(["+"] * len(q))) + self.assertEqual(len(angle_highpass(np.array([1.0]), np.array([1.0]))[0]), 0) + + +class CalibrationFileTest(unittest.TestCase): + def test_a_saved_series_loads_back_and_a_bad_one_is_ignored(self) -> None: + with tempfile.TemporaryDirectory() as d: + path = Path(d) / "calibration.json" + update_joint_calibration( + "right", + "shoulder_1", + cogging=FitTest.TRUE.as_dict(), + hub_serial="hub1", + path=path, + ) + got = load_calibration(path, expected_hub_serial="hub1") + self.assertEqual( + CoggingModel.from_dict(got["right"]["shoulder_1"]["cogging"]), + FitTest.TRUE, + ) + raw = json.loads(path.read_text()) + raw["right"]["elbow"] = {"cogging": {"period_deg": -1, "harmonics": []}} + path.write_text(json.dumps(raw)) + got = load_calibration(path, expected_hub_serial="hub1") + self.assertNotIn("elbow", got["right"]) + with self.assertRaises(ValueError): + update_joint_calibration( + "right", + "elbow", + cogging={"period_deg": 3.6, "harmonics": [[0, 1, 1]]}, + hub_serial="hub1", + path=path, + ) + + def test_clean_cogging_rejects_malformed_series(self) -> None: + good = {"period_deg": 3.62, "harmonics": [[1, 0.1, 0.2], [2.0, -0.3, 0.0]]} + self.assertEqual( + clean_cogging(good), + {"period_deg": 3.62, "harmonics": [[1, 0.1, 0.2], [2, -0.3, 0.0]]}, + ) + for bad in ( + None, + {"period_deg": 0, "harmonics": [[1, 0, 0]]}, + {"period_deg": 3.6, "harmonics": []}, + {"period_deg": 3.6, "harmonics": [[1.5, 0, 0]]}, + {"period_deg": 3.6, "harmonics": [[1, float("nan"), 0]]}, + {"period_deg": 3.6, "harmonics": [[float("inf"), 0, 0]]}, + {"period_deg": 3.6, "harmonics": [[1, 0]]}, + ): + with self.subTest(bad=bad): + self.assertIsNone(clean_cogging(bad)) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/test_command_sections.py b/tests/test_command_sections.py index ac35b881..9fac3721 100644 --- a/tests/test_command_sections.py +++ b/tests/test_command_sections.py @@ -14,6 +14,7 @@ # dashboard renders no other grid for them. Keep this list honest. _CLI_ONLY_TUNING = {"tune.filter"} _WORKBENCH_TABS = { + "tune.a4", "tune.pid", "tune.friction", "tune.gravity", diff --git a/tests/test_controller_option.py b/tests/test_controller_option.py new file mode 100644 index 00000000..ffff98f0 --- /dev/null +++ b/tests/test_controller_option.py @@ -0,0 +1,524 @@ +"""``AxolConfig.controller``: impedance (MIT, 240 Hz) or the firmware position +loops (a4 / pv, 400 Hz) — baked into the per-joint wire modes at +construction, the core's loop rate and wire tokens derived from it.""" + +from __future__ import annotations + +import argparse +import unittest +from dataclasses import replace +from typing import Any +from unittest.mock import patch + +from almond_axol.cli.tune import motion as tune_motion +from almond_axol.constants import ARM_JOINTS, Joint +from almond_axol.robot.axol import AxolHardware +from almond_axol.robot.config import ( + CONTROLLER_LOOP_HZ, + CONTROLLERS, + FAST_IMPEDANCE_HZ, + IMPEDANCE_LOOP_HZ, + IMPEDANCE_RATES, + MIXED_LOOP_HZ, + AxolConfig, + CoggingModel, + check_loop_hz, + impedance_joints, + position_wire_mode, +) +from almond_axol.rt import Axol +from almond_axol.serve.introspect import _KNOWN_OPTIONS + +_MYACTUATOR = ( + Joint.SHOULDER_1, + Joint.SHOULDER_2, + Joint.SHOULDER_3, + Joint.ELBOW, + Joint.WRIST_1, +) +_DAMIAO = (Joint.WRIST_2, Joint.WRIST_3) + + +class ConfigTest(unittest.TestCase): + def test_impedance_is_the_default_and_leaves_wire_modes_alone(self) -> None: + cfg = AxolConfig() + self.assertEqual(cfg.controller, "impedance") + self.assertEqual(cfg.loop_hz, 240.0) + resolved = cfg.resolved() + for arm in (resolved.left, resolved.right): + for j in ARM_JOINTS: + self.assertEqual(getattr(arm, j.value).wire_mode, "mit") + + def test_position_puts_every_joint_on_its_vendors_loop_at_400_hz(self) -> None: + cfg = AxolConfig(controller="position") + self.assertEqual(cfg.loop_hz, 400.0) + resolved = cfg.resolved() + self.assertEqual(resolved.controller, "position") + for arm in (resolved.left, resolved.right): + for j in _MYACTUATOR: + self.assertEqual(getattr(arm, j.value).wire_mode, "a4", j) + for j in _DAMIAO: + self.assertEqual(getattr(arm, j.value).wire_mode, "pv", j) + # Idempotent, like the stiffness blend. + self.assertEqual(resolved.resolved(), resolved) + + def test_position_wire_mode_follows_the_motor_vendor(self) -> None: + for j in _MYACTUATOR: + self.assertEqual(position_wire_mode(j), "a4") + for j in _DAMIAO: + self.assertEqual(position_wire_mode(j), "pv") + + def test_an_explicit_a4_joint_survives_under_impedance(self) -> None: + cfg = AxolConfig() + cfg.right.shoulder_1.wire_mode = "a4" + resolved = cfg.resolved() + self.assertEqual(resolved.right.shoulder_1.wire_mode, "a4") + self.assertEqual(resolved.right.shoulder_2.wire_mode, "mit") + # A mixed arm runs the core at 480 Hz, impedance on alternate ticks. + self.assertEqual(resolved.loop_hz, MIXED_LOOP_HZ) + + def test_the_rate_rule_counts_only_arm_joints_on_mit(self) -> None: + self.assertEqual(IMPEDANCE_LOOP_HZ, 240.0) + # The position controller puts every arm joint on a firmware loop; + # the gripper, always MIT, does not count. + self.assertEqual(impedance_joints(AxolConfig(controller="position")), []) + check_loop_hz(AxolConfig(controller="position"), 400.0) + check_loop_hz(AxolConfig(controller="position"), 240.0) + check_loop_hz(AxolConfig(), 240.0) + mit = impedance_joints(AxolConfig()) + self.assertEqual(len(mit), 2 * len(ARM_JOINTS)) + self.assertIn("right.shoulder_3", mit) + with self.assertRaisesRegex(ValueError, "left.shoulder_1"): + check_loop_hz(AxolConfig(), 400.0) + check_loop_hz(AxolConfig(), MIXED_LOOP_HZ) + self.assertEqual(MIXED_LOOP_HZ, 2 * IMPEDANCE_LOOP_HZ) + + def test_the_default_rate_follows_the_wire_mode_mix(self) -> None: + self.assertEqual(AxolConfig().loop_hz, 240.0) + self.assertEqual(AxolConfig(controller="position").loop_hz, 400.0) + mixed = AxolConfig() + mixed.right.shoulder_1.wire_mode = "a4" + mixed.right.elbow.wire_mode = "a4" + self.assertEqual(mixed.loop_hz, 480.0) + check_loop_hz(mixed, mixed.loop_hz) + + def test_fast_impedance_runs_the_loop_at_480_only(self) -> None: + self.assertEqual(IMPEDANCE_RATES, (240.0, 480.0)) + fast = AxolConfig(impedance_hz=FAST_IMPEDANCE_HZ) + self.assertEqual(fast.loop_hz, 480.0) + check_loop_hz(fast, 480.0) + with self.assertRaisesRegex(ValueError, "480 Hz only"): + check_loop_hz(fast, 240.0) + # Mixed or all-impedance alike: any impedance joint makes it 480. + fast.right.shoulder_1.wire_mode = "a4" + self.assertEqual(fast.loop_hz, 480.0) + # No impedance joint left: the rule does not apply. + pos = AxolConfig(controller="position", impedance_hz=FAST_IMPEDANCE_HZ) + self.assertEqual(pos.loop_hz, 400.0) + check_loop_hz(pos, 400.0) + with self.assertRaisesRegex(ValueError, "impedance_hz 300"): + AxolConfig(impedance_hz=300.0).resolved() + + def test_single_joints_can_run_impedance_at_480(self) -> None: + from almond_axol.robot.config import fast_impedance_joints + + cfg = AxolConfig() + self.assertEqual(fast_impedance_joints(cfg), []) + cfg.right.shoulder_1.impedance_hz = 480.0 + cfg.right.elbow.impedance_hz = 480.0 + self.assertEqual( + fast_impedance_joints(cfg), ["right.shoulder_1", "right.elbow"] + ) + self.assertEqual(cfg.loop_hz, 480.0) + check_loop_hz(cfg, 480.0) + with self.assertRaisesRegex(ValueError, "right.shoulder_1 running impedance"): + check_loop_hz(cfg, 240.0) + # A joint on its firmware loop is not an impedance joint at any rate. + cfg.right.shoulder_1.wire_mode = "a4" + self.assertEqual(fast_impedance_joints(cfg), ["right.elbow"]) + # The config-wide 480 covers the MyActuator joints, not the wrists; a + # joint's own 240 opts it out. + wide = AxolConfig(impedance_hz=480.0) + wide.left.elbow.impedance_hz = 240.0 + fast = fast_impedance_joints(wide) + self.assertIn("left.shoulder_1", fast) + self.assertNotIn("left.elbow", fast) + self.assertNotIn("left.wrist_2", fast) + bad = AxolConfig() + bad.left.elbow.impedance_hz = 400.0 + with self.assertRaisesRegex(ValueError, "left.elbow.impedance_hz 400"): + bad.resolved() + + def test_the_rated_current_is_host_side_and_must_be_positive(self) -> None: + from almond_axol.robot.config import FirmwareGains + + fw = FirmwareGains(position_kp=1.0, tf_rated_current_a=12.0) + # Never written to the motor: not a ROM parameter. + self.assertEqual(fw.as_dict(), {"position_kp": 1.0}) + for bad in (0.0, -3.0, float("nan")): + with self.subTest(bad=bad), self.assertRaises(ValueError): + FirmwareGains(tf_rated_current_a=bad) + + def test_unknown_controller_is_refused(self) -> None: + with self.assertRaises(ValueError): + AxolConfig(controller="velocity").resolved() + + def test_rates_are_pinned(self) -> None: + self.assertEqual(CONTROLLERS, ("impedance", "position")) + self.assertEqual(CONTROLLER_LOOP_HZ, {"impedance": 240.0, "position": 400.0}) + # The dashboard's draccus forms render the field as a select. + self.assertEqual(_KNOWN_OPTIONS["controller"], list(CONTROLLERS)) + + +class _FakeBus: + def __init__(self, channel: str) -> None: + self._channel = channel + + async def close(self) -> None: + return None + + +class _FakeDriver: + kp_max = 500.0 + kd_max = 5.0 + + def __init__(self, *_a: Any, **_k: Any) -> None: + pass + + def set_feedback_callback(self, _cb: Any) -> None: + pass + + +def _hardware(config: AxolConfig) -> AxolHardware: + with ( + patch("almond_axol.robot.axol.CanBus", _FakeBus), + patch( + "almond_axol.motor.motor.make_driver", + side_effect=lambda *_a, **_k: _FakeDriver(), + ), + ): + return AxolHardware(config=config, left_channel="can0", right_channel=None) + + +class RealtimeConfigTest(unittest.TestCase): + def setUp(self) -> None: + # Only the config text is exercised; no core is spawned, so CI needs + # no built axol-rt binary. + patcher = patch("almond_axol.rt.link.find_binary", return_value="/fake/axol-rt") + patcher.start() + self.addCleanup(patcher.stop) + + def _joint_tokens(self, rt: Axol) -> dict[str, str]: + out = {} + for line in rt._config_text().splitlines(): + f = line.split() + if f and f[0] == "joint": + out[f[3]] = f[18] + return out + + def test_impedance_core_runs_at_240_on_mit(self) -> None: + rt = Axol._wrap(_hardware(AxolConfig())) + lines = rt._config_text().splitlines() + self.assertIn("loop_hz 240.0", lines) + self.assertEqual(set(self._joint_tokens(rt).values()), {"mit"}) + + def test_position_core_runs_at_400_with_a4_and_pv_tokens(self) -> None: + rt = Axol._wrap(_hardware(AxolConfig(controller="position"))) + lines = rt._config_text().splitlines() + self.assertIn("loop_hz 400.0", lines) + tokens = self._joint_tokens(rt) + for j in _MYACTUATOR: + self.assertEqual(tokens[j.value], "a4") + for j in _DAMIAO: + self.assertEqual(tokens[j.value], "pv") + + def test_an_explicit_loop_rate_still_wins(self) -> None: + rt = Axol._wrap(_hardware(AxolConfig(controller="position")), loop_hz=240.0) + self.assertIn("loop_hz 240.0", rt._config_text().splitlines()) + + def test_impedance_joints_are_held_to_240_hz(self) -> None: + # The run that shook the arm: impedance with two joints on --a4, at + # 400 Hz. Refused before any core starts. + cfg = AxolConfig() + cfg.right.shoulder_1.wire_mode = "a4" + cfg.right.elbow.wire_mode = "a4" + with self.assertRaisesRegex(ValueError, "impedance runs at 240 Hz only"): + Axol._wrap(_hardware(cfg), loop_hz=400.0) + # All-impedance at another rate is refused the same way. + with self.assertRaisesRegex(ValueError, "240 Hz only"): + Axol._wrap(_hardware(AxolConfig()), loop_hz=300.0) + # At 240 the mixed split runs, and by default at 480 — every + # impedance joint on alternate ticks, so still 240 Hz each. + rt = Axol._wrap(_hardware(cfg), loop_hz=240.0) + self.assertIn("loop_hz 240.0", rt._config_text().splitlines()) + rt = Axol._wrap(_hardware(cfg)) + self.assertIn("loop_hz 480.0", rt._config_text().splitlines()) + + def test_the_core_gets_the_impedance_rate(self) -> None: + rt = Axol._wrap(_hardware(AxolConfig())) + self.assertIn("impedance_hz 240.0", rt._config_text().splitlines()) + rt = Axol._wrap(_hardware(AxolConfig(impedance_hz=480.0))) + lines = rt._config_text().splitlines() + self.assertIn("impedance_hz 480.0", lines) + self.assertIn("loop_hz 480.0", lines) + with self.assertRaisesRegex(ValueError, "480 Hz only"): + Axol._wrap(_hardware(AxolConfig(impedance_hz=480.0)), loop_hz=240.0) + + def test_the_core_gets_each_joints_impedance_rate(self) -> None: + cfg = AxolConfig() + cfg.left.shoulder_1.impedance_hz = 480.0 + rt = Axol._wrap(_hardware(cfg)) + lines = rt._config_text().splitlines() + self.assertIn("loop_hz 480.0", lines) + rate = { + f[3]: float(f[28]) + for f in (ln.split() for ln in lines) + if f and f[0] == "joint" + } + self.assertEqual(rate["shoulder_1"], 480.0) + self.assertEqual(rate["elbow"], 0.0) + + def test_the_core_gets_the_0x73_scale_from_the_rated_current(self) -> None: + cfg = AxolConfig() + cfg.left.shoulder_1.wire_mode = "a4" + cfg.left.shoulder_1.firmware = replace( + cfg.left.shoulder_1.firmware, tf_rated_current_a=12.0 + ) + cfg.left.wrist_2.firmware = replace( + cfg.left.wrist_2.firmware, tf_rated_current_a=3.0 + ) + rt = Axol._wrap(_hardware(cfg)) + scale = { + f[3]: float(f[27]) + for f in (ln.split() for ln in rt._config_text().splitlines()) + if f and f[0] == "joint" + } + # kt (2 Nm/A on shoulder_1) × 12 A / 100 = Nm per 1%. + self.assertAlmostEqual(scale["shoulder_1"], 0.24) + self.assertEqual(scale["shoulder_2"], 0.0) + # A Damiao wrist has no 0x73: never a scale, whatever is configured. + self.assertEqual(scale["wrist_2"], 0.0) + + def test_cogging_lines_arrive_only_with_the_offsets(self) -> None: + import math + + cfg = AxolConfig() + model = CoggingModel(3.62, ((1, 0.03, 0.0), (2, 0.4, -0.2))) + cfg.left.shoulder_1.cogging = model + cfg.left.shoulder_1.cogging_gain = 0.5 + cfg.left.elbow.cogging = model + cfg.left.elbow.cogging_gain = 0.0 # off: no line at all + rt = Axol._wrap(_hardware(cfg)) + self.assertFalse( + [ln for ln in rt._config_text().splitlines() if ln.startswith("cogging")] + ) + lines = [ + ln.split() + for ln in rt._config_text(cogging=True).splitlines() + if ln.startswith("cogging") + ] + self.assertEqual(len(lines), 1) + f = lines[0] + self.assertEqual(f[:5], ["cogging", "0", "can0", "1", "2"]) + # The motor-frame series, halved, reproduces the joint-frame one at + # joint = motor + offset. + offset = float(rt._robot.left._joint_offsets[0]) + terms = [tuple(map(float, f[5 + 3 * k : 8 + 3 * k])) for k in range(2)] + for q_motor in (-0.4, 0.1, 0.77): + got = sum( + a * math.cos(w * q_motor) + b * math.sin(w * q_motor) + for w, a, b in terms + ) + self.assertAlmostEqual( + got, 0.5 * float(model.torque(q_motor + offset)), places=9 + ) + + def test_a_vendor_mismatched_wire_mode_is_refused(self) -> None: + cfg = AxolConfig() + cfg.left.wrist_2.wire_mode = "a4" # a Damiao wrist has no 0xA4 + rt = Axol._wrap(_hardware(cfg)) + with self.assertRaisesRegex(ValueError, "wrist_2.*Damiao|not a MyActuator"): + rt._config_text() + cfg = AxolConfig() + cfg.left.elbow.wire_mode = "pv" # a MyActuator joint has no pos-vel frame + rt = Axol._wrap(_hardware(cfg)) + with self.assertRaisesRegex(ValueError, "elbow"): + rt._config_text() + + +class TuneMotionFlagTest(unittest.TestCase): + def _parse(self, *argv: str) -> argparse.Namespace: + parser = argparse.ArgumentParser() + sub = parser.add_subparsers() + tune_motion.add_parser(sub) + return parser.parse_args(["tune.motion", "--motion", "slow_osc", *argv]) + + def test_loop_hz_override_and_firmware_gain_overrides_parse(self) -> None: + ns = self._parse( + "--loop-hz", "240", "--gain", "right.elbow.firmware.speed_kp=0.03" + ) + self.assertEqual(ns.loop_hz, 240.0) + overrides = tune_motion._parse_gain_overrides(ns.gain) + self.assertEqual(overrides, {("right", "elbow", "firmware.speed_kp"): 0.03}) + both = tune_motion._parse_gain_overrides(["wrist_2.firmware.profile_acc=200"]) + self.assertEqual( + both, + { + ("left", "wrist_2", "firmware.profile_acc"): 200.0, + ("right", "wrist_2", "firmware.profile_acc"): 200.0, + }, + ) + with self.assertRaises(SystemExit): + tune_motion._parse_gain_overrides(["elbow.firmware.bogus=1"]) + + def test_hold_freezes_a_column_at_the_start_or_a_given_angle(self) -> None: + import math + + import numpy as np + + holds = tune_motion._parse_holds(["right.elbow", "right.wrist_2=10"]) + elbow = tune_motion._COLUMNS.index("right.elbow") + wrist_2 = tune_motion._COLUMNS.index("right.wrist_2") + self.assertEqual(holds[elbow], None) + self.assertAlmostEqual(holds[wrist_2], math.radians(10)) + rows = np.arange(3 * 14, dtype=float).reshape(3, 14) + held = tune_motion._apply_holds(rows, holds, rows[0]) + np.testing.assert_array_equal(held[:, elbow], rows[0, elbow]) + np.testing.assert_allclose(held[:, wrist_2], math.radians(10)) + # Every other column still follows the motion; the input is untouched. + other = [i for i in range(14) if i not in (elbow, wrist_2)] + np.testing.assert_array_equal(held[:, other], rows[:, other]) + self.assertEqual(rows[1, elbow], 14 + elbow) + self.assertEqual(self._parse("--hold", "right.elbow").hold, ["right.elbow"]) + + def test_hold_refuses_what_it_cannot_do(self) -> None: + for spec, message in { + "elbow": "SIDE.JOINT", + "right.hip": "SIDE.JOINT", + "right.elbow=bent": "bad angle", + "right.elbow=45": "outside", # the right elbow is -150..0 + }.items(): + with self.subTest(spec=spec), self.assertRaisesRegex(SystemExit, message): + tune_motion._parse_holds([spec]) + + def test_an_override_touches_only_its_own_joint(self) -> None: + # shoulder_1 and shoulder_2 share one firmware-gains object (and the + # zero-friction joints one friction object) in the config; an + # override on one joint must not reach the others, or the next config. + cfg = AxolConfig() + tune_motion._apply_gain_overrides( + cfg, + tune_motion._parse_gain_overrides( + [ + "right.shoulder_1.firmware.planner_accel=60000", + "right.elbow.friction.fc=0.3", + "right.shoulder_1.kd=2.0", + ] + ), + ) + self.assertEqual(cfg.right.shoulder_1.firmware.planner_accel, 60000.0) + self.assertEqual(cfg.right.shoulder_2.firmware.planner_accel, 0.0) + self.assertEqual(cfg.left.shoulder_1.firmware.planner_accel, 0.0) + self.assertEqual(cfg.right.elbow.friction.fc, 0.3) + self.assertEqual( + cfg.left.elbow.friction.fc, AxolConfig().left.elbow.friction.fc + ) + self.assertEqual( + cfg.right.wrist_2.friction.fc, AxolConfig().right.wrist_2.friction.fc + ) + self.assertEqual(cfg.right.shoulder_1.kd, 2.0) + self.assertEqual(AxolConfig().right.shoulder_1.firmware.planner_accel, 0.0) + + def test_planner_overrides_are_checked_before_the_bus(self) -> None: + got = tune_motion._parse_gain_overrides( + [ + "right.elbow.firmware.planner_accel=60000", + "right.elbow.firmware.cap_track=1.2", + ] + ) + self.assertEqual(got[("right", "elbow", "firmware.planner_accel")], 60000.0) + for spec, message in { + "right.elbow.firmware.planner_accel=5000": "barely moves", + "right.elbow.firmware.cap_track=0.5": "never keeps up", + "right.wrist_2.firmware.planner_accel=60000": "Damiao wrist", + }.items(): + with self.subTest(spec=spec), self.assertRaisesRegex(SystemExit, message): + tune_motion._parse_gain_overrides([spec]) + + def test_the_core_gets_each_joints_cap_track(self) -> None: + cfg = AxolConfig() + cfg.left.elbow.wire_mode = "a4" + cfg.left.elbow.firmware.cap_track = 1.2 + cfg.left.elbow.firmware.planner_lead_ms = 5.0 + with patch("almond_axol.rt.link.find_binary", return_value="/fake/axol-rt"): + rt = Axol._wrap(_hardware(cfg)) + caps = { + f[3]: (f[25], f[26]) + for f in (ln.split() for ln in rt._config_text().splitlines()) + if f[0] == "joint" + } + self.assertEqual(caps["elbow"], ("1.2", "5.0")) + self.assertEqual(caps["shoulder_1"], ("0.0", "0.0")) + + def test_repeat_defaults_to_one_pass(self) -> None: + self.assertEqual(self._parse().repeat, 1) + self.assertEqual(self._parse("--repeat", "5").repeat, 5) + self.assertEqual(self._parse("--repeat", "0").repeat, 0) # until Ctrl-C + + def test_impedance_rate_flag_and_the_new_gain_fields(self) -> None: + self.assertIsNone(self._parse().impedance_hz) + self.assertEqual(self._parse("--impedance-hz", "480").impedance_hz, 480.0) + with self.assertRaises(SystemExit): + self._parse("--impedance-hz", "400") + self.assertFalse(self._parse().no_imu) + self.assertEqual( + self._parse( + "--fast-impedance", + "right.shoulder_1", + "--fast-impedance", + "right.elbow", + ).fast_impedance, + ["right.shoulder_1", "right.elbow"], + ) + self.assertTrue(self._parse("--no-imu").no_imu) + got = tune_motion._parse_gain_overrides( + [ + "right.shoulder_1.firmware.tf_rated_current_a=12", + "right.shoulder_1.cogging_gain=0.5", + ] + ) + cfg = AxolConfig() + tune_motion._apply_gain_overrides(cfg, got) + self.assertEqual(cfg.right.shoulder_1.firmware.tf_rated_current_a, 12.0) + self.assertIsNone(cfg.right.shoulder_2.firmware.tf_rated_current_a) + self.assertEqual(cfg.right.shoulder_1.cogging_gain, 0.5) + grav = tune_motion._parse_gain_overrides( + ["right.shoulder_3.com.z=-0.17", "right.wrist_3.mass=0.9"] + ) + cfg2 = AxolConfig() + tune_motion._apply_gain_overrides(cfg2, grav) + self.assertEqual(cfg2.right.shoulder_3.com[2], -0.17) + self.assertEqual( + cfg2.right.shoulder_3.com[:2], AxolConfig().right.shoulder_3.com[:2] + ) + self.assertEqual(cfg2.left.shoulder_3.com, AxolConfig().left.shoulder_3.com) + self.assertEqual(cfg2.right.wrist_3.mass, 0.9) + for spec, message in { + "right.wrist_2.firmware.tf_rated_current_a=3": "Damiao wrist", + "right.elbow.firmware.tf_rated_current_a=0": "> 0", + }.items(): + with self.subTest(spec=spec), self.assertRaisesRegex(SystemExit, message): + tune_motion._parse_gain_overrides([spec]) + + def test_controller_flag_takes_the_two_laws_and_defaults_to_config(self) -> None: + self.assertIsNone(self._parse().controller) + self.assertEqual(self._parse("--controller", "position").controller, "position") + self.assertEqual( + self._parse("--controller", "impedance").controller, "impedance" + ) + with self.assertRaises(SystemExit): + self._parse("--controller", "velocity") + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/test_firmware_gains.py b/tests/test_firmware_gains.py new file mode 100644 index 00000000..ec9b9f77 --- /dev/null +++ b/tests/test_firmware_gains.py @@ -0,0 +1,513 @@ +"""Firmware loop gains: config carriage, the idempotent ROM write on the +MyActuator driver, and the enable-time hook that applies them to cold joints.""" + +from __future__ import annotations + +import struct +import unittest +from dataclasses import replace +from types import SimpleNamespace +from unittest.mock import MagicMock, patch + +from almond_axol.constants import Joint +from almond_axol.motor import MotorError +from almond_axol.motor.damiao import DamiaoMotor +from almond_axol.motor.myactuator import _MA_PID_IDX, MyActuatorMotor +from almond_axol.robot import FirmwareGains, JointConfig +from almond_axol.robot.axol import apply_firmware_gains +from almond_axol.robot.config import ( + _X6_ELBOW_FIRMWARE_GAINS, + _X8_FIRMWARE_GAINS, + AxolConfig, + _calibrated_joint, +) + +_X8 = { + "position_kp": 1.0, + "position_kd": 0.1, + "speed_kp": 0.07, + "speed_ki": 1e-5, + "planner_accel": 0.0, +} + + +class ConfigTest(unittest.TestCase): + def test_every_myactuator_joint_carries_its_stock_set(self) -> None: + # The arms run impedance, where the firmware loops are inert: every + # joint — shoulder_1 and the elbow included — is kept on its motor's + # factory loop (2026-09-24). The tuned 0xA4 sets stay defined for an + # --a4 run to override with. + x8 = { + "position_kp": 0.008, + "position_kd": 0.1, + "speed_kp": 0.03, + "speed_ki": 1e-4, + "planner_accel": 0.0, + } + x6 = { + "position_kp": 0.06, + "position_kd": 0.5, + "speed_kp": 0.01, + "speed_ki": 1e-4, + "planner_accel": 0.0, + } + cfg = AxolConfig() + for arm in (cfg.left, cfg.right): + for joint in (arm.shoulder_1, arm.shoulder_2): + self.assertEqual(joint.firmware.as_dict(), x8) + for joint in (arm.shoulder_3, arm.elbow, arm.wrist_1): + self.assertEqual(joint.firmware.as_dict(), x6) + self.assertEqual(_X8_FIRMWARE_GAINS.as_dict(), _X8) + + def test_damiao_wrists_carry_the_stock_profiler_only(self) -> None: + cfg = AxolConfig() + for arm in (cfg.left, cfg.right): + for name in ("wrist_2", "wrist_3"): + self.assertEqual( + getattr(arm, name).firmware.as_dict(), + {"position_kp": 54.0, "profile_acc": 2.0}, + ) + + def test_the_gripper_has_no_firmware_block(self) -> None: + self.assertFalse(hasattr(AxolConfig().left.gripper, "firmware")) + + def test_defaults_survive_the_stiffness_blend(self) -> None: + cfg = AxolConfig(left_stiffness=0.3).resolved() + self.assertEqual( + cfg.left.shoulder_1.firmware, AxolConfig().left.shoulder_1.firmware + ) + + def test_calibration_entry_overlays_firmware_block(self) -> None: + base = AxolConfig().left.elbow + out = _calibrated_joint(base, {"firmware": {"position_kp": 0.05}}) + self.assertEqual(out.firmware.as_dict(), {"position_kp": 0.05}) + # Untouched entries keep the config's block. + self.assertEqual(_calibrated_joint(base, {"kp": 100.0}).firmware, base.firmware) + + def test_firmware_gains_is_exported_and_replaceable(self) -> None: + jc = replace(AxolConfig().left.elbow, firmware=FirmwareGains(speed_kp=0.05)) + self.assertIsInstance(jc, JointConfig) + self.assertEqual(jc.firmware.as_dict(), {"speed_kp": 0.05}) + + +class _FakeMotor(MyActuatorMotor): + """A V4.2+ motor's gain store behind ``_request``; ROM writes take only + while ``enabled`` is False, as on hardware.""" + + def __init__( + self, + store: dict[int, float], + *, + enabled: bool = False, + planner: tuple[int, int] = (0, 0), + decel_floor: int = 0, + ) -> None: + super().__init__(MagicMock(), 0x01, kt=2.0) + self.store = store + self.enabled = enabled + self.writes: list[tuple[int, float]] = [] + self.resets = 0 + # Position planner accel/decel (0x42 types 0/1), 0x43 writes. + self.planner = {0: planner[0], 1: planner[1]} + self.planner_writes: list[tuple[int, int]] = [] + # The X8-P20 firmware keeps decel >= 10 whatever is written. + self.decel_floor = decel_floor + + async def reset(self) -> None: # type: ignore[override] + self.resets += 1 + + async def _request(self, data: bytes, *args, **kwargs) -> bytes: # type: ignore[override] + cmd, index = data[0], data[1] + if cmd == 0x30: + return bytes([0x30, index, 0, 0]) + struct.pack(" None: + super().__init__(MagicMock(), 0x06, 0x16) + self.store = store + self.writes: list[tuple[int, float]] = [] + self.stores = 0 + + async def _read_register(self, rid, timeout=0.2, attempts=5): # type: ignore[override] + return self.store[rid] + + async def _write_register(self, rid, value): # type: ignore[override] + self.writes.append((rid, float(value))) + self.store[rid] = float(value) + + async def _store_parameters(self): # type: ignore[override] + self.stores += 1 + + +class _LegacyMotor(_FakeMotor): + async def _request(self, data: bytes, *args, **kwargs) -> bytes: # type: ignore[override] + # Pre-V4.2 bulk reply: byte 1 is zero, six uint8 gains follow. + return bytes([0x30, 0, 50, 50, 100, 5, 100, 5]) + + +def _stock() -> dict[int, float]: + return { + _MA_PID_IDX["current_kp"]: 0.8, + _MA_PID_IDX["current_ki"]: 0.08, + _MA_PID_IDX["speed_kp"]: 0.03, + _MA_PID_IDX["speed_ki"]: 1e-4, + _MA_PID_IDX["position_kp"]: 0.008, + _MA_PID_IDX["position_ki"]: 0.0, + _MA_PID_IDX["position_kd"]: 0.1, + } + + +class EnsureRomGainsTest(unittest.IsolatedAsyncioTestCase): + async def asyncSetUp(self) -> None: + # No flash settle in tests. + patcher = patch("almond_axol.motor.myactuator._MA_ROM_SETTLE_S", 0.0) + patcher.start() + self.addCleanup(patcher.stop) + + async def test_writes_only_the_gains_that_differ(self) -> None: + motor = _FakeMotor(_stock()) + changed = await motor.ensure_rom_gains(_X8) + # position_kd is already 0.1 in the motor: read, not written. + self.assertEqual(set(changed), {"position_kp", "speed_kp", "speed_ki"}) + self.assertEqual( + [i for i, _ in motor.writes], + [_MA_PID_IDX[n] for n in ("position_kp", "speed_kp", "speed_ki")], + ) + self.assertAlmostEqual(changed["position_kp"][0], 0.008) + self.assertAlmostEqual(changed["position_kp"][1], 1.0, places=6) + self.assertAlmostEqual(motor.store[_MA_PID_IDX["speed_ki"]], 1e-5, places=9) + + async def test_second_call_is_read_only(self) -> None: + motor = _FakeMotor(_stock()) + await motor.ensure_rom_gains(_X8) + motor.writes.clear() + self.assertEqual(await motor.ensure_rom_gains(_X8), {}) + self.assertEqual(motor.writes, []) + + async def test_float32_rounding_counts_as_a_match(self) -> None: + store = _stock() + store[_MA_PID_IDX["speed_ki"]] = struct.unpack(" None: + motor = _FakeMotor(_stock(), enabled=True) + with self.assertRaisesRegex(MotorError, "reads back"): + await motor.ensure_rom_gains({"position_kp": 0.3}) + + async def test_legacy_firmware_is_refused_not_written(self) -> None: + motor = _LegacyMotor(_stock()) + with self.assertRaisesRegex(MotorError, "V4.2"): + await motor.ensure_rom_gains({"position_kp": 0.3}) + self.assertEqual(motor.writes, []) + + async def test_unknown_gain_name_is_a_programming_error(self) -> None: + with self.assertRaises(ValueError): + await _FakeMotor(_stock()).ensure_rom_gains({"current_kd": 1.0}) + + +def _arm( + drivers: dict[Joint, object], + *, + is_left: bool = True, + config: object = None, + a4: tuple[Joint, ...] = (), +) -> SimpleNamespace: + cfg = AxolConfig() + # The apply/held tests exercise writes: give shoulder_1 and the elbow the + # tuned 0xA4 sets an --a4 run would bring (the config default is stock). + for arm in (cfg.left, cfg.right): + arm.shoulder_1.firmware = replace(_X8_FIRMWARE_GAINS) + arm.elbow.firmware = replace(_X6_ELBOW_FIRMWARE_GAINS) + for j in a4: + getattr(cfg.left if is_left else cfg.right, j.value).wire_mode = "a4" + return SimpleNamespace( + _is_left=is_left, + _arm_config=config or (cfg.left if is_left else cfg.right), + motors={j: SimpleNamespace(_driver=d) for j, d in drivers.items()}, + ) + + +class ApplyFirmwareGainsTest(unittest.IsolatedAsyncioTestCase): + async def asyncSetUp(self) -> None: + patcher = patch("almond_axol.motor.myactuator._MA_ROM_SETTLE_S", 0.0) + patcher.start() + self.addCleanup(patcher.stop) + + async def test_cold_configured_joints_get_their_gains_and_others_are_untouched( + self, + ) -> None: + s1, s2, elbow, w1 = (_FakeMotor(_stock()) for _ in range(4)) + arm = _arm( + { + Joint.SHOULDER_1: s1, + Joint.SHOULDER_2: s2, + Joint.ELBOW: elbow, + Joint.WRIST_1: w1, + } + ) + with self.assertLogs("almond_axol.robot.axol", level="INFO") as logs: + await apply_firmware_gains( + arm, [Joint.SHOULDER_1, Joint.SHOULDER_2, Joint.ELBOW, Joint.WRIST_1] + ) + # shoulder_1 and the elbow (the 0xA4 joints) get their tuned sets. + self.assertAlmostEqual(s1.store[_MA_PID_IDX["position_kp"]], 1.0, 6) + self.assertAlmostEqual(elbow.store[_MA_PID_IDX["position_kp"]], 1.4, 6) + self.assertAlmostEqual(s1.store[_MA_PID_IDX["speed_kp"]], 0.07, 6) + self.assertAlmostEqual(elbow.store[_MA_PID_IDX["speed_kp"]], 0.05, 6) + for motor in (s1, elbow): + self.assertAlmostEqual(motor.store[_MA_PID_IDX["speed_ki"]], 1e-5, 9) + # position_kd 0.1 is already the stock value: read, never written. + self.assertNotIn(_MA_PID_IDX["position_kd"], [i for i, _ in motor.writes]) + # shoulder_2 carries the stock X8 set, which this motor already holds. + self.assertEqual(s2.writes, []) + # wrist_1 carries the stock X6 roll set: written over the X8 values. + self.assertAlmostEqual(w1.store[_MA_PID_IDX["position_kp"]], 0.06, 6) + self.assertAlmostEqual(w1.store[_MA_PID_IDX["speed_kp"]], 0.01, 6) + self.assertAlmostEqual(w1.store[_MA_PID_IDX["position_kd"]], 0.5, 6) + self.assertEqual(sum("written to ROM" in m for m in logs.output), 3) + # Every motor that took a write is rebooted so its loop loads the new + # gains; the untouched one is not. + self.assertEqual([s1.resets, s2.resets, elbow.resets, w1.resets], [1, 0, 1, 1]) + + async def test_a_provisioned_motor_is_neither_written_nor_reset(self) -> None: + s1 = _FakeMotor(_stock()) + await apply_firmware_gains(_arm({Joint.SHOULDER_1: s1}), [Joint.SHOULDER_1]) + s1.writes.clear() + s1.resets = 0 + await apply_firmware_gains(_arm({Joint.SHOULDER_1: s1}), [Joint.SHOULDER_1]) + self.assertEqual((s1.writes, s1.resets), ([], 0)) + + async def test_held_joints_are_not_in_the_list_so_nothing_is_written(self) -> None: + s1 = _FakeMotor(_stock()) + await apply_firmware_gains(_arm({Joint.SHOULDER_1: s1}), []) + self.assertEqual(s1.writes, []) + + async def test_a_refusing_motor_warns_and_does_not_fail_enable(self) -> None: + s1 = _FakeMotor(_stock(), enabled=True) + arm = _arm({Joint.SHOULDER_1: s1}, is_left=False) + with self.assertLogs("almond_axol.robot.axol", level="WARNING") as logs: + await apply_firmware_gains(arm, [Joint.SHOULDER_1]) + self.assertTrue(any("right.shoulder_1" in m for m in logs.output)) + + async def test_a_planner_left_on_is_put_back_to_direct_tracking(self) -> None: + # A test run left the shoulder's planner at 60000 (decel 10 as found + # on right shoulder_1): enable pins both back to the config's 0 and + # reboots the motor, since the X6-P20's 2025-07 firmware ignores a 0 + # written into a running loop until the reset. + s1 = _FakeMotor(_stock(), planner=(60000, 10)) + with self.assertLogs("almond_axol.robot.axol", level="INFO") as logs: + await apply_firmware_gains( + _arm({Joint.SHOULDER_1: s1}, a4=(Joint.SHOULDER_1,)), [Joint.SHOULDER_1] + ) + self.assertEqual(s1.planner, {0: 0, 1: 0}) + self.assertEqual(s1.planner_writes, [(0, 0), (1, 0)]) + self.assertEqual(s1.resets, 1) + self.assertTrue(any("planner_accel 60000 -> 0" in m for m in logs.output)) + + async def test_an_impedance_joints_planner_is_left_alone(self) -> None: + # The jelly robot: every impedance replay pinned the right arm's + # planner to 0 (X8 decel floor 10), and tune.breakaway's single-target + # homing then went wild (2026-09-24). On an impedance joint the + # planner shapes nothing the core sends: leave the stock 5000 alone, + # and do not count it against a held joint. + from almond_axol.robot.axol import held_firmware_gain_mismatches + + s1 = _FakeMotor(_stock(), planner=(5000, 5000)) + await apply_firmware_gains(_arm({Joint.SHOULDER_1: s1}), [Joint.SHOULDER_1]) + self.assertEqual(s1.planner, {0: 5000, 1: 5000}) + self.assertEqual(s1.planner_writes, []) + s1.enabled = True + self.assertEqual( + await held_firmware_gain_mismatches( + _arm({Joint.SHOULDER_1: s1}), [Joint.SHOULDER_1] + ), + [], + ) + + async def test_a_decel_floor_does_not_block_the_gains(self) -> None: + # Right shoulder_1 as found: accel 0 (direct), decel 10 — the X8 + # firmware's floor. It is already on direct tracking: the gains must + # still be written (this read-back used to fail the whole joint), and + # a held joint in that state must not count as a mismatch. + from almond_axol.robot.axol import held_firmware_gain_mismatches + + s1 = _FakeMotor(_stock(), planner=(0, 10), decel_floor=10) + arm = _arm({Joint.SHOULDER_1: s1}, a4=(Joint.SHOULDER_1,)) + with self.assertLogs("almond_axol.robot.axol", level="INFO") as logs: + await apply_firmware_gains(arm, [Joint.SHOULDER_1]) + self.assertTrue(any("written to ROM" in m for m in logs.output), logs.output) + self.assertFalse(any("could not apply" in m for m in logs.output)) + self.assertAlmostEqual(s1.store[_MA_PID_IDX["position_kp"]], 1.0, 6) + self.assertEqual(s1.planner_writes, []) # accel already 0 + s1.enabled = True + self.assertEqual( + await held_firmware_gain_mismatches(arm, [Joint.SHOULDER_1]), [] + ) + # Coming back from the planner: accel returns to 0, decel stays at 10. + s1 = _FakeMotor(_stock(), planner=(60000, 60000), decel_floor=10) + await apply_firmware_gains( + _arm({Joint.SHOULDER_1: s1}, a4=(Joint.SHOULDER_1,)), [Joint.SHOULDER_1] + ) + self.assertEqual(s1.planner, {0: 0, 1: 10}) + + async def test_the_planner_override_reaches_the_motor(self) -> None: + cfg = AxolConfig() + cfg.left.shoulder_1.wire_mode = "a4" # the planner is an a4 setting + cfg.left.shoulder_1.firmware.planner_accel = 60000.0 + # Each joint owns its block: the shoulder_2 and a fresh config keep 0. + self.assertEqual(cfg.left.shoulder_2.firmware.planner_accel, 0.0) + self.assertEqual(AxolConfig().left.shoulder_1.firmware.planner_accel, 0.0) + s1 = _FakeMotor(_stock()) + await apply_firmware_gains( + _arm({Joint.SHOULDER_1: s1}, config=cfg.left), [Joint.SHOULDER_1] + ) + self.assertEqual(s1.planner, {0: 60000, 1: 60000}) + + async def test_gripper_is_skipped(self) -> None: + # Gripper config has no firmware block at all. + await apply_firmware_gains(_arm({Joint.GRIPPER: object()}), [Joint.GRIPPER]) + + async def test_damiao_wrist_is_provisioned_through_its_registers_without_a_reset( + self, + ) -> None: + # A wrist left on the old tuned set (KP_APR 400, ramps 50) goes back + # to the stock 54 / 2 the config now carries. + w2 = _FakeDamiao({25: 0.0037, 26: 0.002, 27: 400.0, 28: 0.0, 4: 50.0, 5: -50.0}) + arm = _arm({Joint.WRIST_2: w2}) + with self.assertLogs("almond_axol.robot.axol", level="INFO") as logs: + await apply_firmware_gains(arm, [Joint.WRIST_2]) + self.assertEqual(w2.store[27], 54.0) + self.assertEqual(w2.stores, 1) + self.assertTrue(any("written and stored" in m for m in logs.output)) + # Second pass: already provisioned — no write, no store. + w2.writes.clear() + w2.stores = 0 + await apply_firmware_gains(arm, [Joint.WRIST_2]) + self.assertEqual((w2.writes, w2.stores), ([], 0)) + + async def test_damiao_profile_ramp_writes_acc_and_negative_dec(self) -> None: + # Tuned wrists: KP_APR 400, ramps ±50. Config wants stock 54 and 2. + w2 = _FakeDamiao({25: 0.0037, 26: 0.002, 27: 400.0, 28: 0.0, 4: 50.0, 5: -50.0}) + arm = _arm({Joint.WRIST_2: w2}) + with self.assertLogs("almond_axol.robot.axol", level="INFO"): + await apply_firmware_gains(arm, [Joint.WRIST_2]) + self.assertEqual((w2.store[27], w2.store[4], w2.store[5]), (54.0, 2.0, -2.0)) + self.assertEqual(w2.stores, 1) + w2.writes.clear() + w2.stores = 0 + await apply_firmware_gains(arm, [Joint.WRIST_2]) + self.assertEqual((w2.writes, w2.stores), ([], 0)) + # A DEC that drifted alone is repaired too. + w2.store[5] = -50.0 + await apply_firmware_gains(arm, [Joint.WRIST_2]) + self.assertEqual(w2.store[5], -2.0) + self.assertEqual(w2.stores, 1) + + def test_wrists_carry_the_profile_ramp_and_myactuator_joints_do_not(self) -> None: + cfg = AxolConfig() + for arm in (cfg.left, cfg.right): + self.assertEqual(arm.wrist_2.firmware.profile_acc, 2.0) + self.assertEqual(arm.wrist_3.firmware.profile_acc, 2.0) + self.assertIsNone(arm.elbow.firmware.profile_acc) + self.assertIsNone(arm.shoulder_1.firmware.profile_acc) + + async def test_configured_gains_on_a_joint_without_a_loop_warn(self) -> None: + arm = _arm({Joint.WRIST_2: object()}) # a driver of neither vendor + arm._arm_config = replace( + arm._arm_config, + wrist_2=replace( + arm._arm_config.wrist_2, firmware=FirmwareGains(speed_kp=1) + ), + ) + with self.assertLogs("almond_axol.robot.axol", level="WARNING") as logs: + await apply_firmware_gains(arm, [Joint.WRIST_2]) + self.assertTrue(any("no firmware position loop" in m for m in logs.output)) + + +if __name__ == "__main__": + unittest.main() + + +class PlannerConfigTest(unittest.TestCase): + """``planner_accel`` / ``cap_track``: the 0xA4 planner and its speed cap.""" + + def test_only_the_two_accelerations_that_follow_a_stream(self) -> None: + FirmwareGains(planner_accel=0.0) + FirmwareGains(planner_accel=60000.0) + with self.assertRaisesRegex(ValueError, "barely moves"): + FirmwareGains(planner_accel=5000.0) + + def test_cap_track_must_keep_up_with_the_command(self) -> None: + FirmwareGains(cap_track=1.2) + FirmwareGains(cap_track=0.0) + with self.assertRaisesRegex(ValueError, "never keeps up"): + FirmwareGains(cap_track=0.8) + + def test_lead_is_bounded(self) -> None: + FirmwareGains(planner_lead_ms=5.0) + with self.assertRaisesRegex(ValueError, "0..50"): + FirmwareGains(planner_lead_ms=80.0) + + def test_cap_track_is_the_cores_not_the_motors(self) -> None: + gains = FirmwareGains( + position_kp=1.0, planner_accel=60000.0, cap_track=1.2, planner_lead_ms=5.0 + ) + self.assertEqual( + gains.as_dict(), {"position_kp": 1.0, "planner_accel": 60000.0} + ) + + +class HeldGainCheckTest(unittest.IsolatedAsyncioTestCase): + """A held joint is never written, so the run must refuse if it differs.""" + + async def asyncSetUp(self) -> None: + patcher = patch("almond_axol.motor.myactuator._MA_ROM_SETTLE_S", 0.0) + patcher.start() + self.addCleanup(patcher.stop) + + async def test_a_held_joint_running_other_gains_is_reported(self) -> None: + from almond_axol.robot.axol import held_firmware_gain_mismatches + + # Stock gains and the planner a test run left on: not the config set. + s1 = _FakeMotor(_stock(), enabled=True, planner=(60000, 60000)) + got = await held_firmware_gain_mismatches( + _arm({Joint.SHOULDER_1: s1}, is_left=False, a4=(Joint.SHOULDER_1,)), + [Joint.SHOULDER_1], + ) + self.assertEqual(len(got), 1) + self.assertTrue(got[0].startswith("right.shoulder_1: "), got) + self.assertIn("planner_accel 60000 (wanted 0)", got[0]) + self.assertIn("(wanted 1)", got[0]) # position_kp + # Reads only: nothing was written to the held motor. + self.assertEqual((s1.writes, s1.planner_writes, s1.resets), ([], [], 0)) + + async def test_a_held_joint_on_the_config_set_passes(self) -> None: + from almond_axol.robot.axol import held_firmware_gain_mismatches + + s1 = _FakeMotor(_stock()) + arm = _arm({Joint.SHOULDER_1: s1}) + await apply_firmware_gains(arm, [Joint.SHOULDER_1]) # provision it + s1.enabled = True # then it is found holding next session + self.assertEqual( + await held_firmware_gain_mismatches(arm, [Joint.SHOULDER_1]), [] + ) + self.assertEqual(await held_firmware_gain_mismatches(arm, []), []) diff --git a/tests/test_joint_frame_wrap.py b/tests/test_joint_frame_wrap.py new file mode 100644 index 00000000..ba81ed37 --- /dev/null +++ b/tests/test_joint_frame_wrap.py @@ -0,0 +1,95 @@ +"""The tuners' joint frame survives a ±360° multi-turn boot wrap. + +Right elbow, 2026-09-22: after the tuner's mode-switch reset the motor +re-derived its multi-turn angle a full turn off (−213.7° for a joint at +146.3°), the fixed motor→joint offset turned that into −363.7° in the joint +frame, and the homing ramp drove the motor a full turn into its hard stop at +40 Nm until the stall protection tripped. The proxy now re-derives the wrap +on every read and the ramp refuses an implausible reading before commanding. +""" + +from __future__ import annotations + +import math +import unittest +from types import SimpleNamespace + +from almond_axol.cli.tune.friction import _ramp_verified +from almond_axol.constants import Joint +from almond_axol.robot.axol import closer_end_stop +from almond_axol.tuning.joint_frame import JointFrameMotor + + +class _FakeMotor: + def __init__(self, joint: Joint, motor_pos_deg: float) -> None: + self.joint = joint + self.position = math.radians(motor_pos_deg) + self.commands: list[float] = [] + + async def get_position(self) -> float: + return self.position + + async def set_position_velocity(self, position: float, max_speed: float) -> None: + self.commands.append(position) + + +class WrapAwareJointFrameTest(unittest.IsolatedAsyncioTestCase): + async def test_wrapped_reading_is_corrected_and_commands_use_the_correction( + self, + ) -> None: + offset = closer_end_stop(Joint.ELBOW, False)[0] + # The motor really is at 146.3° but re-derived its angle as −213.7°. + fake = _FakeMotor(Joint.ELBOW, -213.66) + jm = JointFrameMotor(fake, offset, is_left=False) + q = await jm.get_position() + self.assertAlmostEqual(jm.wrap, math.tau, places=9) + self.assertAlmostEqual(math.degrees(q), 146.34 + math.degrees(offset), places=1) + self.assertLess(abs(math.degrees(q)), 10.0) # a few degrees off rest, not −363 + # Commanding rest sends a motor-frame target next to the wrapped + # reading, not a full turn away from it. + await jm.set_position_velocity(0.0, 0.5) + self.assertAlmostEqual( + math.degrees(fake.commands[-1] - fake.position), -math.degrees(q), places=1 + ) + self.assertLess(abs(math.degrees(fake.commands[-1] - fake.position)), 10.0) + + async def test_unwrapped_reading_needs_no_correction(self) -> None: + offset = closer_end_stop(Joint.ELBOW, False)[0] + fake = _FakeMotor(Joint.ELBOW, 146.3) + jm = JointFrameMotor(fake, offset, is_left=False) + q = await jm.get_position() + self.assertEqual(jm.wrap, 0.0) + self.assertLess(abs(math.degrees(q)), 10.0) + self.assertAlmostEqual(jm.frame_offset, offset) + + async def test_proxy_without_side_keeps_the_legacy_fixed_offset(self) -> None: + fake = _FakeMotor(Joint.ELBOW, -213.66) + jm = JointFrameMotor(fake, 0.5) + self.assertAlmostEqual(await jm.get_position(), fake.position + 0.5) + self.assertEqual(jm.wrap, 0.0) + + +class RampRefusesImplausibleReadingsTest(unittest.IsolatedAsyncioTestCase): + async def test_ramp_reads_first_and_refuses_a_reading_outside_the_limits( + self, + ) -> None: + sent: list[float] = [] + + async def get_position() -> float: + return math.radians(-363.7) + + async def set_position_velocity(position: float, max_speed: float) -> None: + sent.append(position) + + elbow = SimpleNamespace( + _is_left=False, + get_position=get_position, + set_position_velocity=set_position_velocity, + ) + with self.assertRaisesRegex(RuntimeError, "implausible"): + await _ramp_verified({Joint.ELBOW: elbow}, {Joint.ELBOW: 0.0}) + self.assertEqual(sent, []) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/test_rom_partial_arm.py b/tests/test_rom_partial_arm.py index ab886b77..7676dba7 100644 --- a/tests/test_rom_partial_arm.py +++ b/tests/test_rom_partial_arm.py @@ -295,7 +295,9 @@ def test_config_lists_only_present_motors(self) -> None: # Slot-by-motor-id is protocol generation 2; a core that predates it # would slot these wrists at 0 and 1 and then reject every target, # so the config declares the generation and such a core refuses it. - self.assertEqual(lines[0], "proto 2") + from almond_axol.rt.link import CONFIG_PROTO + + self.assertEqual(lines[0], f"proto {CONFIG_PROTO}") joint_lines = [line for line in lines if line.startswith("joint ")] self.assertEqual( [line.split()[3:5] for line in joint_lines], @@ -395,7 +397,34 @@ async def test_bench_arm_streams_soft_pd_and_zero_feedforward(self) -> None: rt = Axol._wrap(axol) for line in rt._config_text().splitlines(): if line.startswith("joint "): - self.assertEqual(line.split()[9:], ["0.0", "0.0", "0.0", "0.0"], line) + self.assertEqual(line.split()[9:13], ["0.0", "0.0", "0.0", "0.0"], line) + # Stiction and dither terms stay off on the bench too, and + # every joint is on the MIT frame. + self.assertEqual( + line.split()[13:], + [ + "0.0", + "0.0017453292519943296", + "0.0", + "0.0", + "60.0", + "mit", + "0.0", + "0.3", + "0.1", + "0.1", + "0.0", + "20.0", + # 0xA4 cap tracking off (fixed cap), no target lead, + # no 0x73 feedforward (plain 0xA4), the config-wide + # impedance rate. + "0.0", + "0.0", + "0.0", + "0.0", + ], + line, + ) def test_only_a_partial_arm_is_a_bench_run(self) -> None: """A full arm on the bus — even with a joint subset selected — is the diff --git a/tests/test_rt_link.py b/tests/test_rt_link.py index ea574211..63d76206 100644 --- a/tests/test_rt_link.py +++ b/tests/test_rt_link.py @@ -99,7 +99,17 @@ def _link(self, proc) -> link.RtLink: def test_config_header_declares_the_protocol(self) -> None: self.assertEqual(link.config_header(), [f"proto {link.CONFIG_PROTO}"]) - self.assertEqual(link.CONFIG_PROTO, 2) + # 2: slot-by-motor-id; 3/4: stiction fields; 5: dither fields; 6: wire + # mode token; 7: Stribeck fields; 8: load-proportional friction fl; + # 9: the Stribeck velocity pole on every joint line; 10: the pv wire + # token; 11: impedance joints at 240 Hz only (480 = alternate ticks); + # 12: the optional trailing 0xA4 cap_track field on joint lines; + # 13: cap_track > 0 (the planner) puts a joint on the half-rate lane; + # 14: the optional trailing 0xA4 target lead (ms); 15: impedance_hz, + # the optional 0x73 feedforward scale and cogging lines; 16: the + # optional per-joint impedance_hz. + # Bump both sides together (rust/axol-rt/src/serve.rs CONFIG_PROTO). + self.assertEqual(link.CONFIG_PROTO, 16) async def test_configure_names_a_stale_binary_when_the_core_exits(self) -> None: rt = self._link(_ExitedProc()) @@ -108,7 +118,7 @@ async def test_configure_names_a_stale_binary_when_the_core_exits(self) -> None: await rt.configure("proto 2\nloop_hz 240\n") message = str(ctx.exception) self.assertIn("/opt/axol-rt", message) - self.assertIn("proto 2", message) + self.assertIn(f"proto {link.CONFIG_PROTO}", message) self.assertIn("axol rt.install", message) # The exit is noticed in well under the 5 s ack timeout. self.assertLess(asyncio.get_running_loop().time() - started, 2.0) diff --git a/tests/test_rt_mantis.py b/tests/test_rt_mantis.py index 9c8e503d..e065c7e1 100644 --- a/tests/test_rt_mantis.py +++ b/tests/test_rt_mantis.py @@ -211,7 +211,9 @@ async def test_enable_grippers_hands_the_buses_to_the_core(self) -> None: (link,) = _FakeLink.instances config_lines = link.config.splitlines() # The protocol declaration leads every config (see RtLink.configure). - self.assertEqual(config_lines[0], "proto 2") + from almond_axol.rt.link import CONFIG_PROTO + + self.assertEqual(config_lines[0], f"proto {CONFIG_PROTO}") self.assertEqual( config_lines[4:], ["gripper 0 can_mantis_l 8", "gripper 1 can_mantis_r 8"], diff --git a/tests/test_rt_trace_columns.py b/tests/test_rt_trace_columns.py new file mode 100644 index 00000000..51f1c0df --- /dev/null +++ b/tests/test_rt_trace_columns.py @@ -0,0 +1,47 @@ +"""The Rust trace CSV grows ``cogging_ff`` / ``tf_pct``; the compactor takes +the new layout and still the one before it.""" + +from __future__ import annotations + +import tempfile +import unittest +from pathlib import Path + +import numpy as np + +from almond_axol.teleop import recorder + + +def _write(path: Path, columns: tuple[str, ...], rows: int) -> None: + lines = [",".join(columns)] + for k in range(rows): + lines.append(",".join(str(k + i * 0.5) for i in range(len(columns)))) + path.write_text("\n".join(lines) + "\n") + + +class CompactTest(unittest.TestCase): + def test_new_and_legacy_layouts_compact_side_by_side(self) -> None: + self.assertEqual(recorder._RT_TRACE_COLUMNS[-2:], ("cogging_ff", "tf_pct")) + with tempfile.TemporaryDirectory() as d: + prefix = str(Path(d) / "run") + _write(Path(f"{prefix}_rt-left.csv"), recorder._RT_TRACE_COLUMNS, 3) + _write(Path(f"{prefix}_rt-right.csv"), recorder._RT_TRACE_COLUMNS_V1, 2) + out = recorder.compact_rt_trace(prefix) + assert out is not None + with np.load(out) as z: + self.assertEqual(len(z["t"]), 5) + cog = z["cogging_ff"] + self.assertTrue(np.all(np.isfinite(cog[:3]))) + self.assertTrue(np.all(np.isnan(cog[3:]))) + self.assertEqual(list(z["side"]), [0, 0, 0, 1, 1]) + + def test_an_unknown_layout_is_refused(self) -> None: + with tempfile.TemporaryDirectory() as d: + prefix = str(Path(d) / "run") + _write(Path(f"{prefix}_rt-left.csv"), ("tick", "bogus"), 1) + with self.assertRaisesRegex(ValueError, "schema"): + recorder.compact_rt_trace(prefix) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/test_stiction.py b/tests/test_stiction.py new file mode 100644 index 00000000..b2e7dfd2 --- /dev/null +++ b/tests/test_stiction.py @@ -0,0 +1,216 @@ +"""Stiction compensation: the control math, its config plumbing, and the +realtime-core config line that carries it. + +The Rust core pins ``filter::stiction`` / ``filter::coulomb_unit`` to the +vectors in ``stiction_matches_python`` (rust/axol-rt/src/filter.rs); the +golden table here is the same one, so a change to either side breaks both. +""" + +from __future__ import annotations + +import math +import unittest + +from almond_axol.robot.config import ( + ArmConfig, + AxolConfig, + JointConfig, + _calibrated_joint, +) +from almond_axol.robot.control import ( + DITHER_PHASE_STAGGER, + STICTION_FADE_VEL, + TorqueDither, + compute_friction, + coulomb_unit, + dither_step, + stiction_amplitude, + stiction_compensation, + stribeck_amplitude, + stribeck_excess, +) + +_SCALE = math.radians(0.1) + +# (err rad, v_meas rad/s, want Nm) for amp = 0.36 (fc = 0.6, gain = 0.6). +_GOLDEN = [ + (-0.02, 0.0, -0.3599999999198255), + (-0.002, 0.0, -0.29390273095808195), + (-0.0005, 0.0, -0.10040068111546155), + (0.0, 0.0, 0.0), + (0.0005, 0.0, 0.10040068111546155), + (0.002, 0.0, 0.29390273095808195), + (0.02, 0.0, 0.3599999999198255), + (0.02, 0.022, 0.1349638509268763), + (0.02, -0.044, 0.03638171680139523), + (0.02, 0.15, 3.268646545848154e-05), +] + +# Dither at 1.5 Nm / 60 Hz stepped at 240 Hz: first four outputs of slot 0 +# and slot 1 (one golden angle further round). +_DITHER_GOLDEN = { + 0: [1.5, 8.498308346471969e-16, -1.5, -1.6996616692943939e-15], + 1: [ + -1.1060533171174791, + -1.0132354413922864, + 1.106053317117479, + 1.0132354413922875, + ], +} + + +class StictionMathTest(unittest.TestCase): + def test_golden_vectors_shared_with_the_core(self) -> None: + for err, v, want in _GOLDEN: + amp = stiction_amplitude(0.6, 0.6, 0.0, 0.0) + got = stiction_compensation(err, v, amp, _SCALE) + self.assertAlmostEqual(got, want, places=12, msg=f"err={err} v={v}") + + def test_dither_golden_vectors_shared_with_the_core(self) -> None: + self.assertAlmostEqual(DITHER_PHASE_STAGGER, 2.399963229728653, places=15) + for slot, want in _DITHER_GOLDEN.items(): + phase = slot * DITHER_PHASE_STAGGER + for k, w in enumerate(want): + phase, torque = dither_step(phase, 1.5, 60.0, 1.0 / 240.0) + self.assertAlmostEqual(torque, w, places=12, msg=f"slot {slot} k {k}") + # Off is exactly off, and does not run the phase. + self.assertEqual(dither_step(1.0, 0.0, 60.0, 0.01), (1.0, 0.0)) + dither = TorqueDither(2) + self.assertEqual(dither.update([0.0, 0.0], [60.0, 60.0]), [0.0, 0.0]) + + def test_zero_gain_is_exactly_the_production_law(self) -> None: + self.assertEqual(stiction_amplitude(0.6, 0.0, 0.0, 12.0), 0.0) + self.assertEqual(stiction_compensation(0.02, 0.0, 0.0, _SCALE), 0.0) + + def test_amplitude_follows_the_gravity_load(self) -> None: + # right shoulder_1: 0.39 Nm at rest, ~2.8 Nm under 12 Nm of gravity. + self.assertAlmostEqual(stiction_amplitude(1.3, 0.3, 0.2, 0.0), 0.39) + self.assertAlmostEqual(stiction_amplitude(1.3, 0.3, 0.2, -12.0), 2.79) + self.assertAlmostEqual(stiction_amplitude(1.3, 0.3, 0.2, 12.0), 2.79) + + def test_pushes_toward_the_target_and_saturates_at_gain_fc(self) -> None: + amp = stiction_amplitude(1.3, 0.6, 0.0, 0.0) + big = stiction_compensation(0.1, 0.0, amp, _SCALE) + self.assertAlmostEqual(big, 0.6 * 1.3, places=6) + self.assertLess(stiction_compensation(-0.1, 0.0, amp, _SCALE), 0.0) + self.assertEqual(stiction_compensation(0.1, 0.0, 0.0, _SCALE), 0.0) + # Within the scale the push is proportional: a stiffer-than-kp spring. + small = stiction_compensation(_SCALE / 10, 0.0, amp, _SCALE) + self.assertAlmostEqual(small, 0.6 * 1.3 * math.tanh(0.1), places=9) + + def test_fades_on_measured_velocity_not_commanded(self) -> None: + # The push is for a *stuck* joint: full at rest, gone once the joint + # actually slides — whatever the command is doing. + amp = stiction_amplitude(1.3, 0.6, 0.0, 0.0) + stuck = stiction_compensation(0.02, 0.0, amp, _SCALE) + one_lsb = stiction_compensation(0.02, 0.022, amp, _SCALE) + sliding = stiction_compensation(0.02, 0.15, amp, _SCALE) + self.assertAlmostEqual(stuck, 0.78, places=6) + self.assertLess(one_lsb, 0.5 * stuck) + self.assertLess(sliding, 0.001) + self.assertEqual(STICTION_FADE_VEL, 0.03) + total = compute_friction(0.3, 1.3, 250.0, 0.0, 0.0) + sliding + self.assertLessEqual(total, (1 + 0.6) * 1.3 + 1e-9) + + def test_coulomb_unit_matches_compute_friction(self) -> None: + for v in (-1.0, -0.05, 0.0, 0.02, 0.4): + self.assertAlmostEqual( + compute_friction(v, 0.6, 250.0, 0.15, 0.02), + 0.6 * coulomb_unit(v, 250.0) + 0.15 * v + 0.02, + places=12, + ) + + +# (v_meas rad/s, want) for amp = 1, v_s = 0.1, v0 = 0.02. +_STRIBECK_GOLDEN = [ + (-0.3, -0.00012340980408665668), + (-0.1, -0.36784603928630505), + (-0.05, -0.7683759879897785), + (-0.02, -0.7317316219624262), + (0.0, 0.0), + (0.01, 0.4575190147179108), + (0.02, 0.7317316219624262), + (0.05, 0.7683759879897785), + (0.1, 0.36784603928630505), + (0.2, 0.018315638813231488), +] + + +class StribeckTest(unittest.TestCase): + def test_golden_vectors_shared_with_the_core(self) -> None: + for v, want in _STRIBECK_GOLDEN: + self.assertAlmostEqual( + stribeck_excess(v, 1.0, 0.1), want, places=12, msg=f"v={v}" + ) + + def test_zero_at_rest_and_off_by_default(self) -> None: + self.assertEqual(stribeck_excess(0.0, 1.0, 0.1), 0.0) + self.assertEqual(stribeck_excess(0.05, 0.0, 0.1), 0.0) + self.assertEqual(stribeck_amplitude(0.0, 0.3, 0.1, 12.0), 0.0) + + def test_has_the_measured_shape(self) -> None: + # Excess peaks in the creep band and is gone by 3·v_s; the slope + # between 0.05 and 0.2 rad/s is negative, like the measured curve. + amp = stribeck_amplitude(1.0, 0.3, 0.1, 12.0) # 1.5 Nm under 12 Nm of load + self.assertAlmostEqual(amp, 1.5) + creep = stribeck_excess(0.05, amp, 0.1) + fast = stribeck_excess(0.2, amp, 0.1) + self.assertGreater(creep, 1.0) + self.assertLess(fast, 0.05) + self.assertLess(stribeck_excess(-0.05, amp, 0.1), 0.0) + + +class LoadFrictionTest(unittest.TestCase): + def test_fl_defaults_to_zero_and_loads_from_calibration(self) -> None: + from almond_axol.robot.config import FrictionParams + + self.assertEqual(FrictionParams(fc=0.6, k=100.0, fv=0.0, fo=0.0).fl, 0.0) + out = _calibrated_joint( + ArmConfig().shoulder_1, + {"friction": {"fc": 0.6, "k": 100.0, "fv": 0.0, "fo": 0.0, "fl": 0.08}}, + ) + self.assertEqual((out.friction.fc, out.friction.fl), (0.6, 0.08)) + # An older calibration file without fl still loads. + out = _calibrated_joint( + ArmConfig().shoulder_1, + {"friction": {"fc": 0.6, "k": 100.0, "fv": 0.0, "fo": 0.0}}, + ) + self.assertEqual(out.friction.fl, 0.0) + + +class StictionConfigTest(unittest.TestCase): + def test_defaults_are_off_on_every_joint(self) -> None: + arm = ArmConfig() + for name in ( + "shoulder_1", + "shoulder_2", + "shoulder_3", + "elbow", + "wrist_1", + "wrist_2", + "wrist_3", + ): + jc: JointConfig = getattr(arm, name) + self.assertEqual(jc.stiction_gain, 0.0, name) + self.assertEqual(jc.stiction_err_deg, 0.1, name) + self.assertEqual(jc.dither_nm, 0.0, name) + self.assertEqual(jc.wire_mode, "mit", name) + self.assertEqual(jc.stribeck_gain, 0.0, name) + + def test_calibration_file_can_set_the_fields(self) -> None: + base = ArmConfig().shoulder_1 + out = _calibrated_joint(base, {"stiction_gain": 0.5, "stiction_err_deg": 0.2}) + self.assertEqual((out.stiction_gain, out.stiction_err_deg), (0.5, 0.2)) + untouched = _calibrated_joint(base, {"kp": 200.0}) + self.assertEqual(untouched.stiction_gain, 0.0) + + def test_resolved_config_keeps_the_fields(self) -> None: + cfg = AxolConfig() + cfg.right.shoulder_1.stiction_gain = 0.6 + resolved = cfg.resolved() + self.assertEqual(resolved.right.shoulder_1.stiction_gain, 0.6) + self.assertEqual(resolved.left.shoulder_1.stiction_gain, 0.0) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/test_sweep_safety.py b/tests/test_sweep_safety.py new file mode 100644 index 00000000..8f744b54 --- /dev/null +++ b/tests/test_sweep_safety.py @@ -0,0 +1,60 @@ +"""Sweep-safety clearance poses stay inside each arm's joint limits. + +Right shoulder_1 was driven into its +90° hard stop by a shoulder_3 sweep on +2026-09-22: the humerus-horizontal raise was a left-arm value applied to the +mirrored right-arm frame. Every clearance target of every sweep, both arms, +must lie strictly inside the arm's limits, and the mirrored joints' targets +must mirror. +""" + +from __future__ import annotations + +import math +import unittest + +from almond_axol.cli.tune.friction import rest_target +from almond_axol.constants import ARM_JOINTS, Joint +from almond_axol.robot.axol import arm_limits +from almond_axol.tuning.runner import probe_clearance_targets, sweep_safety + + +class ClearancePosesTest(unittest.TestCase): + def test_every_clearance_target_is_inside_its_arm_limits(self) -> None: + margin = math.radians(1.0) + for is_left in (True, False): + for joint in ARM_JOINTS: + targets, _lo, _hi, _notes = sweep_safety(joint, is_left) + targets = {**probe_clearance_targets(joint, is_left), **targets} + for j, q in targets.items(): + lo, hi = arm_limits(j, is_left) + with self.subTest( + arm="left" if is_left else "right", + sweep=joint.value, + held=j.value, + ): + self.assertGreater(q, lo + margin) + self.assertLess(q, hi - margin) + + def test_shoulder_3_sweep_raises_shoulder_1_mirrored(self) -> None: + left, *_ = sweep_safety(Joint.SHOULDER_3, True) + right, *_ = sweep_safety(Joint.SHOULDER_3, False) + self.assertAlmostEqual(left[Joint.SHOULDER_1], math.radians(90.0)) + self.assertAlmostEqual(right[Joint.SHOULDER_1], -math.radians(90.0)) + # And the note tells the operator the signed value the arm will take. + _, _, _, notes = sweep_safety(Joint.SHOULDER_3, False) + self.assertTrue(any("-90°" in n for n in notes)) + + +class RestTargetTest(unittest.TestCase): + def test_joints_whose_rest_is_a_hard_stop_park_two_degrees_inside(self) -> None: + # Right elbow: limits −150..0, so 0 is the stop; the left elbow mirrors. + self.assertAlmostEqual(math.degrees(rest_target(Joint.ELBOW, False)), -2.0) + self.assertAlmostEqual(math.degrees(rest_target(Joint.ELBOW, True)), 2.0) + # Everything else rests at 0. + for j in (Joint.SHOULDER_1, Joint.SHOULDER_2, Joint.SHOULDER_3, Joint.WRIST_1): + self.assertEqual(rest_target(j, False), 0.0) + self.assertEqual(rest_target(Joint.ELBOW, None), 0.0) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/test_tune_a4.py b/tests/test_tune_a4.py new file mode 100644 index 00000000..288a7e46 --- /dev/null +++ b/tests/test_tune_a4.py @@ -0,0 +1,576 @@ +"""The pure parts of ``axol tune.a4``: the wave generator, the buzz guard, the +0xA4 frame, and the creep-smoothness scorecard.""" + +from __future__ import annotations + +import math +import unittest + +import numpy as np + +from almond_axol.cli.tune import a4 + + +class WaveformTest(unittest.TestCase): + def test_sine_starts_at_centre_with_matching_velocity(self) -> None: + w = a4.waveform("sine", 0.5, 0.1, 2.0, 100.0, freq=0.5) + self.assertEqual(len(w), 200) + t0, q0, v0 = w[0] + self.assertEqual((t0, q0), (0.0, 0.5)) + self.assertAlmostEqual(v0, 0.1 * 2 * math.pi * 0.5) + # Quarter period later the sine peaks and the velocity is zero. + _, q, v = w[50] + self.assertAlmostEqual(q, 0.6, places=6) + self.assertAlmostEqual(v, 0.0, places=6) + + def test_triangle_runs_every_leg_at_the_set_speed(self) -> None: + w = a4.waveform("triangle", 0.0, 0.2, 20.0, 100.0, speed=0.1) + q = np.array([s[1] for s in w]) + v = np.array([s[2] for s in w]) + self.assertAlmostEqual(q.max(), 0.2, places=6) + self.assertAlmostEqual(q.min(), -0.2, places=6) + # Velocity is ±speed everywhere, and the position slope matches it on + # every sample except the turnarounds. + self.assertTrue(np.allclose(np.abs(v), 0.1)) + slope = np.abs(np.diff(q) * 100.0) + self.assertGreater(np.mean(np.isclose(slope, 0.1, atol=1e-6)), 0.95) + # First leg goes up from the centre at +speed. + self.assertEqual(w[0][1], 0.0) + self.assertGreater(w[1][1], 0.0) + + def test_zero_speed_or_frequency_holds(self) -> None: + self.assertTrue( + all( + q == 1.0 and v == 0.0 + for _, q, v in a4.waveform("triangle", 1.0, 0.2, 1.0, 50.0, speed=0.0) + ) + ) + self.assertTrue( + all( + q == 1.0 + for _, q, _v in a4.waveform("sine", 1.0, 0.2, 1.0, 50.0, freq=0.0) + ) + ) + with self.assertRaises(ValueError): + a4.waveform("square", 0.0, 0.1, 1.0, 50.0) + + +class BuzzGuardTest(unittest.TestCase): + def test_smooth_creep_passes_and_vibration_trips(self) -> None: + guard = a4.BuzzGuard(200.0, math.radians(0.3), 10.0) + # A steady 3 deg/s creep never trips. + for k in range(60): + self.assertIsNone(guard.feed(math.radians(3.0) * k / 200.0, 1.0)) + # A 25 Hz, 0.6° oscillation does within one window. + tripped = None + for k in range(60, 120): + tripped = guard.feed( + math.radians(0.9) + + math.radians(0.6) * math.sin(2 * math.pi * 25 * k / 200.0), + 1.0, + ) + if tripped: + break + self.assertIsNotNone(tripped) + self.assertIn("high-frequency", tripped) + + def test_current_limit_trips_immediately(self) -> None: + guard = a4.BuzzGuard(200.0, math.radians(0.3), 5.0) + self.assertIsNone(guard.feed(0.0, 4.0)) + self.assertIn("current", guard.feed(0.0, -6.0) or "") + + def test_limits_at_zero_are_off(self) -> None: + guard = a4.BuzzGuard(200.0, 0.0, 0.0) + for k in range(100): + self.assertIsNone(guard.feed(math.sin(k), 50.0)) + + +class FrameTest(unittest.TestCase): + def test_a4_frame_matches_the_vendor_example(self) -> None: + self.assertEqual( + a4._a4_frame(2 * math.pi, 500.0), + bytes([0xA4, 0x00, 0xF4, 0x01, 0xA0, 0x8C, 0x00, 0x00]), + ) + iq, speed = a4._decode_a4_reply( + bytes([0xA4, 0x32, 0x64, 0x00, 0xF4, 0x01, 0x2D, 0x00]) + ) + self.assertAlmostEqual(iq, 1.0) + self.assertAlmostEqual(speed, math.radians(500.0)) + + +class MetricsTest(unittest.TestCase): + def _log(self, lag_s: float, ripple: float) -> list[dict]: + rate = 200.0 + rng = np.random.default_rng(1) + rows = [] + for k in range(int(10 * rate)): + t = k / rate + target = 0.05 * t + actual = 0.05 * (t - lag_s) + ripple * math.sin(2 * math.pi * 2.0 * t) + rows.append( + { + "t": t, + "target": target, + "actual": actual + rng.normal(0, 1e-5), + "error": actual - target, + "torque": math.nan, + "speed": 0.05, + "iq": 1.5, + "v_cmd": 0.05, + } + ) + return rows + + def test_scores_lag_and_creep_smoothness(self) -> None: + smooth = a4.a4_metrics(self._log(0.1, 0.0), 200.0) + self.assertAlmostEqual(smooth["lag_ms"], 100.0, delta=6.0) + self.assertLess(smooth["v_ripple"], 0.05) + self.assertEqual(smooth["stuck_frac"], 0.0) + self.assertLess(math.degrees(smooth["band_1_4"]), 0.01) + self.assertAlmostEqual(smooth["iq_rms"], 1.5) + # A constant current is a gravity hold, not vibration: no spread, no mode. + self.assertAlmostEqual(smooth["iq_sd"], 0.0) + self.assertAlmostEqual(smooth["iq_mode"], 0.0) + # A 2 Hz ±0.3° wobble on the same creep shows up in the band and ripple. + wobbly = a4.a4_metrics(self._log(0.1, math.radians(0.3)), 200.0) + self.assertGreater(math.degrees(wobbly["band_1_4"]), 0.15) + self.assertGreater(wobbly["v_ripple"], 0.5) + + +if __name__ == "__main__": + unittest.main() + + +class SpeedCapTest(unittest.TestCase): + def test_zero_track_is_the_fixed_cap(self) -> None: + self.assertEqual(a4.speed_cap(math.radians(3.0), 60.0, 0.0, 1.0), 60.0) + self.assertEqual(a4.speed_cap(0.0, 60.0, 0.0, 1.0), 60.0) + + def test_tracking_cap_follows_commanded_speed_with_floor_and_ceiling(self) -> None: + # 3 deg/s commanded, 1.2× → 3.6 dps, sign-independent. + self.assertAlmostEqual(a4.speed_cap(math.radians(3.0), 60.0, 1.2, 1.0), 3.6) + self.assertAlmostEqual(a4.speed_cap(-math.radians(3.0), 60.0, 1.2, 1.0), 3.6) + # A stationary target keeps the floor so it can still be corrected … + self.assertEqual(a4.speed_cap(0.0, 60.0, 1.2, 1.0), 1.0) + # … and the fixed cap remains the ceiling. + self.assertEqual(a4.speed_cap(math.radians(100.0), 60.0, 1.2, 1.0), 60.0) + + def test_frame_carries_the_per_sample_cap(self) -> None: + frame = a4._a4_frame(0.0, a4.speed_cap(math.radians(3.0), 60.0, 1.2, 1.0)) + self.assertEqual(int.from_bytes(frame[2:4], "little"), 4) # 3.6 rounds to 4 dps + + +class CurrentModeMetricTest(unittest.TestCase): + def test_mode_current_isolates_the_3_to_8_hz_shudder(self) -> None: + rate = 200.0 + n = 2400 + t = np.arange(n) / rate + # 5 Hz, 1 A amplitude on a -8 A gravity hold, plus a 60 Hz 0.3 A buzz. + iq = ( + -8.0 + + 1.0 * np.sin(2 * math.pi * 5.0 * t) + + 0.3 * np.sin(2 * math.pi * 60.0 * t) + ) + log = [ + { + "t": ti, + "target": 0.0, + "actual": 0.0, + "error": 0.0, + "v_cmd": 0.1, + "iq": qi, + } + for ti, qi in zip(t, iq) + ] + m = a4.a4_metrics(log, rate) + # sd of the two sines: sqrt(0.5 + 0.045) ≈ 0.738 A + self.assertAlmostEqual(m["iq_sd"], math.sqrt(0.5 + 0.045), places=2) + # The 3-8 Hz share is the 5 Hz tone's std alone: 1/sqrt(2) ≈ 0.707 A. + self.assertAlmostEqual(m["iq_mode"], 1.0 / math.sqrt(2), places=1) + self.assertLess(m["iq_mode"], m["iq_sd"]) + + +class PoseAndHeldJointsTest(unittest.TestCase): + def test_pose_parses_validates_and_mirrors_shoulder_2_outboard(self) -> None: + from almond_axol.constants import Joint + + pose = a4.parse_pose(["shoulder_1=-90", "elbow=-75"], Joint.SHOULDER_2, False) + self.assertAlmostEqual(pose[Joint.SHOULDER_1], math.radians(-90)) + self.assertAlmostEqual(pose[Joint.ELBOW], math.radians(-75)) + self.assertEqual(a4.parse_pose(None, Joint.ELBOW, True), {}) + with self.assertRaisesRegex(SystemExit, "outside"): + a4.parse_pose(["shoulder_1=120"], Joint.ELBOW, False) # right limit is +90 + with self.assertRaisesRegex(SystemExit, "test joint"): + a4.parse_pose(["elbow=10"], Joint.ELBOW, False) + with self.assertRaisesRegex(SystemExit, "outboard"): + a4.parse_pose( + ["shoulder_2=-10"], Joint.SHOULDER_3, False + ) # right: positive + with self.assertRaisesRegex(SystemExit, "unknown joint"): + a4.parse_pose(["hip=1"], Joint.ELBOW, True) + + def test_held_summary_scores_drift_and_oscillation(self) -> None: + t = np.arange(0, 4.0, 1 / 80) + # shoulder_2 oscillating at 2.5 Hz, ±0.5°, around a +10° hold. + s2 = math.radians(10) + math.radians(0.5) * np.sin(2 * math.pi * 2.5 * t) + # elbow let go: parked 6° below its 0° hold, no oscillation. + el = np.full_like(t, math.radians(-6.0)) + out = a4.held_summary( + { + "shoulder_2": list(zip(t, s2)), + "elbow": list(zip(t, el)), + "wrist_1": [(0.0, 0.0)], + }, + {"shoulder_2": math.radians(10), "elbow": 0.0}, + ) + self.assertNotIn("wrist_1", out) # too few samples to score + self.assertAlmostEqual(out["shoulder_2"]["hz"], 2.5, delta=0.3) + self.assertAlmostEqual(out["shoulder_2"]["p2p"], 1.0, delta=0.05) + self.assertAlmostEqual(out["shoulder_2"]["drift"], 0.0, delta=0.05) + self.assertAlmostEqual(out["elbow"]["drift"], -6.0, places=6) + self.assertEqual(out["elbow"]["std"], 0.0) + + +class RingPowerTest(unittest.TestCase): + """Finding the joint that feeds a ring every held joint shares.""" + + def _joint( + self, phase: float, fs: float = 33.0, hz: float = 4.25, jitter: bool = True + ) -> list[tuple[float, float, float]]: + # Round-robin reads land unevenly; the scorer resamples. + rng = np.random.default_rng(0) + t = np.arange(0, 12.0, 1 / fs) + if jitter: + t = t + rng.uniform(0, 0.3 / fs, len(t)) + w = 0.1 * np.sin(2 * math.pi * hz * t) + # Gravity's DC and the wave's slow content must not count. + tau = 2.0 + 0.3 * np.sin(2 * math.pi * 0.07 * t) + tau = tau + 0.5 * np.sin(2 * math.pi * hz * t + phase) + return list(zip(t, w, tau)) + + def test_the_driving_joint_is_the_only_positive_one(self) -> None: + out = a4.ring_power( + { + "wrist_3": self._joint(0.0), # torque in phase: drives + "shoulder_2": self._joint(math.pi), # opposes velocity: damps + "elbow": self._joint(math.pi / 2), # spring-like: no net power + }, + 4.25, + ) + self.assertGreater(out["wrist_3"]["power_w"], 0.02) + self.assertAlmostEqual(out["wrist_3"]["cos_phi"], 1.0, delta=0.05) + self.assertLess(out["shoulder_2"]["power_w"], -0.02) + self.assertAlmostEqual(out["shoulder_2"]["cos_phi"], -1.0, delta=0.05) + self.assertAlmostEqual(out["elbow"]["cos_phi"], 0.0, delta=0.1) + # 0.1 rad/s and 0.5 Nm amplitudes come back through the band-pass. + self.assertAlmostEqual(out["wrist_3"]["vel_amp"], 0.1, delta=0.01) + self.assertAlmostEqual(out["wrist_3"]["tau_amp"], 0.5, delta=0.05) + + def test_short_or_too_slow_joints_are_left_out(self) -> None: + out = a4.ring_power( + { + "short": self._joint(0.0)[:10], + "slow": self._joint(0.0, fs=8.0, jitter=False), # Nyquist 4 Hz + }, + 4.25, + ) + self.assertEqual(out, {}) + + def test_ring_hz_is_the_most_moving_oscillating_joint(self) -> None: + scores = { + "shoulder_2": {"std": 0.075, "hz": 4.26}, + "wrist_3": {"std": 0.167, "hz": 4.25}, + "shoulder_1": {"std": 0.006, "hz": 6.9}, # quiet: not a ring + } + self.assertEqual(a4.ring_hz(scores), 4.25) + self.assertIsNone(a4.ring_hz({"elbow": {"std": 0.01, "hz": 4.0}})) + self.assertIsNone(a4.ring_hz({"elbow": {"std": 0.2, "hz": math.nan}})) + + def test_held_series_keys_each_joint_on_its_own_time_base(self) -> None: + out = a4.held_series( + {"elbow": [(0.0, 1.0), (0.1, 1.1)], "wrist_1": []}, + {"elbow": [(0.05, 0.2, 3.0)]}, + ) + self.assertEqual( + sorted(out), + [ + "held_elbow_dyn_t", + "held_elbow_pos", + "held_elbow_pos_t", + "held_elbow_tau", + "held_elbow_vel", + ], + ) + np.testing.assert_array_equal(out["held_elbow_pos"], [1.0, 1.1]) + np.testing.assert_array_equal(out["held_elbow_tau"], [3.0]) + + +class DamiaoPathTest(unittest.TestCase): + def test_dm_frame_is_two_little_endian_floats_in_rad_units(self) -> None: + import struct + + frame = a4.dm_frame(1.25, 60.0) + self.assertEqual(len(frame), 8) + p, v = struct.unpack(" None: + # DM-J4310 register map: 0x19 KP_ASR, 0x1A KI_ASR, 0x1B KP_APR, 0x1C KI_APR. + self.assertEqual( + a4._DM_GAIN_REGS, + { + "speed_kp": 0x19, + "speed_ki": 0x1A, + "position_kp": 0x1B, + "position_ki": 0x1C, + }, + ) + self.assertEqual((a4._DM_REG_ACC, a4._DM_REG_DEC, a4._DM_REG_PM), (4, 5, 0x50)) + + +class HeldSamplingTest(unittest.TestCase): + """``_stream``'s round-robin reads of the held joints, on fake drivers.""" + + def test_turns_alternate_position_and_dynamics_without_touching_the_wave( + self, + ) -> None: + import asyncio + import struct + from types import SimpleNamespace + from unittest.mock import AsyncMock, MagicMock + + from almond_axol.constants import Joint + from almond_axol.motor.damiao import DamiaoMotor + from almond_axol.motor.myactuator import MyActuatorMotor + + def myactuator() -> MagicMock: + d = MagicMock(spec=MyActuatorMotor) + d._kt = 2.0 + + async def request(frame: bytes) -> bytes: + if frame[0] == 0xA4: # the wave: iq 1.5 A, 10 dps + return bytes([0xA4, 0]) + struct.pack(" SimpleNamespace: + return SimpleNamespace( + motor=SimpleNamespace(_driver=driver), frame_offset=0.0 + ) + + guard = MagicMock() + guard.feed.return_value = None + log, reason, pos, dyn = asyncio.run( + a4._stream( + SimpleNamespace(frame_offset=0.0), + myactuator(), + [(i / 400, 0.0, 0.0) for i in range(40)], + 60.0, + 400.0, + guard, + MagicMock(), + held={Joint.ELBOW: held(myactuator()), Joint.WRIST_2: held(damiao)}, + ) + ) + self.assertIsNone(reason) + # 40 ticks over 2 joints x 2 kinds: 10 of each, nothing double-counted. + self.assertEqual( + {k: len(v) for k, v in pos.items()}, {"elbow": 10, "wrist_2": 10} + ) + self.assertEqual( + {k: len(v) for k, v in dyn.items()}, {"elbow": 10, "wrist_2": 10} + ) + _, vel, tau = dyn["elbow"][0] + self.assertAlmostEqual(vel, math.radians(7)) + self.assertAlmostEqual(tau, -2.5 * 2.0) # iq x kt + self.assertEqual(dyn["wrist_2"][0][1:], (0.3, -0.4)) + # The held reads never leak into the wave's own iq/speed. + for row in log: + self.assertAlmostEqual(row["iq"], 1.5) + self.assertAlmostEqual(row["speed"], math.radians(10)) + guard.feed.assert_any_call(row["actual"], 1.5) + + +class HeldGainTest(unittest.TestCase): + """``--held-gain``: RAM gains for the joints held during another's wave.""" + + def test_joint_or_side_qualified_specs_group_by_joint(self) -> None: + from almond_axol.constants import Joint + + out = a4.parse_held_gains( + [ + "shoulder_2.position_kp=0.5", + "right.shoulder_2.speed_kp=0.06", + "wrist_2.position_kp=200", + ], + Joint.SHOULDER_3, + False, + ) + self.assertEqual( + out, + { + Joint.SHOULDER_2: {"position_kp": 0.5, "speed_kp": 0.06}, + Joint.WRIST_2: {"position_kp": 200.0}, + }, + ) + self.assertEqual(a4.parse_held_gains(None, Joint.ELBOW, True), {}) + + def test_refuses_what_the_run_cannot_apply(self) -> None: + from almond_axol.constants import Joint + + bad = { + "shoulder_2.position_kp": r"not \[SIDE\.\]JOINT", + "position_kp=0.5": r"not \[SIDE\.\]JOINT", + "left.shoulder_2.position_kp=0.5": "this run is right", + "hip.position_kp=1": "unknown joint", + "gripper.position_kp=1": "not an arm joint", + "shoulder_3.position_kp=1": "is the test joint", + "elbow.bogus=1": "unknown gain", + "elbow.position_kp=fast": "bad value", + # The Damiao wrists' pv loop has no position D or current loop. + "wrist_3.position_kd=0.1": "Damiao motor", + "wrist_2.current_kp=1": "Damiao motor", + } + for spec, message in bad.items(): + with self.subTest(spec=spec), self.assertRaisesRegex(SystemExit, message): + a4.parse_held_gains([spec], Joint.SHOULDER_3, False) + + def test_the_flag_is_repeatable_on_the_cli(self) -> None: + import argparse + + parser = argparse.ArgumentParser() + a4.add_parser(parser.add_subparsers()) + ns = parser.parse_args( + [ + "tune.a4", + "--r", + "--joint", + "shoulder_3", + "--held-gain", + "shoulder_2.position_kp=0.5", + "--held-gain", + "wrist_2.position_kp=200", + ] + ) + self.assertEqual( + ns.held_gain, ["shoulder_2.position_kp=0.5", "wrist_2.position_kp=200"] + ) + + +class LeadTest(unittest.TestCase): + """``--lead-ms``: the 0xA4 target runs ahead along the commanded velocity.""" + + def test_the_motor_gets_the_led_target_and_the_log_keeps_the_wave(self) -> None: + import asyncio + import struct + from types import SimpleNamespace + from unittest.mock import AsyncMock, MagicMock + + from almond_axol.motor.myactuator import MyActuatorMotor + + sent: list[float] = [] + d = MagicMock(spec=MyActuatorMotor) + + async def request(frame: bytes) -> bytes: + if frame[0] == 0xA4: + sent.append(math.radians(struct.unpack_from(" 0.205 rad on the wire. + samples = [(0.0, 0.2, 1.0), (1 / 400, 0.2, -1.0)] + log, *_ = asyncio.run( + a4._stream( + SimpleNamespace(frame_offset=0.0), + d, + samples, + 60.0, + 400.0, + guard, + MagicMock(), + lead_s=0.005, + ) + ) + self.assertAlmostEqual(sent[0], 0.205, delta=5e-4) # 0.01° frame steps + self.assertAlmostEqual(sent[1], 0.195, delta=5e-4) + self.assertEqual([r["target"] for r in log], [0.2, 0.2]) + + def test_flag_defaults_off(self) -> None: + import argparse + + parser = argparse.ArgumentParser() + a4.add_parser(parser.add_subparsers()) + ns = parser.parse_args(["tune.a4", "--r", "--joint", "elbow"]) + self.assertEqual(ns.lead_ms, 0.0) + ns = parser.parse_args(["tune.a4", "--r", "--joint", "elbow", "--lead-ms", "5"]) + self.assertEqual(ns.lead_ms, 5.0) + + +class TfProbeTest(unittest.TestCase): + """``--tf-probe``: 0x73 frames and the rated-current estimate.""" + + def test_the_0x73_frame_is_the_0xa4_frame_with_the_feedforward(self) -> None: + # Vendor manual V4.4 §2.25 example 1: 60% feedforward, 500 dps, +360°. + self.assertEqual( + a4._tf_frame(2 * math.pi, 500.0, 60.0), + bytes([0x73, 0x3C, 0xF4, 0x01, 0xA0, 0x8C, 0x00, 0x00]), + ) + self.assertEqual(a4._tf_frame(0.0, 0.0, -1.0)[1], 0xFF) + self.assertEqual(a4._tf_frame(0.0, 0.0, 500.0)[1], 127) + + def test_the_probe_steps_zero_plus_zero_minus(self) -> None: + half = a4.TF_PROBE_HALF_S + got = [a4.tf_probe_ff((k + 0.5) * half, 5.0) for k in range(8)] + self.assertEqual(got, [0.0, 5.0, 0.0, -5.0, 0.0, 5.0, 0.0, -5.0]) + + def test_the_estimate_reads_the_current_jump_at_each_switch(self) -> None: + # 0.12 A per 1% (a 12 A rated motor) on top of 9 A of gravity, the loop + # then unwinding the step over ~20 ms, plus sensor noise. + rng = np.random.default_rng(0) + rate, pct = 400.0, 5.0 + samples = [] + integ = 0.0 + for k in range(int(8 * rate)): + t = k / rate + ff = a4.tf_probe_ff(t, pct) + # The reply comes back right after the frame; the loop unwinds the + # step in the tick that follows. + iq = 9.0 + 0.12 * ff - integ + 0.01 * rng.standard_normal() + samples.append((t, ff, iq)) + integ += (0.12 * ff - integ) * (1.0 / rate) / 0.02 + est = a4.tf_step_estimate(samples) + self.assertGreater(est["edges"], 20) + self.assertAlmostEqual(est["amps_per_pct"], 0.12, delta=0.01) + self.assertEqual(a4.tf_step_estimate([])["edges"], 0) + + def test_flags(self) -> None: + import argparse + + parser = argparse.ArgumentParser() + a4.add_parser(parser.add_subparsers()) + ns = parser.parse_args(["tune.a4", "--r", "--joint", "shoulder_1"]) + self.assertIsNone(ns.tf_probe) + self.assertFalse(ns.no_imu) + ns = parser.parse_args( + ["tune.a4", "--r", "--joint", "shoulder_1", "--tf-probe", "5", "--no-imu"] + ) + self.assertEqual(ns.tf_probe, 5.0) + self.assertTrue(ns.no_imu) diff --git a/tests/test_tune_breakaway.py b/tests/test_tune_breakaway.py new file mode 100644 index 00000000..8303faf4 --- /dev/null +++ b/tests/test_tune_breakaway.py @@ -0,0 +1,99 @@ +"""The pure parts of ``axol tune.breakaway``: the escalation schedule, the +ramp shape, the trim search, the pose clamping, and the friction/bias split +the hardware loop feeds its measurements into.""" + +from __future__ import annotations + +import math +import unittest + +from almond_axol.cli.tune import breakaway as ba +from almond_axol.constants import Joint + + +class ScheduleTest(unittest.TestCase): + def test_peaks_scale_with_fc_and_stop_at_the_cap(self) -> None: + peaks = ba.peak_schedule(1.3, 3.0) + self.assertEqual(peaks[0], 0.5 * 1.3) + self.assertEqual(peaks[-1], 3.0 * 1.3) + self.assertEqual(peaks, sorted(peaks)) + self.assertTrue(all(p <= 3.0 * 1.3 + 1e-9 for p in peaks)) + + def test_low_friction_joints_get_the_floor(self) -> None: + # A Damiao wrist with fc = 0.1 Nm would otherwise ramp to 0.05 Nm. + peaks = ba.peak_schedule(0.1, 2.0) + self.assertEqual(peaks[0], 0.5 * ba._FC_FLOOR_NM) + self.assertEqual(peaks[-1], 2.0 * ba._FC_FLOOR_NM) + + def test_triangle_rises_to_one_and_returns_to_zero(self) -> None: + self.assertEqual(ba.triangle(0.0, 4.0), 0.0) + self.assertAlmostEqual(ba.triangle(1.0, 4.0), 0.5) + self.assertAlmostEqual(ba.triangle(2.0, 4.0), 1.0) + self.assertAlmostEqual(ba.triangle(3.0, 4.0), 0.5) + self.assertEqual(ba.triangle(4.0, 4.0), 0.0) + self.assertEqual(ba.triangle(5.0, 4.0), 0.0) + + +class SplitTest(unittest.TestCase): + def test_symmetric_releases_are_pure_friction(self) -> None: + f_static, bias = ba.split_breakaway([3.0, 3.1], [2.9, 3.0]) + self.assertAlmostEqual(f_static, 3.0) + self.assertAlmostEqual(bias, -0.05) + + def test_a_feedforward_residual_shows_up_as_bias(self) -> None: + # Hold torque over-supplies +0.4 Nm: the + direction releases 0.4 + # early, the - direction 0.4 late. + f_static, bias = ba.split_breakaway([2.6], [3.4]) + self.assertAlmostEqual(f_static, 3.0) + self.assertAlmostEqual(bias, 0.4) + + def test_predicted_stair_is_the_uncovered_torque_over_kp(self) -> None: + # 3.0 Nm breakaway, 1.3 Nm fc, kp 250: (1.7 / 250) rad ≈ 0.39°. + self.assertAlmostEqual( + ba.predicted_stair_deg(3.0, 1.3, 250.0), 0.3896, places=3 + ) + self.assertEqual(ba.predicted_stair_deg(1.0, 1.3, 250.0), 0.0) + self.assertTrue(math.isnan(ba.predicted_stair_deg(3.0, 1.3, 0.0))) + + +class TrimSearchTest(unittest.TestCase): + def test_converges_on_a_standing_hold(self) -> None: + search = ba.TrimSearch(step=0.2, max_trim=4.0) + # The joint drifts + (torque too high) until the trim comes down. + self.assertAlmostEqual(search.update(math.radians(0.5)), -0.2) + self.assertAlmostEqual(search.update(math.radians(0.3)), -0.4) + # Overshoot: drift flips sign, so the step halves. + self.assertAlmostEqual(search.update(math.radians(-0.2)), -0.3) + self.assertFalse(search.done) + search.update(0.0) + self.assertTrue(search.done) + self.assertFalse(search.failed) + + def test_gives_up_when_the_trim_runs_away(self) -> None: + search = ba.TrimSearch(step=1.0, max_trim=2.5) + for _ in range(3): + search.update(math.radians(1.0)) + self.assertTrue(search.failed) + + +class PosesTest(unittest.TestCase): + def test_default_pose_is_rest(self) -> None: + self.assertEqual(ba._probe_poses(Joint.SHOULDER_1, False, None), [0.0]) + + def test_base_collision_joint_is_pushed_outboard(self) -> None: + # Right shoulder_2's outboard side is +; the rest pose sits on the + # boundary, so the probe moves 5° out where a release cannot cross it. + (pose,) = ba._probe_poses(Joint.SHOULDER_2, False, [0.0]) + self.assertAlmostEqual(pose, ba._BOUNDARY_MARGIN) + (pose_l,) = ba._probe_poses(Joint.SHOULDER_2, True, [0.0]) + self.assertAlmostEqual(pose_l, -ba._BOUNDARY_MARGIN) + + def test_poses_are_kept_inside_the_safe_range(self) -> None: + poses = ba._probe_poses(Joint.SHOULDER_1, False, [-720.0, 720.0]) + lo, hi = ba.safe_limits(Joint.SHOULDER_1, False) + self.assertGreater(poses[0], lo) + self.assertLess(poses[1], hi) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/test_tune_motion_start.py b/tests/test_tune_motion_start.py new file mode 100644 index 00000000..e190c2b7 --- /dev/null +++ b/tests/test_tune_motion_start.py @@ -0,0 +1,81 @@ +"""``tune.motion`` refuses to replay from a pose the approach never reached.""" + +from __future__ import annotations + +import math +import unittest + +import numpy as np + +from almond_axol.cli.tune.motion import _START_POSE_TOL, start_pose_stragglers + +_LEFT = np.arange(0, 7) +_RIGHT = np.arange(7, 14) +_BOTH = [("left", _LEFT), ("right", _RIGHT)] + + +class StartPoseStragglersTest(unittest.TestCase): + def test_all_joints_at_start_is_clean(self) -> None: + q = np.linspace(-1.0, 1.0, 14) + self.assertEqual(start_pose_stragglers(q, q.copy(), _BOTH), []) + nudged = q + 0.5 * _START_POSE_TOL + self.assertEqual(start_pose_stragglers(nudged, q, _BOTH), []) + + def test_names_the_joint_and_reports_the_error_in_degrees(self) -> None: + q_start = np.zeros(14) + q_now = q_start.copy() + q_now[7 + 3] = math.radians(23.5) # right.elbow left at rest, 23.5° short + q_now[2] = -math.radians(4.0) # left.shoulder_3 a little off the other way + out = start_pose_stragglers(q_now, q_start, _BOTH) + self.assertEqual([n for n, _ in out], ["left.shoulder_3", "right.elbow"]) + self.assertAlmostEqual(out[1][1], 23.5, places=6) + self.assertAlmostEqual(out[0][1], -4.0, places=6) + + def test_an_arm_left_off_is_not_judged(self) -> None: + # ``--arms right``: the left arm reads as rest while the motion's + # first row is elsewhere — that is not a straggler. + q_start = np.linspace(-1.0, 1.0, 14) + q_now = q_start.copy() + q_now[:7] = 0.0 + q_now[7 + 3] += math.radians(22.5) + out = start_pose_stragglers(q_now, q_start, [("right", _RIGHT)]) + self.assertEqual(out, [("right.elbow", 22.5)]) + + +if __name__ == "__main__": + unittest.main() + + +class RetimeMeasurementsTest(unittest.TestCase): + """Cache reads are put back on the command clock before scoring.""" + + def test_a_sawtooth_cache_age_is_removed(self) -> None: + from almond_axol.cli.tune.motion import retime_measurements + + # A 240 Hz log of a joint moving at 1 rad/s whose cache was refreshed + # at 400 Hz: each sample is 0..2.5 ms old, in a repeating pattern. + t = np.arange(0, 1.0, 1 / 240) + age = (np.arange(len(t)) % 5) * 0.0005 + offsets = np.zeros((len(t), 14)) + offsets[:, 7] = -age + actual = np.full((len(t), 14), np.nan, dtype=np.float32) + actual[:, 7] = (t - age).astype(np.float32) # the position when sampled + torque = np.full_like(actual, np.nan) + fixed, _ = retime_measurements(t, offsets, actual, torque) + # Raw error against the command clock is the sawtooth; re-timed it is ~0. + raw = np.sqrt(np.mean((actual[:, 7] - t) ** 2)) + new = np.sqrt(np.mean((fixed[5:-5, 7] - t[5:-5]) ** 2)) + self.assertGreater(raw, 5e-4) + self.assertLess(new, 2e-5) + # An absent arm's NaN columns and unknown offsets pass through untouched. + self.assertTrue(np.all(np.isnan(fixed[:, 0]))) + + def test_unknown_offsets_leave_the_series_alone(self) -> None: + from almond_axol.cli.tune.motion import retime_measurements + + t = np.arange(0, 0.1, 1 / 240) + actual = np.random.default_rng(0).normal(size=(len(t), 14)).astype(np.float32) + torque = actual.copy() + a, q = retime_measurements(t, np.zeros((len(t), 14)), actual, torque) + np.testing.assert_array_equal(a, actual) + np.testing.assert_array_equal(q, torque) diff --git a/tests/test_wrist_imu.py b/tests/test_wrist_imu.py new file mode 100644 index 00000000..e097b267 --- /dev/null +++ b/tests/test_wrist_imu.py @@ -0,0 +1,150 @@ +"""Wrist-camera IMU shake scoring and the recorder's subprocess plumbing +(with a fake camera worker — no ZED SDK needed).""" + +from __future__ import annotations + +import math +import time +import unittest +from typing import Any + +import numpy as np + +from almond_axol.tuning.wrist_imu import WristImu, format_imu, shake_metrics + +_W = 2.0 * math.pi * 5.0 # a 5 Hz shake + + +def _record(seconds: float = 10.0, fs: float = 400.0, amp_m: float = 0.5e-3): + t = np.arange(0.0, seconds, 1.0 / fs) + acc = np.zeros((len(t), 3)) + acc[:, 2] = 9.81 - amp_m * _W * _W * np.sin(_W * t) # vertical, gravity on z + gyro = np.zeros((len(t), 3)) + gyro[:, 0] = 2.0 * np.sin(_W * t) + return t, acc, gyro + + +def fake_worker( + serial: int, + out_path: str, + ready: Any, + stop: Any, + dump: Any, + dumped: Any, + errors: Any, +) -> None: + """A camera that streams the 5 Hz shake on the perf_counter clock.""" + from almond_axol.zed.imu_worker import write_samples as _write_samples + + ts, acc, gyro = [], [], [] + ready.set() + while not stop.is_set(): + now = time.perf_counter() + ts.append(now) + acc.append((0.0, 0.0, 9.81 - 0.5e-3 * _W * _W * math.sin(_W * now))) + gyro.append((0.0, 0.0, 0.0)) + if dump.is_set(): + dump.clear() + _write_samples(out_path, ts, acc, gyro) + dumped.set() + time.sleep(0.0025) + _write_samples(out_path, ts, acc, gyro) + + +def broken_worker( + serial: int, + out_path: str, + ready: Any, + stop: Any, + dump: Any, + dumped: Any, + errors: Any, +) -> None: + errors.put(f"camera {serial} did not open: CAMERA NOT DETECTED") + + +class ShakeMetricsTest(unittest.TestCase): + def test_a_half_millimetre_5_hz_shake_scores_1_mm_peak_to_peak(self) -> None: + m = shake_metrics(*_record()) + self.assertAlmostEqual(m["shake_mm"], 1.0, places=2) + self.assertAlmostEqual(m["vertical_mm"], 1.0, places=2) + self.assertAlmostEqual(m["high_mm"], 1.0, places=2) + self.assertLess(m["low_mm"], 0.02) + self.assertAlmostEqual(m["peak_hz"], 5.0, delta=0.2) + self.assertAlmostEqual(m["acc_rms"], 0.5e-3 * _W * _W / math.sqrt(2), places=2) + self.assertAlmostEqual(m["gyro_rms"], 2.0 / math.sqrt(2), delta=0.02) + + def test_a_2_hz_sway_counts_and_lands_in_the_low_band(self) -> None: + fs = 400.0 + t = np.arange(0.0, 12.0, 1.0 / fs) + w = 2 * math.pi * 2.0 + acc = np.zeros((len(t), 3)) + acc[:, 2] = 9.81 - 1e-3 * w * w * np.sin(w * t) # 1 mm amplitude + m = shake_metrics(t, acc) + self.assertAlmostEqual(m["vertical_mm"], 2.0, delta=0.05) + self.assertAlmostEqual(m["low_mm"], 2.0, delta=0.05) + self.assertLess(m["high_mm"], 0.05) + self.assertAlmostEqual(m["peak_hz"], 2.0, delta=0.1) + + def test_slow_arm_motion_and_gravity_do_not_count(self) -> None: + t, acc, gyro = _record() + acc[:, 1] += 0.05 * np.sin(2 * math.pi * 0.3 * t) # the motion itself + self.assertAlmostEqual(shake_metrics(t, acc, gyro)["shake_mm"], 1.0, places=2) + still = np.tile([0.0, 0.0, 9.81], (len(t), 1)) + self.assertLess(shake_metrics(t, still)["shake_mm"], 1e-6) + + def test_vertical_follows_gravity_whatever_the_camera_orientation(self) -> None: + t, acc, _ = _record() + # Tilt the camera 90°: gravity (and the vertical shake) now on x. + tilted = acc[:, [2, 1, 0]] + m = shake_metrics(t, tilted) + self.assertAlmostEqual(m["vertical_mm"], 1.0, places=2) + + def test_too_short_is_empty(self) -> None: + t, acc, gyro = _record(seconds=1.5) + self.assertEqual(shake_metrics(t, acc, gyro), {}) + + def test_format_names_each_side(self) -> None: + lines = format_imu({"right": shake_metrics(*_record())}) + self.assertEqual(len(lines), 1) + self.assertIn("wrist IMU (right)", lines[0]) + self.assertIn("1.00 mm", lines[0]) + + +class RecorderTest(unittest.TestCase): + def test_records_windows_on_the_run_clock_and_flushes_mid_session(self) -> None: + imu = WristImu( + ["right"], serial_of=lambda side: 1234, worker=f"{__name__}:fake_worker" + ) + imu.start() + try: + self.assertEqual(imu.sides, ["right"]) + t0 = time.perf_counter() + time.sleep(5.0) + t1 = time.perf_counter() + imu.flush() + metrics, series = imu.run_blocks(t0, t1, origin=t0 - 1.0) + self.assertAlmostEqual(metrics["right"]["shake_mm"], 1.0, delta=0.1) + # t relative to the given origin: the window starts ~1 s in. + self.assertAlmostEqual(float(series["imu_right_t"][0]), 1.0, delta=0.05) + self.assertEqual(series["imu_right_acc"].shape[1], 3) + finally: + imu.stop() + imu.stop() # idempotent + + def test_no_camera_or_a_camera_that_fails_leaves_no_data(self) -> None: + with WristImu(["left"], serial_of=lambda side: None) as imu: + self.assertEqual(imu.sides, []) + self.assertEqual(imu.run_blocks(0.0, 1e9), ({}, {})) + with WristImu( + ["left"], serial_of=lambda side: 1, worker=f"{__name__}:broken_worker" + ) as imu: + self.assertEqual(imu.sides, []) + disabled = WristImu(["left"], enabled=False, serial_of=lambda side: 1) + disabled.start() + self.assertEqual(disabled.sides, []) + disabled.stop() + + +if __name__ == "__main__": + unittest.main() diff --git a/web/app/src/components/diagnostics/control-health.tsx b/web/app/src/components/diagnostics/control-health.tsx index 538d3ae4..fb57a8e9 100644 --- a/web/app/src/components/diagnostics/control-health.tsx +++ b/web/app/src/components/diagnostics/control-health.tsx @@ -86,8 +86,11 @@ function sample(timing: ControlTiming, kind: TimingSeries): (number | null)[] { /** Adapt timing messages to the generic chart's keyed numeric-series shape. */ function chartFrames(frames: TimingFrame[]): TelemetryFrame[] { return frames.map((frame) => { + // The loop rate the core is running (240 Hz impedance, 400 Hz position + // controller) comes from the observer, per frame. + const hz = frame.arms.left?.targetHz ?? frame.arms.right?.targetHz ?? 240 const m: TelemetryFrame["m"] = { - target: [240, null, null, null, 1000 / 240, null], + target: [hz, null, null, null, 1000 / hz, null], } for (const side of SIDES) { const timing = frame.arms[side] @@ -189,7 +192,7 @@ export function ControlHealth({ frames, version, nowT, view, onViewChange }: Con ? [ ...commandSeries, ...feedbackSeries, - { key: "target", label: "4.17 ms target", color: COLORS.target }, + { key: "target", label: "RT period target", color: COLORS.target }, ] : [...commandSeries, ...feedbackSeries] @@ -258,7 +261,11 @@ export function ControlHealth({ frames, version, nowT, view, onViewChange }: Con

{side} arm

- {clean ? "240 Hz clean" : isFresh ? "timing issue" : "idle"} + {clean && timing + ? `${Math.round(timing.targetHz)} Hz clean` + : isFresh + ? "timing issue" + : "idle"} {timing && ( @@ -325,7 +332,9 @@ export function ControlHealth({ frames, version, nowT, view, onViewChange }: Con

-

missed 240 Hz

+

+ missed {timing ? Math.round(timing.targetHz) : 240} Hz +

{timing?.deadlineMisses ?? "–"}/s

@@ -362,8 +371,10 @@ export function ControlHealth({ frames, version, nowT, view, onViewChange }: Con />

Send batch is first-to-last arm command; full cycle continues through the final feedback. - “Missed 240 Hz” counts command gaps that lost one or more 4.17 ms deadlines. All values come - from passive kernel-timestamped evidence on the Rust-owned CAN wire. + “Missed N Hz” counts command gaps that lost one or more loop periods — 4.17 ms on the 240 Hz + impedance controller, 2.5 ms on the 400 Hz position controller; the observer infers which is + running. All values come from passive kernel-timestamped evidence on the Rust-owned CAN + wire.

) diff --git a/web/app/src/components/diagnostics/tuning-workbench.tsx b/web/app/src/components/diagnostics/tuning-workbench.tsx index 6383a069..5ea7ad13 100644 --- a/web/app/src/components/diagnostics/tuning-workbench.tsx +++ b/web/app/src/components/diagnostics/tuning-workbench.tsx @@ -5,6 +5,7 @@ import { Button } from "@/components/ui/button" import { Card } from "@/components/ui/card" import { useToast } from "@/components/ui/toast" import { cn } from "@/lib/utils" +import { type FirmwareVendor, jointVendor, shownForJoint } from "@/lib/firmware-loop" import { RunChart, type RunChartSeries } from "@/components/diagnostics/run-chart" import type { CommandSpec, FormValue } from "@/lib/supervisor" import { @@ -20,7 +21,16 @@ import { type TuningRecording, type TuningRunData, type TuningRunMeta, + type TuningWireModes, } from "@/lib/tuning" +import { fetchMotorDetails } from "@/lib/telemetry" +import { + MYACTUATOR_JOINTS, + SIDES, + effectiveWireMode, + parseA4Tokens, + toggleA4Token, +} from "@/lib/wire-mode" const COMMANDED_COLOR = "rgba(255,255,255,0.45)" const ACTUAL_COLOR = "#eff483" @@ -55,7 +65,7 @@ const KNOWN_KINDS = new Set(["sine", "step", "motion", "gravity", "filter", "bui interface WbField { key: string label: string - type: "number" | "text" | "select" | "boolean" | "overrides" | "pose" + type: "number" | "text" | "select" | "boolean" | "overrides" | "pose" | "wire" options?: string[] /** Placeholder shown when empty; empty means "command default". */ placeholder?: string @@ -68,8 +78,19 @@ interface WbField { * and an empty box means "run with config". */ gainKey?: string + /** + * Key into the selected motor's *firmware* loop gains (`position_kp`, + * `speed_kp`, …), read live from the motor over the idle link: the field + * shows that value as its baseline and an empty box runs with it. + */ + fwGainKey?: string /** Render a slider next to the value box, over this range. */ slider?: { min: number; max: number; step: number } + /** + * The motor vendors whose firmware loop has this knob; the field hides for + * a joint of any other vendor (see `lib/firmware-loop`). Unset = always. + */ + vendors?: readonly FirmwareVendor[] } interface WbTab { @@ -137,6 +158,74 @@ const GAIN_FIELDS: WbField[] = [ }, ] +/** + * The firmware loop gains of the Firmware-loop tab. Each shows the selected + * motor's *live* value ("motor N", read over the idle link when arm and joint + * are picked); an empty box runs with the motor's value. Plain number boxes, + * no sliders: the two vendors' gains live on different scales (a MyActuator + * position_kp near 1, a Damiao KP_APR in the hundreds), so no one range fits. + * Fields tagged with `vendors` show only for a joint on that vendor's motor — + * the Damiao wrists have no position D, no exposed current loop. + */ +const FW_GAIN_FIELDS: WbField[] = [ + { + key: "position_kp", + label: "position_kp", + type: "text", + fwGainKey: "position_kp", + hint: + "position loop P — lag ∝ 1/kp. MyActuator: config 1.0 (elbow 1.4), stock 0.008 " + + "on the X8 shoulders. Damiao KP_APR: config 400", + }, + { + key: "position_ki", + label: "position_ki", + type: "text", + fwGainKey: "position_ki", + hint: "position loop I (Damiao KI_APR)", + }, + { + key: "position_kd", + label: "position_kd", + type: "text", + fwGainKey: "position_kd", + vendors: ["myactuator"], + hint: "position loop D — measured inert in the 0xA4 loop on the X8-P20", + }, + { + key: "speed_kp", + label: "speed_kp", + type: "text", + fwGainKey: "speed_kp", + hint: + "speed loop P (Damiao KP_ASR) — on MyActuator the only damping term and the " + + "buzz knob; 0.1 vibrated on shoulder_1 (stock 0.03)", + }, + { + key: "speed_ki", + label: "speed_ki", + type: "text", + fwGainKey: "speed_ki", + hint: "speed loop I (Damiao KI_ASR) — what pushes through stiction", + }, + { + key: "current_kp", + label: "current_kp", + type: "text", + fwGainKey: "current_kp", + vendors: ["myactuator"], + hint: "current loop P — leave unless the vendor says otherwise", + }, + { + key: "current_ki", + label: "current_ki", + type: "text", + fwGainKey: "current_ki", + vendors: ["myactuator"], + hint: "current loop I", + }, +] + const TABS: WbTab[] = [ { key: "sine", @@ -193,6 +282,15 @@ const TABS: WbTab[] = [ type: "number", placeholder: "off", }, + { + key: "no_imu", + label: "skip wrist IMU", + type: "boolean", + hint: + "by default the wrist ZED X One's IMU is recorded and the run gets an IMU " + + "shake score — 1–15 Hz displacement at the gripper, 2 s peak-to-peak in mm " + + "(what the joint encoders cannot see: backlash, flex)", + }, { key: "label", label: "label", type: "text", placeholder: "note", width: "w-40" }, ], required: ["arm", "joint"], @@ -245,6 +343,157 @@ const TABS: WbTab[] = [ options: ["full", "gravity", "friction", "none"], }, { key: "stiffness", label: "stiffness s", type: "number", placeholder: "—" }, + { + key: "no_imu", + label: "skip wrist IMU", + type: "boolean", + hint: + "by default the wrist ZED X One's IMU is recorded and the run gets an IMU " + + "shake score — 1–15 Hz displacement at the gripper, 2 s peak-to-peak in mm " + + "(what the joint encoders cannot see: backlash, flex)", + }, + { key: "label", label: "label", type: "text", placeholder: "note", width: "w-40" }, + ], + required: ["arm", "joint"], + drivesMotors: true, + }, + { + key: "a4", + label: "Firmware loop", + command: "tune.a4", + description: + "Tune a joint's own firmware position loop — 0xA4 on the MyActuator joints " + + "(wire_mode a4), position-velocity on the Damiao wrists (pv) — with a sine or " + + "a constant-speed triangle; the gain boxes follow the joint's motor. Firmware " + + "gains are written to RAM for the run and restored afterwards (persist " + + "writes ROM, keep leaves them); planner acceleration must be 0 for the " + + "joint to follow a stream. A buzz guard restores the previous gains on " + + "any high-frequency motion. Compare runs on velocity ripple (MIT " + + "stick-slip ≈ 0.8, smooth < 0.2), stuck windows, lag and the 1–4 Hz band. " + + "The joint holds stiffly and does not yield to a hand: clear the space.", + presets: { save_run: true }, + fields: [ + { key: "arm", label: "arm", type: "select", options: ["left", "right"] }, + { + key: "joint", + label: "joint", + type: "select", + options: ARM_JOINT_OPTIONS, + }, + { key: "mode", label: "wave", type: "select", options: ["triangle", "sine"] }, + { + key: "center", + label: "center (°)", + type: "number", + placeholder: "mid", + hint: "joint-frame centre (0 = rest); probe under gravity load, e.g. -35 on shoulder_1", + }, + { key: "amp", label: "half-travel (°)", type: "number", placeholder: "10" }, + { + key: "pose", + label: "pose — hold other joints (°)", + type: "pose", + hint: + "hold other joints at an angle during the run (overrides the sweep's own " + + "clearance pose for that joint). A firmware loop that is well damped with the " + + "arm hanging can oscillate with it extended — right shoulder_2 did, held during " + + "a shoulder_3 sweep — so tune the worst-case pose too; the held joints are " + + "sampled during the wave and scored", + }, + { key: "speed", label: "triangle speed (°/s)", type: "number", placeholder: "3" }, + { key: "freq", label: "sine freq (Hz)", type: "number", placeholder: "0.3" }, + { key: "duration", label: "duration (s)", type: "number", placeholder: "12" }, + { key: "rate", label: "rate (Hz)", type: "number", placeholder: "400" }, + { key: "cap", label: "speed cap (°/s)", type: "number", placeholder: "60" }, + { + key: "cap_track", + label: "cap tracks speed ×", + type: "number", + placeholder: "0", + hint: + "0 = fixed cap. With planner accel 60000 a fixed cap lets the planner burst " + + "through each 200 Hz step at the cap and idle the rest of the tick (4× the " + + "current spread on the elbow); 1.1–1.2 sets the per-command cap to that " + + "multiple of the commanded speed so the joint moves continuously", + }, + { + key: "cap_floor", + label: "cap floor (°/s)", + type: "number", + placeholder: "1", + hint: "lowest cap the tracking cap may set, so a stationary target still corrects", + }, + { + key: "dm_acc", + label: "ACC/DEC (rad/s²)", + type: "number", + placeholder: "stored", + vendors: ["damiao"], + hint: + "wrist_2 / wrist_3 only: the position-velocity profiler's acceleration (and " + + "-deceleration), written to the registers for the run and restored afterwards " + + "unless kept. Found at 2 rad/s² (~115 °/s²), far too slow to follow a stream", + }, + { + key: "accel", + label: "planner accel (dps/s)", + type: "text", + fwGainKey: "planner_accel", + vendors: ["myactuator"], + width: "w-24", + hint: + "shows what the motor stores; 0 = direct PI tracking (required to follow the " + + "stream). Written for the run and restored afterwards unless kept", + }, + ...FW_GAIN_FIELDS, + { + key: "held_gain", + label: "held joint gains", + type: "text", + width: "w-72", + placeholder: "shoulder_2.position_kp=0.5 wrist_2.position_kp=200", + hint: + "space-separated JOINT.GAIN=VALUE for the joints *held* during the wave, in " + + "RAM and restored afterwards unless kept. The held loops are what feed a ring " + + "they all share — the run's power table names the ones putting energy in; " + + "the gain boxes above set the test joint only. Damiao wrists: position/speed " + + "kp/ki only", + }, + { key: "buzz_abort", label: "buzz abort (°)", type: "number", placeholder: "0.3" }, + { + key: "iq_abort", + label: "current abort (A)", + type: "number", + placeholder: "30", + hint: + "a loaded X8 shoulder holds ~10 A of gravity alone at -55°; keep this above the " + + "pose's static current", + }, + { + key: "tf_probe", + label: "0x73 probe (% rated)", + type: "number", + placeholder: "off", + vendors: ["myactuator"], + hint: + "instead of the wave: hold the joint at center on 0x73 (position control with " + + "torque feedforward, V4.4 firmware) and step the feedforward 0 / +P / 0 / −P % " + + "of rated current — the current jump per step gives the motor's rated current, " + + "the firmware.tf_rated_current_a the realtime core scales its 0x73 feedforward " + + "with. 5 is a gentle ~1 Nm on a shoulder; planner accel must be 0", + }, + { key: "persist", label: "persist gains to ROM", type: "boolean" }, + { key: "keep", label: "keep gains + planner after run", type: "boolean" }, + { + key: "no_imu", + label: "skip wrist IMU", + type: "boolean", + hint: + "by default the wrist ZED X One's IMU is recorded and the run gets an IMU " + + "shake score — 1–15 Hz displacement at the gripper, 2 s peak-to-peak in mm " + + "(what the joint encoders cannot see: backlash, flex)", + }, + { key: "label", label: "label", type: "text", placeholder: "note", width: "w-40" }, ], required: ["arm", "joint"], @@ -267,8 +516,123 @@ const TABS: WbTab[] = [ presets: {}, fields: [ { key: "motion", label: "motion", type: "select", options: [] }, + { + key: "arms", + label: "arms", + type: "select", + options: ["both", "left", "right"], + placeholder: "both", + hint: + "which arm(s) to bring up and drive; the other arm's channel is left " + + "untouched, so a single-arm run does not need the other arm powered", + }, + { + key: "controller", + label: "controller", + type: "select", + options: ["impedance", "position"], + placeholder: "impedance", + width: "w-40", + hint: + "impedance (240 Hz) is the production MIT frame: host gravity, " + + "friction, inertia and damping feed-forward around the firmware PD, " + + "compliant. position (400 Hz) hands every joint to its motor's own " + + "position loop — MyActuator 0xA4, Damiao position-velocity, the gains " + + "on the Firmware-loop tab — streamed at 400 Hz, where the loop's " + + "target staircase is gone: stiff, no host feed-forward, NaN torque " + + "on the MyActuator joints (contact watchdog blind there). Same " + + "motion, same scoring, so the two controllers compare directly.", + }, { key: "stiffness", label: "stiffness s", type: "number", placeholder: "1" }, + { + key: "fast_impedance", + label: "480 Hz impedance joints", + type: "text", + width: "w-56", + placeholder: "right.shoulder_1 right.elbow", + hint: + "space-separated SIDE.JOINT run at 480 Hz — every tick of a 480 Hz core " + + "loop — while every other impedance joint stays at 240 Hz on alternate " + + "ticks. An experiment: the gains were tuned at 240", + }, + { + key: "impedance_hz", + label: "impedance rate (Hz)", + type: "select", + options: ["240", "480"], + placeholder: "240", + hint: + "command rate of the MyActuator impedance joints: 240 is the verified rate; " + + "480 runs them every tick of a 480 Hz core loop while the Damiao wrists stay " + + "at 240 Hz on alternate ticks — an experiment (the gains were tuned at 240)", + }, + { + key: "loop_hz", + label: "core loop (Hz)", + type: "number", + placeholder: "auto", + hint: + "realtime-core tick rate override; auto follows the wire modes (240 all " + + "impedance, 400 all firmware loops, 480 mixed — impedance joints on " + + "alternate ticks — or at impedance rate 480). With any arm joint on " + + "impedance only 240 or 480 is accepted (only 480 at impedance rate 480)", + }, + { + key: "record", + label: "record", + type: "text", + width: "w-32", + placeholder: "prefix", + hint: + "flight-recorder prefix: measured joints to PREFIX_meas.npz and the " + + "realtime core's per-tick trace to PREFIX_rt.npz in the recordings " + + "directory, for diag.teleop-jitter or offline analysis", + }, + { + key: "hold", + label: "hold joints steady", + type: "text", + width: "w-56", + placeholder: "right.elbow right.wrist_2=10", + hint: + "space-separated SIDE.JOINT[=DEG]: held at the motion's start angle (or the " + + "given one) instead of following it, same controller and gains, scored as " + + "parked. Only the approach is collision-checked — watch the first pass", + }, + { + key: "repeat", + label: "repeat", + type: "number", + placeholder: "1", + hint: + "replay the motion this many times back to back (0 = until stopped), each " + + "pass scored and saved as its own run [k/N] — for soak runs and catching an " + + "intermittent buzz", + }, + { + key: "no_imu", + label: "skip wrist IMU", + type: "boolean", + hint: + "by default each driven arm's wrist ZED X One IMU is recorded and every pass " + + "gets an IMU shake score — 1–15 Hz displacement at the gripper, 2 s " + + "peak-to-peak in mm (what the joint encoders cannot see: backlash, flex)", + }, { key: "gain", label: "gains — edit a cell to override it for this run", type: "overrides" }, + { + key: "a4", + label: "controller per joint — click a cell to put that joint on the firmware loop", + type: "wire", + hint: + "inside the impedance controller, single MyActuator joints can go on " + + "their 0xA4 firmware loop for this run only (--a4 side.joint), the rest " + + "staying on impedance — no compliance, no host feed-forward and NaN " + + "torque telemetry on that joint. Everything else about the replay is " + + "unchanged, so runs compare directly. The Damiao wrists' firmware loop " + + "(position-velocity) comes with the position controller above, which " + + "puts every joint on its firmware loop at 400 Hz. A joint already " + + "configured wire_mode a4 is pinned.", + }, { key: "ik", label: "run as IK", @@ -523,17 +887,186 @@ const KIND_TABS: Record = { kinematics: "ik", } +/** + * A `tune.a4` run: saved as kind `sine` (it shares the sine/triangle charts) + * but tagged `wire: "a4"` — it belongs to the Firmware-loop tab, carries the + * firmware gains instead of impedance gains, and is scored on creep + * smoothness rather than the impedance score. + */ +function isA4Run(meta: TuningRunMeta): boolean { + return meta.kind === "sine" && meta.params?.wire === "a4" +} + +/** The launcher tab a saved run re-arms, or null for kinds without one. */ +function runTab(meta: TuningRunMeta): string | null { + if (isA4Run(meta)) return "a4" + return KIND_TABS[meta.kind] ?? null +} + +/** What to call a run in badges: its kind, except firmware-loop runs. */ +function runKindLabel(meta: TuningRunMeta): string { + return isA4Run(meta) ? "a4" : meta.kind +} + /* ------------------------------------------------------------------ */ /* Gain-override editor (Recorded motion tab) */ /* ------------------------------------------------------------------ */ // Matches tune.motion's --gain fields (see _GAIN_FIELDS there). -const OVERRIDE_FIELDS = ["kp", "kd", "kd_host", "kd_host_hz", "kd_host_q", "j_eff"] +const OVERRIDE_FIELDS = [ + "kp", + "kd", + "kd_host", + "kd_host_hz", + "kd_host_q", + "j_eff", + "stiction_gain", + "stiction_load_gain", + "dither_nm", + "stribeck_gain", + // Share of the joint's calibrated cogging series fed forward (the "osc + // cancellation"; blank = no series calibrated for the joint). + "cogging_gain", + // The firmware position loop (0xA4 / pv), in effect on --a4 joints and + // under the position controller; written to ROM at enable. + "firmware.position_kp", + "firmware.speed_kp", + "firmware.speed_ki", + // MyActuator 0xA4 only: the planner (0 direct / 60000) and its speed-cap + // tracking (>= 1). The Damiao wrists' cells are disabled. + "firmware.planner_accel", + "firmware.cap_track", + "firmware.planner_lead_ms", + // MyActuator 0x73: the rated current that scales the torque feedforward + // (gravity + inertia + cogging) on V4.4 firmware; blank = plain 0xA4. + "firmware.tf_rated_current_a", +] + +/** + * Fields whose value belongs to the selected joint — its gains (config or + * live motor baseline) and the joint-frame / vendor-specific wave settings — + * cleared when the arm or joint changes (see `setValue`). + */ +const PER_JOINT_KEYS = new Set(["center", "dm_acc"]) +function isPerJointField(f: WbField): boolean { + return f.gainKey != null || f.fwGainKey != null || PER_JOINT_KEYS.has(f.key) +} + +/** Override fields that exist only on the MyActuator (0xA4) joints. */ +const MYACTUATOR_ONLY_FIELDS = new Set([ + "firmware.planner_accel", + "firmware.cap_track", + "firmware.planner_lead_ms", + "firmware.tf_rated_current_a", +]) + +/** Grid header for an override field (`firmware.x` shortened to `fw x`). */ +function overrideLabel(field: string): string { + return field.startsWith("firmware.") ? `fw ${field.slice("firmware.".length)}` : field +} /** Format a config gain for seeding/comparison (trims float32 noise). */ function fmtGain(v: unknown): string { if (typeof v !== "number" || !Number.isFinite(v)) return "" - return String(Number(v.toFixed(3))) + // Four significant digits: kp 250 and speed_ki 1e-5 both survive. + return String(Number(v.toPrecision(4))) +} + +/** + * Per-joint controller picker for tune.motion: one row per MyActuator joint, + * one cell per arm, each a two-way toggle between the MIT impedance frame + * and the firmware position loop (`--a4 side.joint`). Cells the robot's + * config already pins to `wire_mode a4` show as firmware and cannot be + * switched back — a run can only add `--a4` joints. Serializes to the token + * string the CLI takes, so the launch path and run re-arming stay generic. + */ +function WireModeEditor({ + value, + onChange, + disabled, + configModes, +}: { + value: string + onChange: (v: string) => void + disabled: boolean + configModes: TuningWireModes | null +}) { + const picked = parseA4Tokens(value) + return ( +
+ + + + + ))} + + + + {MYACTUATOR_JOINTS.map((joint) => ( + + + {SIDES.map((side) => { + const mode = effectiveWireMode(value, configModes, side, joint) + const pinned = + (configModes?.[side]?.[joint] ?? "mit").toLowerCase() === "a4" && + !picked.has(`${side}.${joint}`) + const dirty = picked.has(`${side}.${joint}`) + return ( + + ) + })} + + ))} + +
+ {SIDES.map((side) => ( + + {side} +
{joint} + +
+ {picked.size > 0 && ( +
+ + {picked.size} joint{picked.size === 1 ? "" : "s"} on the firmware loop this run + + +
+ )} +
+ ) } /** @@ -580,7 +1113,9 @@ function GainOverrideEditor({ for (const tok of tokens.split(/\s+/).filter(Boolean)) { const [path = "", v = ""] = tok.split("=") const parts = path.split(".") - const key = parts.length === 3 ? `${parts[1]}.${parts[2]}` : path + // `side.joint.field[.sub]` and `joint.field[.sub]` both map to the + // side-less cell key. + const key = parts[0] === "left" || parts[0] === "right" ? parts.slice(1).join(".") : path if (key in cells) cells[key] = v } return cells @@ -632,7 +1167,7 @@ function GainOverrideEditor({ {OVERRIDE_FIELDS.map((f) => ( - {f} + {overrideLabel(f)} ))} @@ -660,9 +1195,12 @@ function GainOverrideEditor({ setCells((prev) => ({ ...prev, [key]: seeds[key].text })) } }} - disabled={disabled} + disabled={ + disabled || + (MYACTUATOR_ONLY_FIELDS.has(field) && jointVendor(joint) === "damiao") + } className={cn( - "h-7 w-16 rounded border bg-[#1c1c1c] px-1.5 font-mono text-xs outline-none placeholder:text-white/25 focus:border-[#eff483]/40", + "h-7 w-16 rounded border bg-[#1c1c1c] px-1.5 font-mono text-xs outline-none placeholder:text-white/25 focus:border-[#eff483]/40 disabled:opacity-30", dirty ? "border-[#eff483]/50 text-[#eff483]" : "border-white/10 text-white/60" @@ -831,6 +1369,41 @@ function runFormValues(meta: TuningRunMeta): Record | null { out[key] = v } } + if (isA4Run(meta)) { + put("arm", meta.side) + put("joint", meta.joint) + put("mode", p.mode) + put("center", p.center_deg) + put("amp", p.amp_deg) + put("speed", p.speed_dps) + put("freq", p.freq_hz) + put("duration", p.duration_s) + put("rate", p.rate_hz) + put("cap", p.cap_dps) + if (typeof p.cap_track === "number" && p.cap_track > 0) put("cap_track", p.cap_track) + if (typeof p.cap_track === "number" && p.cap_track > 0) put("cap_floor", p.cap_floor_dps) + if (Array.isArray(p.accel) && typeof p.accel[0] === "number") out["accel"] = String(p.accel[0]) + if (Array.isArray(p.dm_acc) && typeof p.dm_acc[0] === "number") + out["dm_acc"] = String(p.dm_acc[0]) + if (Array.isArray(p.pose) && p.pose.length > 0) out["pose"] = p.pose.join(" ") + // Held joints' gains, as run: {joint: {gain: value}} → "joint.gain=value …". + if (p.held_gains && typeof p.held_gains === "object") { + const held = Object.entries(p.held_gains as Record>) + .flatMap(([j, gains]) => + Object.entries(gains ?? {}) + .filter(([, v]) => typeof v === "number" && Number.isFinite(v)) + .map(([n, v]) => `${j}.${n}=${fmtFwGain(v)}`) + ) + .join(" ") + if (held) out["held_gain"] = held + } + for (const k of ["position_kp", "position_ki", "position_kd", "speed_kp", "speed_ki"]) { + const v = g[k] + if (typeof v === "number" && Number.isFinite(v)) out[k] = fmtFwGain(v) + } + if (p.persist === true) out["persist"] = "true" + return out + } switch (meta.kind) { case "sine": case "step": @@ -854,6 +1427,7 @@ function runFormValues(meta: TuningRunMeta): Record | null { break case "motion": { put("motion", p.motion) + put("controller", p.controller) put("stiffness", p.stiffness) put("noise", p.noise) if (p.ik === true) out["ik"] = "true" @@ -863,6 +1437,18 @@ function runFormValues(meta: TuningRunMeta): Record | null { .map(([k, v]) => `${k}=${v}`) .join(" ") if (overrides) out["gain"] = overrides + if (Array.isArray(p.a4) && p.a4.length > 0) { + out["a4"] = p.a4.filter((t): t is string => typeof t === "string").join(" ") + } + if (Array.isArray(p.hold) && p.hold.length > 0) { + out["hold"] = p.hold.filter((t): t is string => typeof t === "string").join(" ") + } + if (Array.isArray(p.fast_impedance) && p.fast_impedance.length > 0) { + out["fast_impedance"] = p.fast_impedance + .filter((t): t is string => typeof t === "string") + .join(" ") + } + if (p.arms === "left" || p.arms === "right") out["arms"] = p.arms break } case "gravity": @@ -935,6 +1521,18 @@ function parseLiveProbe(lines: string[]): LiveProbe | null { return probe } +/** Firmware loop gains span 0.0001 … 1: four significant digits, no padding. */ +function fmtFwGain(v: unknown): string { + if (v == null || typeof v !== "number" || !Number.isFinite(v)) return "–" + return String(Number(v.toPrecision(4))) +} + +/** The baseline a gain box falls back to: the motor's live value, or config. */ +function baselineText(f: WbField, cfg: number | null): string { + if (cfg == null) return f.fwGainKey ? "motor" : "config" + return f.fwGainKey ? fmtFwGain(cfg) : fmtNum(cfg) +} + function fmtNum(v: unknown, digits = 2): string { if (v == null || typeof v !== "number" || !Number.isFinite(v)) return "–" const a = Math.abs(v) @@ -996,9 +1594,48 @@ function headline(meta: TuningRunMeta): { label: string; value: string } | null return null } +/** One side's wrist-IMU shake score (`almond_axol.tuning.wrist_imu`). */ +interface ImuScore { + shake_mm: number + shake_mm_p90: number + vertical_mm: number + vertical_mm_p90: number + low_mm?: number + high_mm?: number + acc_rms: number + gyro_rms: number | null + peak_hz: number +} + +/** A run's `imu` metrics block, per side, or null when it recorded none. */ +function imuScores(meta: TuningRunMeta): Record | null { + const block = (meta.metrics as Record).imu + if (!block || typeof block !== "object") return null + const out: Record = {} + for (const [side, v] of Object.entries(block as Record)) { + if (v && typeof v === "object" && typeof (v as ImuScore).shake_mm === "number") { + out[side] = v as ImuScore + } + } + return Object.keys(out).length > 0 ? out : null +} + +/** The worst side's IMU shake (mm, 1 s peak-to-peak), for the run list. */ +function imuHeadline(meta: TuningRunMeta): string | null { + const s = imuScores(meta) + if (!s) return null + const worst = Math.max(...Object.values(s).map((v) => v.shake_mm)) + return `${fmtNum(worst)} mm` +} + /** One per-joint chart: commanded vs actual position for a single joint. */ interface JointChart { joint: string + /** + * Shown after the joint in the chart title: "held" for a joint the run + * held steady (`tune.motion --hold`), "parked" for one it never moved. + */ + note?: string series: RunChartSeries[] /** Error lane (reference − output, in degrees) under the position plot. */ sub: RunChartSeries[] @@ -1031,10 +1668,39 @@ function errorLane( } /** Commanded-vs-actual charts for every joint of `arm` that actually moved. */ +/** `side.joint` columns a motion run held steady (`--hold SIDE.JOINT[=DEG]`). */ +function heldColumns(run: TuningRunData): Set { + const hold = run.meta.params.hold + return new Set( + (Array.isArray(hold) ? hold : []) + .filter((h): h is string => typeof h === "string") + .map((h) => h.split("=")[0] ?? h) + ) +} + +/** Whether a commanded series moves less than ~1° (0.017 rad) end to end. */ +function isStationary(values: (number | null)[]): boolean { + let min = Infinity + let max = -Infinity + for (const v of values) { + if (v == null) continue + if (v < min) min = v + if (v > max) max = v + } + return max - min < 0.017 +} + +/** + * Commanded vs actual per joint for a motion run. Joints that moved come + * first; joints the run held steady (`--hold`) or never moved follow, noted + * as such — a parked joint still buzzes or sags, which is worth seeing. + */ function motionJointCharts(run: TuningRunData, arm: string): JointChart[] { const columns = (run.meta.params.columns as string[] | undefined) ?? [] const t = run.series.t ?? [] + const held = heldColumns(run) const out: JointChart[] = [] + const still: JointChart[] = [] for (let i = 0; i < columns.length; i++) { const name = columns[i] if (!name?.startsWith(`${arm}.`)) continue @@ -1042,15 +1708,7 @@ function motionJointCharts(run: TuningRunData, arm: string): JointChart[] { const actual = run.series[`actual/${i}`] const sent = run.series[`sent/${i}`] if (!commanded || !actual || !actual.some((v) => v != null)) continue - // Only joints that were actually commanded to move (> ~1° of travel). - let min = Infinity - let max = -Infinity - for (const v of commanded) { - if (v == null) continue - if (v < min) min = v - if (v > max) max = v - } - if (max - min < 0.017) continue + const note = held.has(name) ? "held" : isStationary(commanded) ? "parked" : undefined const series: RunChartSeries[] = [ { label: "commanded", color: COMMANDED_COLOR, x: t, data: degSeries(commanded) }, ] @@ -1060,13 +1718,14 @@ function motionJointCharts(run: TuningRunData, arm: string): JointChart[] { series.push({ label: "sent", color: NOISY_COLOR, x: t, data: degSeries(sent) }) } series.push({ label: "actual", color: ACTUAL_COLOR, x: t, data: degSeries(actual) }) - out.push({ + ;(note ? still : out).push({ joint: name.slice(arm.length + 1), + note, series, sub: errorLane(t, commanded, actual), }) } - return out + return [...out, ...still] } /** @@ -1428,7 +2087,10 @@ function compareJointCharts(a: TuningRunData, b: TuningRunData, arm: string | nu const colsA = (a.meta.params.columns as string[] | undefined) ?? [] const colsB = (b.meta.params.columns as string[] | undefined) ?? [] const idxB = new Map(colsB.map((n, i) => [n, i])) + const heldA = kind === "motion" ? heldColumns(a) : new Set() + const heldB = kind === "motion" ? heldColumns(b) : new Set() const out: JointChart[] = [] + const still: JointChart[] = [] for (let i = 0; i < colsA.length; i++) { const name = colsA[i] if (arm != null && !name?.startsWith(`${arm}.`)) continue @@ -1438,16 +2100,21 @@ function compareJointCharts(a: TuningRunData, b: TuningRunData, arm: string | nu const refB = j != null ? b.series[`${refKey}/${j}`] : undefined const outB = j != null ? b.series[`${outKey}/${j}`] : undefined if (!refA || !outA || !refB || !outB) continue - let min = Infinity - let max = -Infinity - for (const v of refA) { - if (v == null) continue - if (v < min) min = v - if (v > max) max = v - } - if (max - min < 0.017) continue - out.push({ + // Motion runs keep held / parked joints (after the moving ones); a filter + // channel that never moves has nothing to compare. + const stationary = isStationary(refA) + if (stationary && kind !== "motion") continue + const note = + kind !== "motion" + ? undefined + : heldA.has(name) || heldB.has(name) + ? `held (${[heldA.has(name) && "A", heldB.has(name) && "B"].filter(Boolean).join(", ")})` + : stationary + ? "parked" + : undefined + ;(note ? still : out).push({ joint: arm != null ? name.slice(arm.length + 1) : name, + note, series: [ { label: refLabel, color: COMMANDED_COLOR, x: tA, data: degSeries(refA) }, { label: "A", color: ACTUAL_COLOR, x: tA, data: degSeries(outA) }, @@ -1459,7 +2126,7 @@ function compareJointCharts(a: TuningRunData, b: TuningRunData, arm: string | nu ], }) } - return out + return [...out, ...still] } /** A scorecard column: which metric key, how to show it. */ @@ -1498,6 +2165,23 @@ const SINE_COLS: ScoreCol[] = [ { key: "score", label: "score", digits: 3 }, ] +// tune.a4's creep-smoothness scorecard (see a4_metrics): the MIT frame's +// stick-slip sits near 0.8 velocity ripple, smooth is under 0.2. +const A4_COLS: ScoreCol[] = [ + { key: "rms", label: "tracking RMS °", deg: true, digits: 3, warn: 0.5, bad: 2.0 }, + { key: "max", label: "max err °", deg: true, digits: 3, warn: 1.5, bad: 5 }, + { key: "lag_ms", label: "lag ms", digits: 0, warn: 100, bad: 300 }, + { key: "v_ripple", label: "vel ripple", digits: 2, warn: 0.3, bad: 0.8 }, + { key: "stuck_frac", label: "stuck", digits: 2, warn: 0.05, bad: 0.3 }, + { key: "band_1_4", label: "1–4 Hz °", deg: true, digits: 3, warn: 0.1, bad: 0.3 }, + { key: "buzz", label: ">10 Hz buzz °", deg: true, digits: 3, warn: 0.02, bad: 0.1 }, + { key: "iq_mode", label: "3–8 Hz mode A", digits: 2, warn: 0.5, bad: 1.0 }, + { key: "iq_sd", label: "current spread A", digits: 2, warn: 1.3, bad: 1.8 }, + { key: "iq_rms", label: "current RMS A", digits: 2 }, + { key: "iq_max", label: "peak A", digits: 1 }, + { key: "hz", label: "loop Hz", digits: 0 }, +] + const FILTER_COLS: ScoreCol[] = [ { key: "input_rms", label: "noise in °", deg: true, digits: 3 }, { key: "rms_err", label: "error out °", deg: true, digits: 3 }, @@ -1542,6 +2226,17 @@ const STEP_COLS: ScoreCol[] = [ { key: "score", label: "score", digits: 3 }, ] +const A4_LEGEND = + "firmware position loop (0xA4). tracking RMS / max / lag = how the stream was " + + "followed. vel ripple = std of measured minus commanded velocity over the " + + "commanded speed — the MIT frame's stick-slip sits near 0.8, smooth is under 0.2. " + + "stuck = fraction of the pass with the joint not moving. 1–4 Hz = the stick-slip " + + "band in the error; >10 Hz buzz = high-frequency position motion. 3–8 Hz mode = " + + "the position loop's own mode in the current — the shudder felt at speed (a 12 deg/s " + + "triangle's reversals kick it to ~1.9 A at position_kp 0.7; 0.2 A is quiet); current " + + "spread = all current variation, the gravity hold removed. Anything above 100 Hz is " + + "invisible to the 200 Hz stream, so an audible buzz can leave every column clean." + const SCORE_LEGEND: Record = { motion: "tracking RMS = average distance from the commanded joint position " + @@ -1625,7 +2320,13 @@ function scoreRows( } if (meta.kind === "sine" || meta.kind === "step" || meta.kind === "gravity") { return { - cols: meta.kind === "sine" ? SINE_COLS : meta.kind === "step" ? STEP_COLS : GRAVITY_COLS, + cols: isA4Run(meta) + ? A4_COLS + : meta.kind === "sine" + ? SINE_COLS + : meta.kind === "step" + ? STEP_COLS + : GRAVITY_COLS, rows: [{ joint: meta.joint ?? "joint", values: m }], } } @@ -1858,6 +2559,11 @@ export function TuningWorkbench({ // Effective per-joint config gains (defaults + calibration): the slider // baselines and "config N" labels on the gain fields. const [gains, setGains] = useState(null) + const [wireModes, setWireModes] = useState(null) + // The selected motor's live firmware loop gains (Firmware-loop tab): read + // from the motor over the idle link whenever arm/joint change or a run + // ends, so the baselines are what the motor actually holds right now. + const [fwGains, setFwGains] = useState | null>(null) const [runs, setRuns] = useState([]) const [loading, setLoading] = useState(false) @@ -1903,7 +2609,10 @@ export function TuningWorkbench({ const refreshGains = useCallback(() => { fetchTuningGains() - .then(({ gains }) => setGains(gains)) + .then(({ gains, wire_modes }) => { + setGains(gains) + setWireModes(wire_modes ?? null) + }) .catch(() => {}) }, []) @@ -1942,7 +2651,7 @@ export function TuningWorkbench({ (meta: TuningRunMeta) => { select(meta.id) const form = runFormValues(meta) - const tabFor = KIND_TABS[meta.kind] + const tabFor = runTab(meta) if (!form || !tabFor) return setTabKey(tabFor) setValues((prev) => ({ ...prev, [tabFor]: form })) @@ -2021,12 +2730,49 @@ export function TuningWorkbench({ const setValue = useCallback( (key: string, v: string) => { - setValues((prev) => ({ ...prev, [tab.key]: { ...(prev[tab.key] ?? {}), [key]: v } })) + setValues((prev) => { + const cur = prev[tab.key] ?? {} + const next = { ...cur, [key]: v } + // A new arm or joint starts from that joint's own values: drop what + // was typed into the per-joint fields for the previous one, so each + // box falls back to the new joint's config / live-motor baseline. + if ((key === "arm" || key === "joint") && (cur[key] ?? "") !== v) { + for (const f of tab.fields) { + if (isPerJointField(f)) delete next[f.key] + } + } + return { ...prev, [tab.key]: next } + }) }, - [tab.key] + [tab.key, tab.fields] ) const tabValues = useMemo(() => values[tab.key] ?? {}, [values, tab.key]) + const fwArm = tabValues["arm"] ?? "" + const fwJoint = tabValues["joint"] ?? "" + useEffect(() => { + if (tabKey !== "a4" || !fwArm || !fwJoint || runningOurs) return + let stale = false + setFwGains(null) + fetchMotorDetails(fwArm, fwJoint.toUpperCase()) + .then((d) => { + if (stale) return + // Loop gains plus the planner acceleration, under one lookup so the + // accel field gets the same "motor N" baseline as the gains. + setFwGains({ + ...(d.gains ?? {}), + planner_accel: d.planner?.accel ?? null, + planner_decel: d.planner?.decel ?? null, + }) + }) + .catch(() => { + if (!stale) setFwGains(null) + }) + return () => { + stale = true + } + }, [tabKey, fwArm, fwJoint, runningOurs]) + // Sine and step probe the same joint with the same gains, so their shared // fields (arm, joint, kp/kd/kd_host/…, amp, rate, …) behave as one set: // switching between the two tabs carries the current values across — @@ -2064,6 +2810,9 @@ export function TuningWorkbench({ if (miss.length > 0) return const args: Record = { ...tab.presets } for (const f of tab.fields) { + // A value typed for the other vendor's knob stays in the form (it + // comes back if the joint does) but is never sent: tune.a4 refuses it. + if (!shownForJoint(f.vendors, tabValues["joint"])) continue const raw = (tabValues[f.key] ?? "").trim() if (!raw) continue args[f.key] = f.type === "boolean" ? raw === "true" : raw @@ -2077,6 +2826,10 @@ export function TuningWorkbench({ */ const configValue = useCallback( (f: WbField): number | null => { + if (f.fwGainKey) { + const v = fwGains?.[f.fwGainKey] + return typeof v === "number" && Number.isFinite(v) ? v : null + } if (!f.gainKey || !gains) return null const side = tabValues["arm"] const joint = tabValues["joint"] @@ -2084,7 +2837,7 @@ export function TuningWorkbench({ const v = gains[side]?.[joint]?.[f.gainKey] return typeof v === "number" && Number.isFinite(v) ? v : null }, - [gains, tabValues] + [gains, fwGains, tabValues] ) const meta = run?.meta ?? null @@ -2102,7 +2855,7 @@ export function TuningWorkbench({ return single ? [single] : [] }, [run, arm, armed]) const scores = meta ? scoreRows(meta, armed ? arm : null) : null - const legend = meta ? SCORE_LEGEND[meta.kind] : null + const legend = meta ? (isA4Run(meta) ? A4_LEGEND : SCORE_LEGEND[meta.kind]) : null const perJoint = (meta?.metrics as Record | undefined)?.per_joint as | Record> | undefined @@ -2236,122 +2989,173 @@ export function TuningWorkbench({

{tab.description}

- {tab.fields.map((f) => { - const cfg = configValue(f) - return ( - - ) - })} + className={cn( + "h-8 rounded-md border border-white/10 bg-[#1c1c1c] px-2 text-xs text-white/85 outline-none focus:border-[#eff483]/40", + f.width ?? "w-32" + )} + > + {defaultOpt == null && ( + + )} + {(f.key === "motion" + ? motions.map((m) => ({ value: m.name, label: m.name })) + : f.key === "prefix" && tab.key === "build" + ? recordings.map((r) => ({ + value: r.name, + label: + `${r.name} — ` + + (r.kind === "gravity-comp" ? "hand-guided" : "teleop") + + (r.durationS != null ? ` · ${Math.round(r.durationS)}s` : ""), + })) + : (f.options ?? []).map((o) => ({ + value: o, + label: o === defaultOpt ? `${o} (default)` : o, + })) + ).map((o) => ( + + ))} + + ) + })() + ) : f.slider ? ( + (() => { + // The slider tracks the typed value (first number of a + // sweep) and starts at the joint's config value; dragging + // it fills the box, an empty box runs with config. + const raw = (tabValues[f.key] ?? "").trim() + const first = Number.parseFloat(raw.split(/\s+/)[0] ?? "") + const sliderVal = Number.isFinite(first) ? first : (cfg ?? f.slider.min) + return ( + + setValue(f.key, e.target.value)} + disabled={runningOurs || busy || (cfg == null && !raw)} + title={f.hint} + className="w-24 accent-[#eff483] disabled:opacity-40" + /> + setValue(f.key, e.target.value)} + disabled={runningOurs || busy} + className="h-8 w-16 rounded-md border border-white/10 bg-[#1c1c1c] px-2 font-mono text-xs text-white/85 outline-none placeholder:text-white/25 focus:border-[#eff483]/40" + /> + + ) + })() + ) : ( + setValue(f.key, e.target.value)} + disabled={runningOurs || busy} + className={cn( + "h-8 rounded-md border border-white/10 bg-[#1c1c1c] px-2 font-mono text-xs text-white/85 outline-none placeholder:text-white/25 focus:border-[#eff483]/40", + f.width ?? (f.type === "number" ? "w-24" : "w-28") + )} + /> + )} + + ) + })}
{runningThisTab ? (