diff --git a/.eidetic/memory/arm101-cli__public.jsonl b/.eidetic/memory/arm101-cli__public.jsonl index ea622b3..308594f 100644 --- a/.eidetic/memory/arm101-cli__public.jsonl +++ b/.eidetic/memory/arm101-cli__public.jsonl @@ -11,3 +11,5 @@ {"id": "wrist-roll-cannot-be-re-zeroed", "hash": "5cdf5d348b52a7306604d48e39851288b73411c86bbda7a4021299be9dfe1079", "content": "DESIGN FINDING (2026-07-12, /think spec for issues #33/#34/#35): RE-ZERO CANNOT FIX wrist_roll, EVEN IN PRINCIPLE. Issue #35 recommended re-zeroing BOTH wrapping joints (elbow_flex, wrist_roll) so the 4095->0 encoder seam falls outside physical travel. That works for elbow_flex \u2014 it has real walls (measured span 2020), so the seam can be parked in its UNREACHABLE arc. It is IMPOSSIBLE for wrist_roll: wrist_roll was mapped as free through its whole range (measured [21,4073], no contact anywhere \u2014 it stopped only where the 0-4095 clamp stopped it, not at a wall), i.e. its travel covers the WHOLE CIRCLE. No choice of zero can move a seam out of a travel that includes every angle \u2014 re-zero only relocates the seam, it cannot evict it. USER DECISION: elbow_flex is RE-ZEROED; wrist_roll is SOFT-LIMITED instead (a software bound in arm_spec/the map, NOT an EEPROM angle-limit write) to under a full turn, creating a DEAD ARC that must CONTAIN the seam. Acceptance test = sweep wrist_roll end-to-end of its soft-limited range and assert present_position is MONOTONIC (no 4095->0 jump inside permitted travel). GENERAL RULE: for any joint, ask first whether its travel is bounded or continuous \u2014 a re-zero is only available to a BOUNDED joint. SECOND FINDING from the same session: bus.py implements NO offset/homing register today (grep: only ADDR 5 id, 6 baud, 40 torque, 41 accel, 42 goal_pos, 46 speed, 48 torque_limit, 55 lock, 56 present_pos) and the STS3215 offset mechanism is UNVERIFIED \u2014 so the re-zero wave is gated on a READ-ONLY spike (identify register address/width/EEPROM-vs-RAM/range, prove it survives a power cycle) before any EEPROM write. THIRD: a re-zero INVALIDATES every tick ever recorded for that joint (maps, thresholds, run-logs, arm_spec bounds), so reachability maps must carry a CALIBRATION IDENTITY and explore must REFUSE a map measured under a different calibration rather than silently misreading it.", "scope": {"name": "arm101-cli", "visibility": "public"}, "metadata": {"type": "note", "record_metadata": {"source": "think-spec", "area": "arm101/hardware/arm_spec.py", "issue": "35", "status": "spec-converged", "spec": "docs/specs/2026-07-12-arm-explore-now-maps-the-arm-s-real-joint-space-it.md"}, "created": "2026-07-12T08:07:28.070943+00:00", "last_recall": null, "recall_count": 0, "links": ["elbow-flex-encoder-wraps-within-travel", "arm-explore-defaults-unusable-at-real-speeds", "motion-verb-crash-leaves-motors-energized"], "supersedes": null, "lifecycle": "active", "added_by": null}} {"id": "torque-release-must-use-clear-overload", "hash": "95dddbbd5ff09773f25ba98eced1dee2798d20484b726965e71e7251a815ca4f", "content": "BUILD FINDING (2026-07-12, #33 safety wave, PR #38 v0.19.0): THE OBVIOUS TORQUE-RELEASE CALL IS THE WRONG ONE. To de-energize a motor on an abnormal exit, route through bus.clear_overload(motor), NOT bus.enable_torque(motor, False). Both are the IDENTICAL wire write (Torque_Enable=0, addr 40) and differ only in how they read the RESPONSE: a servo latched in overload tags EVERY packet response with the overload bit (0x20) \u2014 including the response to the very write that clears the latch \u2014 so enable_torque() raises OverloadError on exactly the motor that most needs de-energizing. clear_overload() masks that bit and treats it as success (bus.py:1136). A crashed motion verb is a prime way to leave a motor latched, so this is the common case, not the exotic one.\n\nSECOND FINDING (same wave): FakeBus.clear_overload used to record its write SEPARATELY instead of routing through FakeBus.enable_torque. Since the two are the same byte on the wire, a test subclass could intercept enable_torque and SILENTLY MISS the identical byte written by clear_overload \u2014 i.e. the fake modelled a bus that cannot exist, and would let a test PROVE a torque-disable never happened when on real hardware it did. Fixed: FakeBus.clear_overload now calls self.enable_torque and tolerates OverloadError. LESSON: when two bus methods are the same wire op, the fake MUST funnel them through one overridable method or its subclass seams lie.\n\nTHIRD: torque_guard semantics = HOLD ON SUCCESS, RELEASE ON ABNORMAL (user decision). Clean exit issues ZERO release writes so gentle_move stop-and-hold survives (a closed gripper must not drop its object); abnormal exit (any BaseException, so SIGINT included) releases. TorqueGuard.__exit__ is typed -> None (not -> bool) \u2014 Sonar S3516 BLOCKER on a constant `return False`; None is falsy so behavior is identical and the signature states the never-suppress guarantee.\n\nFOURTH: TorqueGuard.disown() exists for arm setup ONLY \u2014 writing a new servo id to EEPROM addr 5 MOVES the servo bus address, so a guard still owning the old id would aim its release at a servo that no longer exists and falsely report a motor as possibly-energized.", "scope": {"name": "arm101-cli", "visibility": "public"}, "metadata": {"type": "note", "record_metadata": {"source": "build", "area": "arm101/hardware/safety.py", "issue": "33", "pr": "38", "version": "0.19.0"}, "created": "2026-07-12T08:50:35.892215+00:00", "last_recall": null, "recall_count": 0, "links": ["motion-verb-crash-leaves-motors-energized", "present-load-saturates-at-torque-limit"], "supersedes": null, "lifecycle": "active", "added_by": null}} {"id": "usb-yank-cannot-be-fixed-in-software", "hash": "a6744430cda0c6d8dc7d991854d125b9f3636a6779ef710a3d85c93af009da05", "content": "HARD LIMIT (2026-07-12, found while drafting t4 hardware acceptance for #33): A PHYSICAL USB YANK CANNOT BE RECOVERED IN SOFTWARE, and I wrongly wrote it into the spec as an achievable acceptance criterion (honesty conditions h1/h25) before catching it. The SO-101 servos are powered from the 12V SUPPLY; USB carries only DATA. Unplug the cable and /dev/ttyACM* disappears \u2014 the host has NO channel to the arm, every torque-release write fails, and the motors keep holding their last commanded goal. No torque_guard, no finally, no context manager can release torque over a bus that no longer exists.\n\nWHAT THE GUARD *CAN* COVER (all real, all fixed by PR #38): every abnormal exit where the PROCESS STILL HAS A BUS \u2014 SIGINT/Ctrl-C, any unhandled Python exception, and the ACTUAL #33 incident (a second process opening the port; the error was `device reports readiness to read but returned no data (multiple access on port?)` \u2014 the port is CONTENDED but still ours, so the release lands).\n\nThe only thing that could close the yanked-cable hole is a SERVO-SIDE watchdog (the STS3215 dropping torque by itself on comms loss). Whether one exists is UNKNOWN and is filed as issue #37 \u2014 it pairs with the offset-register spike (plan task t5) since both interrogate the same memory table. CAVEAT for that investigation: a watchdog that drops torque during a LEGITIMATE hold would break gentle_move`s stop-and-hold gripper contract, so any timeout must exceed any real hold or it is unusable.\n\nGENERAL LESSON: before writing a testable acceptance criterion, check it is PHYSICALLY achievable. I asserted this one and the user confirmed it on my word; it would have cost a hardware session to discover.", "scope": {"name": "arm101-cli", "visibility": "public"}, "metadata": {"type": "note", "record_metadata": {"source": "spec-review", "area": "arm101/hardware/safety.py", "issue": "37", "severity": "known-limit"}, "created": "2026-07-12T08:50:35.973210+00:00", "last_recall": null, "recall_count": 0, "links": ["torque-release-must-use-clear-overload", "motion-verb-crash-leaves-motors-energized"], "supersedes": null, "lifecycle": "active", "added_by": null}} +{"id": "sts3215-offset-register-facts", "hash": "6cc56871bfa8a793c4ac8db344448990cfcacad772a1faa065b3c5ecf0e14bc8", "content": "STS3215 ENCODER-OFFSET REGISTER \u2014 verified facts (t5 spike, 2026-07-12, triple-sourced: Feetech SMS_STS.h/.cpp, Feetech FTServo_Python, LeRobot tables.py/feetech.py/encoding_utils.py). Register `Ofs` (LeRobot: Homing_Offset): ADDRESS 31 (OFS_L; 32 = OFS_H), 2 BYTES, encoding SIGN-MAGNITUDE WITH SIGN ON BIT 11 \u2014 NOT twos complement (wire = (sign<<11)|magnitude; -1073 -> 3121, +1073 -> 1073). RANGE +/-2047 (max magnitude (1<<11)-1). EEPROM (persistent) \u2014 sits under //---EEPROM--- in SMS_STS.h; SRAM starts at addr 40. Therefore needs the addr-55 Lock unlock->write->relock dance (same region as ID/Baud \u2014 the exact registers PR #21 proved silently revert without it). EFFECT: Present_Position = Actual_Position - Homing_Offset.\n\nRANGE MATH for elbow_flex: its unreachable arc spans raw (126, 2020); the offset must land INSIDE that arc; midpoint H = 1073, |1073| <= 2047 with 974 ticks headroom. KEY INSIGHT: the required offset is bounded by WHERE THE UNREACHABLE ARC SITS, not by the travel length \u2014 which is why +/-2047 clears a ~2020-tick travel comfortably. LeRobot half-turn rule independently gives 1074.\n\nSECOND MECHANISM: writing 128 to Torque_Enable (addr 40) triggers Feetech firmware midpoint calibration (CalibrationOfs). Needs no wrap-aware move \u2014 human hand-parks the joint, torque off, one byte \u2014 sidestepping the chicken-and-egg that you cannot COMMAND a move across a seam you have not fixed yet.\n\nTRAP FOR ANY IMPLEMENTER: do NOT copy LeRobot write_calibration wholesale \u2014 it ALSO writes Min/Max_Position_Limit (addrs 9/11), which in servo mode CLAMP goals and would narrow the very reachable set you are recovering. Write ONLY addr 31.\n\nUNRESOLVED CAVEAT (the whole ballgame): it is UNDOCUMENTED whether the firmware computes Present = (raw - Ofs) MOD 4096 (seam RELOCATES -> re-zero works) or plain signed subtraction (seam STAYS PINNED at the physical angle where the magnet rolls over -> re-zero achieves NOTHING). Present_Position is itself sign-magnitude on bit 15 in both Feetech and LeRobot, so negatives ARE representable. Evidence favours modular (LeRobot SO-101 calibration = literally this fix; community says centring PREVENTS overflow) but is circumstantial. SETTLED ONLY BY A HAND-SWEEP with torque off, checking monotonicity \u2014 reading the offset back proves it was APPLIED, NOT that the seam MOVED.", "scope": {"name": "arm101-cli", "visibility": "public"}, "metadata": {"type": "reference", "record_metadata": {"source": "spike", "area": "arm101/hardware/bus.py", "issue": "35", "pr": "40", "spike": "docs/spikes/sts3215-offset-register.md"}, "created": "2026-07-12T10:24:51.133820+00:00", "last_recall": null, "recall_count": 0, "links": ["encoder-wrap-two-joints", "wrist-roll-cannot-be-re-zeroed", "arm-alive-eeprom-lock-fix"], "supersedes": null, "lifecycle": "active", "added_by": null}} +{"id": "seam-verification-false-pass-traps", "hash": "fb45432f219a4ecc4067c26ada3e28d37e262c1043db183481fda358c28356a6", "content": "TWO TRAPS THAT WOULD PRODUCE A FALSE PASS when verifying an encoder re-zero (found building t8, PR #40, 2026-07-12). Both matter because the verification is the ONLY thing standing between us and shipping a re-zero that silently did nothing.\n\nTRAP 1 \u2014 THE DISCONTINUITY THRESHOLD MUST BE ~500, NOT THE OBVIOUS 2048. The intuition is `a seam jump is ~4096 ticks, so anything over half that (2048) is a wrap`. WRONG: if the firmware does a plain signed subtraction AND read_position`s `& 0x0FFF` mask folds the resulting negative back into range, the observed seam jump is only ~1949 ticks. A 2048 threshold sails straight past it and reports PASS on a broken fix. Use 500 (sensor noise is a few ticks; there is a huge gap between noise and any real discontinuity).\n\nTRAP 2 \u2014 A SHORT CLEAN SWEEP IS `INCONCLUSIVE`, NEVER A PASS. A hand-sweep covering 200 of 2202 ticks that saw no discontinuity has proven NOTHING \u2014 the seam simply was not in the swept region. Claiming a pass from it closes an open question with a lie. Require >=80% travel coverage before `no discontinuity found` is allowed to mean anything.\n\nGENERAL LESSON: when a verification exists to settle an unproven premise, enumerate how it could report SUCCESS while the premise is false. Both traps here are of that shape.\n\nRELATED DESIGN CALL THAT PAID OFF: rather than assume the favourable semantics, FakeBus models BOTH via `offset_wraps` (True = seam relocates, the premise; False = seam stays pinned, the pessimistic reading). The verb is tested against each, so if hardware settles it the wrong way the pessimistic behaviour is already built and tested, not discovered in production.", "scope": {"name": "arm101-cli", "visibility": "public"}, "metadata": {"type": "note", "record_metadata": {"source": "build", "area": "arm101/hardware/rezero.py", "issue": "35", "pr": "40"}, "created": "2026-07-12T10:24:51.190254+00:00", "last_recall": null, "recall_count": 0, "links": ["sts3215-offset-register-facts", "wrist-roll-cannot-be-re-zeroed"], "supersedes": null, "lifecycle": "active", "added_by": null}} diff --git a/CHANGELOG.md b/CHANGELOG.md index 7796c74..354d5f0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,6 +5,32 @@ All notable changes to this project will be documented in this file. Format follows [Keep a Changelog](https://keepachangelog.com/). This project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html). +## [0.21.0] - 2026-07-12 + +### Added + +- `arm rezero ` — a gated EEPROM write that evicts the encoder seam from a wrapping joint travel. Writes only the STS3215 `Ofs` register (addr 31, sign-magnitude on bit 11, range +/-2047, EEPROM) via the unlock -> write -> relock dance PR #21 established. The offset is DERIVED from the joint measured unreachable arc, never typed. +- `arm rezero --verify` — the seam-eviction proof. Torque off; the operator hand-moves the joint through its travel while the verb polls position and asserts MONOTONICITY. Reading the offset back only proves it was APPLIED; only the sweep proves the seam MOVED. +- `arm101/hardware/bus.py`: `read_offset` / `write_offset` / `encode_offset` / `decode_offset`, plus a `FakeBus` that models the offset effect on reported position. `arm read` now shows the signed offset (read-only). +- `docs/spikes/sts3215-offset-register.md` — the triple-sourced research behind the register, including the range arithmetic and the one semantic that is still unproven. +- `docs/hardware-rezero-procedure.md` — the followable procedure, including the mandatory power-cycle. + +### Changed + +- `wrist_roll` gains a software soft limit (100, 3995) whose dead arc CONTAINS the 4095->0 seam, + and `arm_spec.resolve_bounds()` now INTERSECTS each joint's EEPROM limits with its soft limit. + Every site that resolved move bounds from EEPROM (`arm flex`, `arm explore`'s grid, the demo + sweep) routes through it, so the dead arc is genuinely unreachable rather than merely declared. + +### Fixed + +- `arm rezero` tolerates a motor latched in overload. Planning does register READS, and on a + latched servo reads raise too — so the verb aborted before reaching the only code that clears + the latch. Since `elbow_flex`'s unreachable arc is measured by driving the joint into a wall + (exactly how a servo latches), this fired on the documented procedure every time. The latch is + now cleared and planning retried once; the recovery is conditional, so a healthy holding joint + is never silently de-energised. Found by qodo on PR #40. + ## [0.20.1] - 2026-07-12 ### Fixed diff --git a/arm101/cli/_commands/arm.py b/arm101/cli/_commands/arm.py index c70816d..4922515 100644 --- a/arm101/cli/_commands/arm.py +++ b/arm101/cli/_commands/arm.py @@ -1,4 +1,4 @@ -"""``arm101 arm`` — arm noun group (overview + read + flex + explore + profile + setup). +"""``arm101 arm`` — arm noun group (overview + read + flex + explore + profile + rezero + setup). Verbs ----- @@ -10,12 +10,19 @@ ``arm read`` Read every joint's live register state (position/load/speed/voltage/ - temperature/torque) via :func:`~arm101.hardware.arm_read.read_arm`. + temperature/torque, plus the signed encoder ``offset``) via + :func:`~arm101.hardware.arm_read.read_arm`. Read-only: it opens a bus and reads, but commands no motion and writes no register — so it carries NO consent gate. Retry-tolerant: a joint whose reads keep failing is marked ``failed``/``partial`` while the rest still read. Supports ``--role``, ``--port``, ``--json``. + ``offset`` is the servo's ``Ofs``/``Homing_Offset`` (EEPROM addr 31), shown + signed. It exists here so a human can INSPECT the encoder re-zero that + issue #35 needs for ``elbow_flex`` without writing anything; the write + primitive lives on the bus (``MotorBus.write_offset``) and is not exposed + as a verb by this task. + ``arm flex`` Gated motion: move one joint to ``--to ``, or sweep every joint with ``--demo``. ``--gentle`` uses the load-watch back-off-then-hold primitive @@ -57,6 +64,25 @@ hand-fitted in one bench session and this is what replaces the guess. Gated by the same three-mode consent as ``arm flex``. +``arm rezero `` + Gated **EEPROM write, and no motion at all**: shift the servo's encoder zero + (``Ofs``/``Homing_Offset``, addr 31) so the 4095->0 seam falls in the arc the + joint physically cannot reach — the fix for issue #35, and the only joint it + applies to is ``elbow_flex``. ``wrist_roll`` is REFUSED with the reason (a + re-zero relocates a seam, it cannot evict one from a joint that turns all the + way round; it has a soft limit instead), and so are the four joints that never + wrap. Gated by the same three-mode consent as ``arm flex`` (dry_run / + interactive / agent ``--apply``); the dry-run touches no bus at all and prints + the exact register writes. + + ``--verify`` runs the **seam-eviction proof** instead of the write: torque + off, the human hand-moves the joint through its whole travel, and the verb + polls ``present_position`` and asserts there is **no discontinuity anywhere**. + Reading the offset back proves only that it was APPLIED; only the sweep proves + the seam MOVED. A discontinuity under a written offset is a STOP condition — + the verb fails loudly (exit 2) because the re-zero then achieves nothing. See + :mod:`arm101.hardware.rezero`. + ``arm setup `` Drive the existing setup-motors gated three-mode-consent walk (dry_run / interactive / agent — see :mod:`arm101.cli._consent`) for the given role @@ -74,6 +100,17 @@ :func:`arm101.cli._commands.setup_motors._process_one_motor` — that is where its per-motor bus is opened.) ``read`` does not: it energises nothing. +``rezero`` is guarded too, and it is the one verb that guards a motor it never +energises. That is not over-claiming for its own sake: it is a verb whose whole +job is to leave the joint LIMP — it de-energises before the EEPROM write (a +servo must not be *holding* while its frame of reference changes underneath it) +and again before the ``--verify`` sweep (a human is about to move the joint by +hand). A crash between the torque-off and the re-lock, or an operator's Ctrl-C +mid-sweep on a joint some earlier verb left hot, must both end with the motor +released, and the guard is what makes that true without rezero having to +re-implement it. The release is a no-op on an already-limp motor, so the guard +costs nothing on every path where nothing went wrong. + This exists because an ``arm explore`` run died on an unhandled ``serial.SerialException`` — a second process had opened the port — and left **all six motors energised**, holding the arm up against gravity at ~50 C with @@ -123,8 +160,9 @@ from arm101.explore import engine from arm101.explore.budget import DEFAULT_MAX_MOVES, Budget from arm101.explore.types import GridSpec, JointConfig -from arm101.hardware import arm_spec +from arm101.hardware import arm_spec, rezero from arm101.hardware.arm_read import JointReading, is_complete, read_arm +from arm101.hardware.bus import OverloadError, encode_offset from arm101.hardware.demo import demo_sweep from arm101.hardware.gentle import CONTACT_LOAD_CEILING as _CONTACT_LOAD_CEILING from arm101.hardware.gentle import gentle_move @@ -167,6 +205,9 @@ #: Consent verb label for ``arm profile`` (hoisted to avoid duplicating the literal). _PROFILE_VERB = "arm profile" +#: Consent verb label for ``arm rezero`` (hoisted to avoid duplicating the literal). +_REZERO_VERB = "arm rezero" + #: Shared dry-run footer for the gated motion verbs (hoisted; identical text). _DRY_RUN_FOOTER = "No motion commanded (dry-run). Re-run non-interactively with --apply to execute." @@ -219,7 +260,7 @@ def cmd_arm_overview(args: argparse.Namespace) -> None: payload: dict[str, object] = { "noun": "arm", - "verbs": ["overview", "read", "flex", "explore", "profile", "setup"], + "verbs": ["overview", "read", "flex", "explore", "profile", "rezero", "setup"], "roles": arm_spec.roles(), "motor_map": roles_data, } @@ -231,7 +272,7 @@ def cmd_arm_overview(args: argparse.Namespace) -> None: lines = [ "## arm — arm-level operations", "", - "Verbs: overview, read, flex, explore, profile, setup", + "Verbs: overview, read, flex, explore, profile, rezero, setup", "", "Roles: " + ", ".join(arm_spec.roles()), "", @@ -361,6 +402,10 @@ def _emit_read( "voltage": r.voltage, "temperature": r.temperature, "torque": r.torque, + # Signed encoder offset (Ofs/Homing_Offset, EEPROM addr + # 31) — read-only. Issue #35 re-zeros elbow_flex by + # writing this; seeing it must never require writing it. + "offset": r.offset, } for r in readings ], @@ -372,15 +417,18 @@ def _emit_read( lines = [ f"## arm read ({role}) — {port}", "", - "| joint | id | health | position | load | speed | voltage | temperature | torque |", - "|-------|----|--------|----------|------|-------|---------|-------------|--------|", + "| joint | id | health | position | load | speed | voltage | temperature | torque" + " | offset |", + "|-------|----|--------|----------|------|-------|---------|-------------|--------" + "|--------|", ] for r in readings: mark = " [OVERLOAD]" if r.overloaded else "" lines.append( f"| {r.joint} | {r.motor_id} | {r.health} | {_fmt_cell(r.position)}" f" | {_fmt_cell(r.load)} | {_fmt_cell(r.speed)} | {_fmt_cell(r.voltage)}" - f" | {_fmt_cell(r.temperature)} | {_fmt_cell(r.torque)} |{mark}" + f" | {_fmt_cell(r.temperature)} | {_fmt_cell(r.torque)}" + f" | {_fmt_cell(r.offset)} |{mark}" ) lines.append("") @@ -523,6 +571,42 @@ def _confirm_flex( return False +def _resolve_joint_bounds(joint: str, info: "dict[str, int]") -> "tuple[int, int]": + """Turn one joint's ``read_info`` snapshot into the bounds a move may use. + + The single place in this module where a servo's EEPROM angle limits become + move bounds — deliberately, so the soft limit cannot be forgotten at one + call site and honoured at another. It intersects the EEPROM range with the + joint's :data:`~arm101.hardware.arm_spec.SOFT_LIMITS` entry (see + :func:`~arm101.hardware.arm_spec.resolve_bounds`): on this arm the EEPROM + is the untouched factory ``0-4095`` on every joint, so for ``wrist_roll`` + — whose travel wraps the encoder seam — the EEPROM alone would happily + permit a move into the dead arc and across the seam. + + The soft limit is read-side ONLY: this reads the servo's registers, it + never writes the resolved range back into EEPROM. + + Raises + ------ + CliError(EXIT_ENV_ERROR) + If the servo's configured range and the joint's soft limit have no + overlap at all (the servo is configured to live entirely inside the + dead arc). That is a hardware/configuration contradiction, not a bad + argument from the user — hence an ENV error, raised before any motion. + """ + try: + return arm_spec.resolve_bounds(joint, int(info["min_angle"]), int(info["max_angle"])) + except ValueError as exc: + raise CliError( + code=EXIT_ENV_ERROR, + message=str(exc), + remediation=( + f"Check {joint}'s min_angle/max_angle with 'arm101 arm read --json'; they " + "contradict the joint's software travel limit, so no move is possible." + ), + ) from exc + + def _execute_single( bus: object, role: str, @@ -534,8 +618,7 @@ def _execute_single( """Run a single-joint move (gentle or compliant) and return its result dict.""" motor_id = arm_spec.joint_ids(role)[joint] info = bus.read_info(motor_id) # type: ignore[attr-defined] - min_angle = info["min_angle"] - max_angle = info["max_angle"] + min_angle, max_angle = _resolve_joint_bounds(joint, info) if gentle: return gentle_move( bus, # type: ignore[arg-type] @@ -714,18 +797,27 @@ def _build_grid_spec(bus: object, role: str, resolution: int) -> GridSpec: """Read the live arm state and build the exploration :class:`GridSpec`. Each joint's live position seeds the grid origin (home), each joint's - calibrated ``[min_angle, max_angle]`` seeds the per-joint bounds, and + calibrated ``[min_angle, max_angle]`` — intersected with its software soft + limit via :func:`_resolve_joint_bounds` — seeds the per-joint bounds, and *resolution* is the uniform per-joint bucket size. Reads flow through ``bus.read_info`` — a per-joint read failure propagates as a :class:`CliError` (never a traceback), matching ``arm read``/``arm flex``. + + Soft-limiting the GRID is what soft-limits the whole exploration run: the + engine takes every move bound it ever uses from ``GridSpec.bounds`` (both + the flood-fill's neighbour moves and the multi-joint escape probes read + ``spec.bounds[joint]``), so a bound that never crosses the encoder seam + here cannot be crossed anywhere downstream. The origin is then clamped + into those same bounds — which matters concretely: the t9 hardware run + found ``wrist_roll`` parked at raw tick 4, sitting ON the seam, and the + flood-fill must start from a cell the joint is actually permitted to be in. """ ids = arm_spec.joint_ids(role) origin_ticks: "list[int]" = [] bounds: "list[tuple[int, int]]" = [] for joint in arm_spec.JOINTS: info = bus.read_info(ids[joint]) # type: ignore[attr-defined] - bound_min = int(info["min_angle"]) - bound_max = int(info["max_angle"]) + bound_min, bound_max = _resolve_joint_bounds(joint, info) position = max(bound_min, min(bound_max, int(info["present_position"]))) origin_ticks.append(position) bounds.append((bound_min, bound_max)) @@ -1422,6 +1514,554 @@ def cmd_arm_profile(args: argparse.Namespace) -> None: _emit_profile_result(role, port, prof, json_mode=json_mode) +# --------------------------------------------------------------------------- +# arm rezero (gated EEPROM write — and NOT a move) +# --------------------------------------------------------------------------- + + +def _rezero_write_sequence(offset: int) -> "list[str]": + """The exact wire writes ``rezero`` will perform, as the dry-run prints them. + + Rendered from the same :func:`~arm101.hardware.bus.encode_offset` the write + itself uses, so the plan cannot claim one wire value and the write send + another. Note what is NOT in the list: no goal position, at any point. See + :mod:`arm101.hardware.rezero` for why commanding motion here would rotate + ``elbow_flex`` the long way round, through its whole travel, into a wall. + """ + wire = encode_offset(offset) + return [ + "write1ByteTxRx(addr=40, value=0) # Torque_Enable OFF — the servo must not be " + "holding while its own frame of reference moves", + "write1ByteTxRx(addr=55, value=0) # Lock OPEN — without this the write reads back " + "fine and REVERTS on the next power-cycle (PR #21)", + f"write2ByteTxRx(addr=31, value={wire}) # Ofs/Homing_Offset = {offset:+d} " + "(sign-magnitude on bit 11), and NOTHING else — not min/max angle limits", + "write1ByteTxRx(addr=55, value=1) # Lock CLOSED — restore write-protection", + "(no goal position is ever written — this verb commands NO motion)", + ] + + +def _emit_rezero_plan( + role: str, + joint: str, + motor: int, + offset: int, + arc: "arm_spec.UnreachableArc", + verify: bool, + duration: float, + *, + port: "str | None", + json_mode: bool, +) -> None: + """Emit the dry-run plan — **zero bus access**, not merely zero writes. + + Deliberately offline, exactly like ``arm flex``'s and ``arm explore``'s + dry-runs: everything a plan can honestly say about a re-zero is already + known without a servo (the offset is derived from the arc table, the wire + sequence from the offset), and everything it CANNOT say offline — what the + joint currently reads, what offset it already holds, whether it is somewhere + it should not be — is a live fact that is checked at apply time, where it can + actually be acted on. A plan that opened a port would fail on a laptop and + still not know any more than this one does. Use ``arm read`` to see the live + registers, including the ``offset`` column. + """ + plan: "dict[str, object]" = { + "verb": _REZERO_VERB, + "role": role, + "joint": joint, + "motor": motor, + "port": port or _PORT_UNRESOLVED, + "mode": "verify" if verify else "write", + } + if verify: + plan.update( + { + "duration_s": duration, + "note": ( + "COMMANDS NO MOTION, and DE-ENERGISES the joint: torque goes off and " + "STAYS off while YOU hand-move it through its entire travel. An arm " + "holding a pose will sag when its torque is released — support it. The " + "sweep polls present_position and asserts there is no discontinuity " + "anywhere; a discontinuity means the re-zero did not evict the seam." + ), + "writes": ["write1ByteTxRx(addr=40, value=0) # Torque_Enable OFF"], + } + ) + else: + plan.update( + { + "current_offset": "(read at apply)", + "target_offset": offset, + "wire_value": encode_offset(offset), + "register": f"addr {31} (Ofs/Homing_Offset, EEPROM, 2 bytes)", + "unreachable_arc": [arc.low, arc.high], + "seam_moves_to_raw_tick": arc.midpoint, + "expected_travel_ticks": arc.travel_ticks, + "note": ( + "COMMANDS NO MOTION. This is a persistent EEPROM write: it shifts the " + "joint's encoder zero so the 4095->0 seam falls inside the arc the " + "joint cannot reach. The joint is NOT moved — not before, not during, " + "not after." + ), + "writes": _rezero_write_sequence(offset), + } + ) + + if json_mode: + emit_result({"plan": plan}, json_mode=True) + return + + lines = [f"## Dry-run plan: {_REZERO_VERB} {joint}", ""] + for key, value in plan.items(): + if key == "writes": + continue + lines.append(f"- {key}: {value}") + lines += ["", "### Register writes", ""] + lines += [f" {line}" for line in plan["writes"]] # type: ignore[union-attr] + lines += [ + "", + "No motion commanded, and no bus opened (dry-run). Re-run non-interactively " + "with --apply to execute.", + ] + emit_result("\n".join(lines), json_mode=False) + + +def _confirm_rezero(joint: str, motor: int, offset: int, verify: bool, *, json_mode: bool) -> bool: + """Prompt the human; return True to proceed, False (and emit an abort) otherwise.""" + if verify: + emit_diagnostic( + f"⚠ This DE-ENERGISES {joint} (motor {motor}) and LEAVES IT LIMP: torque goes " + "off and stays off while you hand-move the joint through its entire travel. " + "If the arm is holding a pose, SUPPORT IT — it will sag. No motion is " + "commanded." + ) + else: + emit_diagnostic( + f"⚠ This writes {joint}'s (motor {motor}) encoder offset to EEPROM: " + f"Ofs = {offset:+d} at addr 31. PERSISTENT — it changes every position the " + "servo reports, for good. Torque is disabled first and left off. No motion is " + "commanded." + ) + ans = _prompt("Type 'yes' to confirm") + if ans.strip().lower() == "yes": + return True + if json_mode: + emit_result({"aborted": True, "joint": joint}, json_mode=True) + else: + emit_result("Aborted; nothing written, no motion commanded.", json_mode=False) + return False + + +def _emit_rezero_write( + role: str, + port: str, + plan: "rezero.RezeroPlan", + read_back: int, + shift: "dict[str, object]", + *, + json_mode: bool, +) -> None: + """Emit the result of a successful offset write, and tell the operator what is left. + + Two things remain undone at this point, and neither is optional: + + * The write is proven **applied** (it read back) but not proven + **persistent** — PR #21 exists because id/baud writes read back correctly + and silently reverted on the next power-cycle. Only a power-cycle proves it. + * The offset is proven applied but the seam is not proven **moved**. Only + ``--verify`` proves that. + + So the result does not end on a success line; it ends on the next two steps. + """ + next_steps = [ + "1. POWER-CYCLE the servo — cut and restore BUS POWER (not just the USB/serial " + "link). An EEPROM write can read back correctly and still revert on the next " + "power-up if the Lock register was mishandled; that is PR #21, and it is the only " + "way to know.", + f"2. Re-read the offset: 'arm101 arm read --json' — {plan.joint}'s 'offset' must " + f"still be {plan.target_offset}. If it reverted to 0, the write did not persist.", + f"3. PROVE THE SEAM MOVED: 'arm101 arm rezero {plan.joint} --verify'. The read-back " + "above proves only that the offset was APPLIED. It does NOT prove the seam " + "RELOCATED — only a torque-off sweep of the joint's whole travel can.", + ] + + if json_mode: + emit_result( + { + "verb": _REZERO_VERB, + "role": role, + "port": port, + "plan": plan.as_dict(), + "read_back_offset": read_back, + "applied": read_back == plan.target_offset, + "shift": shift, + "persistence_proven": False, + "seam_eviction_proven": False, + "next_steps": next_steps, + }, + json_mode=True, + ) + return + + lines = [ + f"## arm rezero {plan.joint} ({role}) — encoder offset written on {port}", + "", + f"- motor : {plan.motor}", + f"- offset before : {plan.current_offset}", + f"- offset written : {plan.target_offset:+d} (wire value " + f"{encode_offset(plan.target_offset)}, EEPROM addr 31)", + f"- offset read back : {read_back} <- the write LANDED", + f"- raw position : {plan.raw_position} (unchanged — no motion was commanded)", + f"- reported before : {plan.reported_position}", + f"- reported after : {shift['observed_position']}" + f" (predicted {shift['predicted_position']}, delta {shift['delta']})", + "", + ] + + if not shift["in_range"]: + lines += [ + "*** WARNING — the servo now reports a position OUTSIDE [0, 4095]. That is a " + "value the position register cannot hold, which means the corrected position " + "is an UNWRAPPED signed subtraction: the seam has NOT moved and this re-zero " + "achieves nothing. Run --verify to confirm, then stop and re-decide. ***", + "", + ] + elif shift["unchanged"]: + lines += [ + "*** WARNING — the reported position did not change. The offset register took " + "the value (it read back), but the servo is not applying it to what it " + "reports. Run --verify to confirm, then stop and re-decide. ***", + "", + ] + elif not shift["as_predicted"]: + lines += [ + f"*** WARNING — the reported position moved, but not to the predicted " + f"{shift['predicted_position']} (delta {shift['delta']} ticks). The joint is " + "limp, so a few ticks of gravity/backlash are expected; this is more than " + "that. Run --verify before trusting the frame. ***", + "", + ] + + lines += ["### What is NOT yet proven", ""] + next_steps + emit_result("\n".join(lines), json_mode=False) + + +def _sweep_progress(json_mode: bool, every: int = 10) -> "Callable[[int, int], None]": + """Build the ``on_sample`` hook that shows the operator the joint moving. + + A human hand-moving a limp joint for 30 seconds with no feedback has no way + to tell "I am driving the joint and the tool is watching" apart from "the + tool wedged and I am wobbling a dead arm". Both look identical from where + they are standing, and the second one silently produces a useless sweep. + + Goes to **stderr** (:func:`~arm101.cli._output.emit_diagnostic`), like every + other diagnostic — stdout is reserved for the one result document, and under + ``--json`` that matters: a progress line interleaved into stdout would wedge + a partial JSON object between the reader and the report. Throttled to every + *every*-th sample (~2 lines/second at the default poll interval), because a + line per 50 ms poll is not feedback, it is a waterfall. + """ + + def announce(index: int, position: int) -> None: + if index % every: + return + if json_mode: + emit_diagnostic(json.dumps({"sample": index, "position": position})) + else: + emit_diagnostic(f" sample {index:>4} position {position:>6}") + + return announce + + +def _run_rezero_verify( + bus: object, + role: str, + port: str, + joint: str, + motor: int, + duration: float, + *, + json_mode: bool, +) -> None: + """Run the seam-eviction sweep and emit its report — raising on the STOP condition. + + The report goes to **stdout on every path**, including the failure path, and + is emitted BEFORE the :class:`CliError` is raised. That ordering is the + point: the numbers are why the operator ran the command, and they are exactly + as valuable when the answer is "the fix does not work" as when it is "the fix + works". Failing without showing them would leave a human standing at an arm + with a non-zero exit code and nothing to take back to the decision. + + Raises + ------ + CliError(EXIT_ENV_ERROR) + If the sweep found a discontinuity **while the seam-evicting offset was + in force** — i.e. the servo does not reduce the corrected position modulo + 4096, the seam never moved, and the whole re-zero approach to issue #35 + is dead. This is a stop-and-return-to-the-user condition, not a retryable + error, and it exits non-zero so that no script can mistake it for + success. + """ + emit_diagnostic( + f"Torque is now OFF on {joint} (motor {motor}) and will STAY off.\n" + f"Hand-move the joint SLOWLY through its ENTIRE travel — from one hard stop all " + f"the way to the other — for the next {duration:.0f} seconds. Do not hurry: " + f"hurrying is how a seam crossing hides between two samples." + ) + + report = rezero.sweep( + bus, # type: ignore[arg-type] + motor, + joint, + samples=rezero.samples_for(duration), + on_sample=_sweep_progress(json_mode), + ) + + if json_mode: + emit_result( + {"verb": _REZERO_VERB, "role": role, "port": port, "sweep": report.as_dict()}, + json_mode=True, + ) + else: + emit_result( + f"## arm rezero {joint} --verify ({role}) — seam-eviction sweep on {port}\n\n" + + report.describe(), + json_mode=False, + ) + + if report.failed: + raise CliError( + code=EXIT_ENV_ERROR, + message=( + f"SEAM NOT EVICTED: {joint} carries the seam-evicting offset " + f"({report.offset_in_force}) and its reported position still jumps " + f"{report.largest_jump} ticks mid-travel. The servo does NOT reduce the " + "corrected position modulo 4096 — the offset merely relabels positions and " + "the discontinuity stays pinned to the physical angle where the magnet " + "rolls over. The re-zero does not fix issue #35." + ), + remediation=( + "STOP — do not build on this. The one undocumented assumption behind the " + "re-zero (docs/spikes/sts3215-offset-register.md, section 4) has resolved " + "against us, and the fix needs a new approach, not a retry. Take the sweep " + "report above back to the user for a re-decision. The remaining options are " + "a software-only soft limit (as wrist_roll uses) or unwrapping the encoder " + "in software; neither is this verb's to choose." + ), + ) + + +def _run_rezero_write( + bus: object, + role: str, + port: str, + joint: str, + motor: int, + *, + json_mode: bool, +) -> None: + """Plan, write, and read back the encoder offset. Commands NO motion. + + ``plan_rezero`` looks READ-ONLY — ``read_offset`` then ``read_position``, + no torque write, no EEPROM — and on most servos that would mean it is also + overload-*proof*. It is not, here. ``FeetechBus._read_register`` raises + through ``_status_error`` whenever the returned status byte reports a + non-zero error, and ``_status_error`` hands back an ``OverloadError`` + specifically when the overload bit (0x20) is set — a property of the + STATUS BYTE that comes back with the reply, not of which register or + which direction (read vs. write) the packet asked for. A motor latched in + overload therefore fails a read exactly as it fails a write, and + ``plan_rezero`` would raise before this verb ever reached + ``apply_rezero`` — the only place that calls ``bus.clear_overload``. + + That is not a corner case worth shrugging off: ``elbow_flex``'s + unreachable arc (the whole reason this joint is re-zeroable) was measured + by driving the joint into a wall, which is precisely how a Feetech servo + latches an overload. An operator who has just finished that measurement — + exactly the order ``docs/hardware-rezero-procedure.md`` describes — would + hit this every single time, on the one joint the verb exists to fix. + + Raises + ------ + CliError(EXIT_ENV_ERROR) + If the offset read back from EEPROM is not the one written — the write + did not take, and every position the servo reports from here is in a + frame nobody chose. + """ + try: + plan = rezero.plan_rezero(bus, motor, joint) # type: ignore[arg-type] + except OverloadError: + # Recover exactly once, and ONLY here — inside the except, never ahead + # of the `try`. `clear_overload` is `enable_torque(motor, False)` + # under the hood: it de-energises the joint as its side effect of + # clearing the latch. Calling it unconditionally, before every plan, + # would silently drop torque on the common/no-op path too — including + # the case where `plan_rezero` finds the offset `already_applied` and + # nothing is ever written — de-energising a joint that was holding + # its pose just fine for no reason connected to anything that went + # wrong. The overload branch is the only place this verb has actual + # evidence the joint is latched, so it is the only place allowed to + # pay that de-energising cost. + emit_diagnostic( + f"{joint} (motor {motor}) was latched in an overload fault while reading " + "its live state for the re-zero plan. Clearing the latch now: torque is " + "OFF and the joint is LIMP as a direct result. This is the expected " + "recovery — not a malfunction — for a joint that was just driven into " + "its unreachable arc, which is how that arc was measured in the first " + "place; it is not a surprise this verb should spring on an operator who " + "did not ask for it." + ) + bus.clear_overload(motor) # type: ignore[attr-defined] + # One retry, no loop: if the servo is still latched after a torque + # release, the fault is not the transient kind `clear_overload` is + # documented to clear, and spinning on it would just hang against a + # servo that is never going to answer differently. + plan = rezero.plan_rezero(bus, motor, joint) # type: ignore[arg-type] + + if plan.already_applied: + _emit_rezero_noop(role, port, plan, json_mode=json_mode) + return + + read_back = rezero.apply_rezero(bus, motor, plan.target_offset) # type: ignore[arg-type] + if read_back != plan.target_offset: + raise CliError( + code=EXIT_ENV_ERROR, + message=( + f"The encoder offset did NOT take: wrote {plan.target_offset} to " + f"{joint} (motor {motor}, EEPROM addr 31), read back {read_back}." + ), + remediation=( + "The servo accepted the write but is not holding the value. Check the " + "EEPROM Lock register (addr 55) is not being re-closed by another process, " + "and that motor " + f"{motor} is the servo you think it is ('arm101 arm read'). The Lock dance " + "is exactly what PR #21 was written to fix." + ), + ) + + # Free, and already decisive in two of its outcomes: does the servo's OWN + # report move the way a modular correction would move it? + observed = bus.read_position(motor) # type: ignore[attr-defined] + shift = rezero.describe_shift(plan, observed) + _emit_rezero_write(role, port, plan, read_back, shift, json_mode=json_mode) + + +def _emit_rezero_noop( + role: str, + port: str, + plan: "rezero.RezeroPlan", + *, + json_mode: bool, +) -> None: + """Report that the servo already holds the target offset — and write nothing. + + Idempotence matters here more than it usually does: the procedure this verb + belongs to tells the operator to power-cycle the arm and come back, so a + second run against an already-re-zeroed joint is the *expected* path, not a + mistake. Re-writing the same value would be harmless on the wire and + corrosive in the log — it would make "the offset was written" ambiguous about + which run wrote it. + """ + if json_mode: + emit_result( + { + "verb": _REZERO_VERB, + "role": role, + "port": port, + "plan": plan.as_dict(), + "written": False, + "reason": "already-applied", + "seam_eviction_proven": False, + }, + json_mode=True, + ) + return + emit_result( + "\n".join( + [ + f"## arm rezero {plan.joint} ({role}) — already re-zeroed on {port}", + "", + f"- motor : {plan.motor}", + f"- offset in force : {plan.current_offset} (this joint's computed offset)", + f"- reported now : {plan.reported_position} (raw {plan.raw_position})", + "", + "Nothing written — the servo already holds this offset.", + "", + "That the offset is APPLIED does not mean the seam MOVED. If you have not " + f"proven it yet, run: arm101 arm rezero {plan.joint} --verify", + ] + ), + json_mode=False, + ) + + +def cmd_arm_rezero(args: argparse.Namespace) -> None: + """Shift a joint's encoder zero so the seam falls where the joint cannot reach. + + The gated EEPROM write for issue #35, plus (``--verify``) the sweep that + proves it actually worked. Commands **no motion on any path** — see + :mod:`arm101.hardware.rezero` for the bootstrap problem that forbids it. + """ + role: str = args.role + json_mode = bool(getattr(args, "json", False)) + joint: str = args.joint + verify = bool(getattr(args, "verify", False)) + raw_duration = getattr(args, "duration", None) + duration: float = rezero.DEFAULT_SWEEP_DURATION if raw_duration is None else float(raw_duration) + + # Eligibility FIRST — before consent, before a port, before a bus. "Why can't + # I re-zero wrist_roll?" is a question about the arm's geometry, and it is + # answerable (and answered, at length) with no hardware attached. + offset, arc = rezero.require_rezeroable(joint) + motor = arm_spec.joint_ids(role)[joint] + + # Validate the sweep length before prompting for consent, so a bad --duration + # is a user error caught up front rather than after the operator has already + # said yes. + if verify: + rezero.samples_for(duration) + + mode = resolve_consent(args, verb=_REZERO_VERB, require_plan_hash=False) + + # --- dry_run: plan only, zero writes, zero bus access --- + if mode == "dry_run": + _emit_rezero_plan( + role, + joint, + motor, + offset, + arc, + verify, + duration, + port=getattr(args, "port", None), + json_mode=json_mode, + ) + return + + # --- interactive: confirm at a prompt before any bus is opened --- + if mode == "interactive" and not _confirm_rezero( + joint, motor, offset, verify, json_mode=json_mode + ): + return + + port = _resolve_port(getattr(args, "port", None)) + bus = _open_bus(port) + try: + # The guard owns the one joint this verb touches. rezero never ENERGISES + # it — both paths de-energise and leave it limp — so the guard is here to + # catch the inverse hazard: a crash between the torque-off and the + # EEPROM re-lock, or a Ctrl-C mid-sweep on a joint an earlier verb left + # hot. Releasing an already-limp motor is a no-op, so this costs nothing + # on the paths where nothing went wrong. + with torque_guard(bus, (motor,), on_release=_release_announcer(json_mode)): + if verify: + _run_rezero_verify(bus, role, port, joint, motor, duration, json_mode=json_mode) + else: + _run_rezero_write(bus, role, port, joint, motor, json_mode=json_mode) + finally: + bus.close() + + # --------------------------------------------------------------------------- # arm setup # --------------------------------------------------------------------------- @@ -1815,6 +2455,62 @@ def register(sub: "argparse._SubParsersAction[argparse.ArgumentParser]") -> None pr.add_argument("--json", action="store_true", help=_JSON_HELP) pr.set_defaults(func=cmd_arm_profile, json=False) + # rezero — gated EEPROM write (and NOT a move); --verify is the seam-eviction proof + rz = noun_sub.add_parser( + "rezero", + help=( + "Shift a joint's encoder zero (EEPROM addr 31) so the 4095->0 seam falls in " + "the arc it cannot reach — the issue-#35 fix for elbow_flex; commands NO " + "motion. --verify proves the seam actually moved via a torque-off, " + "hand-driven sweep. Gated (use --apply in non-TTY agent mode)." + ), + ) + rz.add_argument( + "joint", + help=( + "Joint to re-zero. Only elbow_flex wraps inside its travel; every other " + "joint is refused with the reason (ask, and it will tell you)." + ), + ) + rz.add_argument( + "--verify", + action="store_true", + default=False, + help=( + "Do not write. De-energise the joint and poll its position while YOU " + "hand-move it through its whole travel, asserting there is no discontinuity " + "anywhere — the only proof the seam actually moved. Leaves the joint limp." + ), + ) + rz.add_argument( + "--duration", + type=float, + default=None, + help=( + "Seconds to sweep for with --verify " + f"(default {rezero.DEFAULT_SWEEP_DURATION:.0f}); ignored without it." + ), + ) + rz.add_argument( + "--role", + choices=arm_spec.roles(), + default="follower", + help=_ROLE_HELP, + ) + rz.add_argument( + "--port", + default=None, + help=_PORT_HELP, + ) + rz.add_argument( + "--apply", + action="store_true", + default=False, + help="Execute (non-TTY agent mode; ignored under a TTY).", + ) + rz.add_argument("--json", action="store_true", help=_JSON_HELP) + rz.set_defaults(func=cmd_arm_rezero, json=False) + # setup — gated action verb sp = noun_sub.add_parser( "setup", diff --git a/arm101/cli/_commands/learn.py b/arm101/cli/_commands/learn.py index dc4dcf2..b4c6ea6 100644 --- a/arm101/cli/_commands/learn.py +++ b/arm101/cli/_commands/learn.py @@ -58,6 +58,13 @@ FAILURE, not a pass. Reports the joint's safe speed, ticks/second, and motion-onset latency; gated motion (TTY prompt or agent via --apply). + arm101-cli arm rezero Shift a joint's encoder zero (Ofs/Homing_Offset, EEPROM + addr 31) so the 4095->0 encoder seam falls in the arc the + joint cannot reach — the issue-#35 fix, and elbow_flex is + the only joint it applies to (every other joint is refused + WITH the reason). Commands NO motion. --verify runs the + torque-off, hand-driven sweep that proves the seam actually + moved (gated; TTY prompt or agent via --apply). arm101-cli cli overview Describe the CLI surface itself. Hardware (SO-101 motor verbs) @@ -89,6 +96,20 @@ highest safe speed, its measured ticks/second, and its motion-onset latency; the ramp stops at the first speed that fails, and a --contact-to the joint can actually reach voids the run (exit 1) rather than certifying a speed against thin air. + +arm rezero is a gated EEPROM write that commands NO motion: it shifts a joint's +encoder zero (Ofs/Homing_Offset, addr 31) so the 4095->0 encoder seam falls in +the arc the joint physically cannot reach. Only elbow_flex wraps inside its +travel (issue #35) and only elbow_flex can be re-zeroed; wrist_roll is refused +because a re-zero relocates a seam and can never evict one from a joint that +turns all the way round (it has a soft limit instead), and the other four are +refused because they never wrap. It commands no motion on purpose: elbow_flex +rests PAST its wrap, so a linear goal would rotate it the long way round into a +wall — the tool that makes the axis linear cannot rely on the axis being linear. +--verify is the proof: torque off, a human hand-moves the joint through its whole +travel, and the verb asserts there is no discontinuity anywhere. Reading the +offset back proves only that it was APPLIED; only the sweep proves the seam +MOVED, and a discontinuity under a written offset exits 2 as a stop condition. calibrate is a profile-write (disk only) verb with a dry-run preview on non-TTY: TTY captures poses and saves, non-TTY without --apply emits a read-only preview (no bus, no write), non-TTY with --apply exits 1 (physical @@ -213,6 +234,17 @@ def _as_json_payload() -> dict[str, object]: "motion (TTY prompt or agent via --apply)." ), }, + { + "path": ["arm", "rezero"], + "summary": ( + "Shift a joint's encoder zero (Ofs/Homing_Offset, EEPROM addr 31) so " + "the 4095->0 encoder seam falls in the arc the joint cannot reach — " + "the issue-#35 fix; elbow_flex only, every other joint refused WITH " + "the reason. Commands NO motion. --verify runs the torque-off, " + "hand-driven sweep that proves the seam moved (gated; TTY prompt or " + "agent via --apply)." + ), + }, {"path": ["cli", "overview"], "summary": "Describe the CLI surface."}, ], "exit_codes": { @@ -234,6 +266,7 @@ def _as_json_payload() -> dict[str, object]: "arm flex", "arm explore", "arm profile", + "arm rezero", ], "sdk_extra": "pip install 'arm101-cli[seeed]'", "note": ( @@ -244,12 +277,12 @@ def _as_json_payload() -> dict[str, object]: "no write); non-TTY with --apply exits 1 (physical pose capture cannot " "be automated). set-motor-id (EEPROM id write), set-baudrate (EEPROM " "baud write, id unchanged), center-motor (motion), setup-motors, " - "arm setup, arm flex, arm explore and arm profile are gated, destructive, " - "and use the three-mode consent core: TTY interactive, non-TTY dry-run " - "plan, or non-TTY --apply (set-motor-id, set-baudrate, setup-motors, " - "arm setup, arm flex, arm explore and arm profile are 1-step; center-motor " - "is 2-step with --plan-hash). arm setup additionally auto-catalogs F/L " - "motor entries " + "arm setup, arm flex, arm explore, arm profile and arm rezero are gated, " + "destructive, and use the three-mode consent core: TTY interactive, " + "non-TTY dry-run plan, or non-TTY --apply (set-motor-id, set-baudrate, " + "setup-motors, arm setup, arm flex, arm explore, arm profile and arm " + "rezero are 1-step; center-motor is 2-step with --plan-hash). arm setup " + "additionally auto-catalogs F/L motor entries " "from arm_spec (servo_model + gear_ratio) after each write. arm read is " "the one read-only motor verb (no consent gate): it reads every joint's " "live state but commands no motion. arm flex moves one joint (--to) or " @@ -267,7 +300,16 @@ def _as_json_payload() -> dict[str, object]: "detection are COUPLED, so a speed the servo merely SURVIVES is a failure " "of that speed, not a pass, and free motion at a speed proves nothing; it " "reports the joint's highest safe speed, its measured ticks/second, and its " - "motion-onset latency. Headless writes are attributed " + "motion-onset latency. arm rezero is a gated EEPROM write that commands NO " + "motion: it shifts a joint's encoder zero (addr 31) so the 4095->0 seam " + "falls in the arc the joint cannot reach (issue #35). elbow_flex is the " + "only re-zeroable joint; wrist_roll is refused because a re-zero relocates " + "a seam and can never evict one from a joint that turns all the way round " + "(it has a soft limit instead), and the other four are refused because they " + "never wrap. --verify is the proof: torque off, a human hand-moves the " + "joint through its whole travel, and the verb asserts no discontinuity " + "anywhere — the read-back proves the offset was APPLIED, only the sweep " + "proves the seam MOVED. Headless writes are attributed " "(ARM101_OPERATOR / culture nick) and logged to ~/.arm101/audit.log." ), }, diff --git a/arm101/cli/_commands/overview.py b/arm101/cli/_commands/overview.py index 39e4d33..79711fe 100644 --- a/arm101/cli/_commands/overview.py +++ b/arm101/cli/_commands/overview.py @@ -49,6 +49,10 @@ " (TTY prompt or agent via --apply)", "arm explore — flood-fill + map the reachable joint-space via the overload-safe gentle" " move; writes JSONL log + compact map; gated motion (--apply)", + "arm rezero — shift a joint's encoder zero (EEPROM addr 31) so the 4095->0 seam" + " falls where the joint cannot reach (issue #35; elbow_flex only — others refused WITH" + " the reason); commands NO motion; --verify runs the torque-off hand-driven sweep that" + " proves the seam moved (gated; --apply)", "arm profile — find the highest speed at which CONTACT DETECTION still works:" " ramps the speed and certifies each candidate against a real contact (--contact-to);" " reports safe speed, ticks/second, motion-onset latency; gated motion (--apply)", diff --git a/arm101/explain/catalog.py b/arm101/explain/catalog.py index 1d09369..2d25e26 100644 --- a/arm101/explain/catalog.py +++ b/arm101/explain/catalog.py @@ -403,8 +403,9 @@ Noun group for arm-level operations on the SO-101 robotic arm. Provides a read-only surface snapshot (`arm overview`), a read-only live-state read (`arm read`), a gated motion verb (`arm flex`), a gated reachability-mapping -walk (`arm explore`), a gated speed-profiling ramp (`arm profile `), and -a gated setup walk (`arm setup `). +walk (`arm explore`), a gated speed-profiling ramp (`arm profile `), a +gated encoder re-zero (`arm rezero ` — an EEPROM write that commands no +motion), and a gated setup walk (`arm setup `). ## Verbs @@ -412,9 +413,9 @@ and the per-role id / baud / servo_model / gear_ratio map). Read-only; always exits 0. - `arm101-cli arm read` — read every joint's live register state - (position/load/speed/voltage/temperature/torque). Read-only on the bus — - no consent gate; a flaky joint is marked `partial`/`failed` while the rest - still read. + (position/load/speed/voltage/temperature/torque, plus the signed encoder + `offset`). Read-only on the bus — no consent gate; a flaky joint is marked + `partial`/`failed` while the rest still read. - `arm101-cli arm flex` — command a bounded, gentle joint move (`--to`) or a demo sweep (`--demo`). Gated motion: three-mode consent + `--apply`, with `--gentle`/`--threshold` selecting the load-watch back-off-then-hold path. @@ -428,6 +429,11 @@ A speed the servo merely survives is a failure, not a pass. Records the joint's safe speed, ticks/second, and motion-onset latency. Gated motion: three-mode consent + `--apply`. +- `arm101-cli arm rezero ` — shift the servo's encoder zero (EEPROM addr + 31) so the 4095->0 seam falls in the arc the joint cannot reach (issue #35; + only `elbow_flex` — every other joint is refused *with the reason*). + **Commands no motion.** `--verify` runs the torque-off, hand-driven sweep that + proves the seam actually moved. Gated: three-mode consent + `--apply`. - `arm101-cli arm setup ` — assign EEPROM ids 1–6 at 1 000 000 baud for all 6 motors of the given role and auto-catalog each motor's servo_model and gear_ratio from `arm_spec`. Gated; uses the three-mode consent walk. @@ -445,6 +451,8 @@ arm101-cli arm flex shoulder_pan --to 2048 --apply arm101-cli arm flex --demo --apply arm101-cli arm profile shoulder_pan --contact-to 3500 --apply + arm101-cli arm rezero elbow_flex --apply + arm101-cli arm rezero elbow_flex --verify --apply arm101-cli arm setup follower arm101-cli arm setup follower --apply """ @@ -455,9 +463,16 @@ Read every joint's live register state for an arm role and print it as a table (or `--json`). Read-only on the motor bus — it opens a bus and reads `present_position`, `present_load`, `present_speed`, `present_voltage`, -`present_temperature`, and `torque_enable` for each of the six joints, but -commands no motion and writes no register. Because nothing is mutated, there -is **no consent gate** — unlike `arm flex`/`arm setup`. +`present_temperature`, `torque_enable`, and `Homing_Offset` for each of the six +joints, but commands no motion and writes no register. Because nothing is +mutated, there is **no consent gate** — unlike `arm flex`/`arm setup`. + +The `offset` column / field is the servo's encoder offset (`Ofs` / +`Homing_Offset`, EEPROM address 31), shown **signed** — the bus decodes the +register's sign-magnitude wire form, so an offset of `-1073` reads as `-1073` +and not as the raw `3121`. It is surfaced because issue #35 fixes `elbow_flex`'s +mid-travel encoder wrap by re-zeroing that register, and inspecting the current +re-zero must never require performing one. `0` on a factory servo. Retry-tolerant: each joint is read with bounded retries (`arm101.hardware.arm_read.read_arm`). A joint whose first read succeeds is @@ -836,6 +851,134 @@ `servo_model`, and `gear_ratio`. """ +_ARM_REZERO = """\ +# arm101-cli arm rezero + +Shift a joint's **encoder zero** — the servo's `Ofs` / `Homing_Offset` register +(EEPROM addr 31) — so that the encoder's 4095->0 **seam** falls inside the arc +the joint physically cannot reach. **Commands no motion, on any path.** + +## The bug this fixes (issue #35) + +`elbow_flex`'s 12-bit encoder wraps *inside its own physical travel*. Driven far +enough it crosses the raw 4095->0 seam and reads back near zero, so its reported +position is **not monotonic with joint angle**: its two measured endpoints sort +into a `[min, max]` pair describing exactly the arc it CANNOT reach, and every +position comparison in this codebase — `gentle_move`'s arrival check, +`clamp_goal`, the reachability map's ranges — is silently wrong for it. It +currently rests at raw ~126, i.e. *past* its wrap. + +Move the seam into the joint's unreachable arc and every tick it can actually +reach lies on one side of it. The tick axis is linear again — genuinely, not by +assumption. + +## Why it commands no motion — the bootstrap problem + +The tool that MAKES the axis linear cannot itself rely on the axis being linear. +"Drive the joint to mid-travel, then centre it" is the natural procedure and it +is exactly the one that must not run: from its rest position at raw ~126, a +linear goal of 3121 (its mid-travel) looks like a modest move and is in fact a +rotation *the long way round* — down through 0, across the whole 1894-tick arc +the joint cannot reach, and into a wall. So this verb reads where the joint +physically **is**, computes the offset from the joint's known unreachable arc (a +measured table fact, in `arm_spec.REZERO_ARCS`), and writes it. No goal position +is ever written. + +Torque is disabled before the EEPROM write and left off: a servo must not be +*holding* while its own frame of reference changes underneath it. + +## Which joints + +Only `elbow_flex`. Every other joint is refused **with the reason** — and there +are two different reasons, which the verb keeps apart: + +- **`wrist_roll` — impossible.** A re-zero only *relocates* a seam; it can never + *evict* one. Eviction needs an arc the joint cannot reach, and exploration + found no wall anywhere in `wrist_roll`'s travel (measured free range + `[21, 4073]`) — it turns freely all the way round, so every angle is reachable, + including whichever one the seam is moved to. It is handled instead by a + software **soft limit** (`arm_spec.SOFT_LIMITS`), already in force. +- **The other four — unnecessary.** Their encoders do not wrap inside their + travel, so there is no seam in the way and nothing to evict. + +## `--verify` — the seam-eviction proof + +**Reading the offset back only proves it was APPLIED. It does not prove the seam +MOVED.** One undocumented bit of firmware semantics decides which: + + Present = (raw - Ofs) mod 4096 seam RELOCATES -> the fix works + Present = raw - Ofs (signed) seam STAYS -> the fix does NOTHING + +Every source (and LeRobot's shipped SO-101 calibration) implies the first; no +primary Feetech source states it. `--verify` settles it: torque goes **off** and +stays off, a **human hand-moves** the joint through its entire travel, and the +verb polls `present_position` and asserts there is **no discontinuity anywhere**. +A human arm is the right instrument precisely because it is the only actuator +available that does not need a linear tick axis to work. + +It reports the range reached, whether the sweep was monotonic, and the largest +single-sample jump — a seam crossing is ~1949-4095 ticks; sensor noise and a +human hand are tens. It also measures `elbow_flex`'s **far wall** for the first +time (nothing could see across the seam before). + +Four verdicts, because "did not fail" is not the same claim as "proved it works": + +- `seam-evicted` — re-zeroed, continuous, and the sweep actually covered the + travel. The fix works. +- `seam-not-evicted` — re-zeroed and **still** discontinuous. **STOP.** The + re-zero achieves nothing; exit code 2, and the decision goes back to the user. +- `seam-present-baseline` — not re-zeroed, discontinuous. The bug, photographed. + Expected before the write; not a failure. +- `inconclusive` — continuous, but either no offset was in force or the joint was + not moved through enough of its travel for "no seam" to mean anything. + +`--verify` deliberately ends with the joint **limp** — the operator's hand is on +it. If the arm is holding a pose it will sag: support it. + +## Consent modes + +Same three-mode gate as `arm flex` / `arm explore` (1-step tier): + +1. **TTY (interactive)** — confirm at a prompt. +2. **Non-TTY without `--apply`** — dry-run: prints the exact register writes and + opens **no bus at all**. +3. **Non-TTY with `--apply`** — executes. + +## Usage + + arm101-cli arm rezero elbow_flex # dry-run: the exact writes + arm101-cli arm rezero elbow_flex --apply # write the offset + arm101-cli arm rezero elbow_flex --verify --apply # prove the seam moved + arm101-cli arm rezero elbow_flex --verify --duration 45 --apply + arm101-cli arm rezero wrist_roll # refused, with the reason + arm101-cli arm rezero elbow_flex --json + +## After the write — what is NOT yet proven + +The read-back proves the offset was **applied**, not that it **persists**: PR #21 +exists because id/baud EEPROM writes read back correctly and silently reverted on +the next power-cycle. **Power-cycle the servo** (cut and restore bus power, not +just the serial link), re-read with `arm read`, then run `--verify`. The full +hand-run procedure is in `docs/hardware-rezero-procedure.md`. + +## Exit codes + +- `0` success, clean abort, a non-TTY dry-run plan, or an informative sweep + (baseline / inconclusive). +- `1` user/usage error (an unknown joint, a joint that cannot be re-zeroed, a + `--duration` too short to collect two samples). +- `2` environment error (no port, SDK absent, comms failure), the offset failing + to read back, the servo holding an unrecognised offset, the joint reporting a + raw position inside its own unreachable arc — **and the `seam-not-evicted` + verdict**, which is a stop condition, not a retryable error. + +## Hardware / TTY behavior + +Requires a real motor bus and the Feetech SDK (the `[seeed]` extra). The result +and the sweep report go to stdout; the prompt, the live sample feed, and every +warning go to stderr. +""" + _ARM_SETUP = """\ # arm101-cli arm setup @@ -900,5 +1043,6 @@ ("arm", "flex"): _ARM_FLEX, ("arm", "explore"): _ARM_EXPLORE, ("arm", "profile"): _ARM_PROFILE, + ("arm", "rezero"): _ARM_REZERO, ("arm", "setup"): _ARM_SETUP, } diff --git a/arm101/hardware/arm_read.py b/arm101/hardware/arm_read.py index e2b0796..9e6e843 100644 --- a/arm101/hardware/arm_read.py +++ b/arm101/hardware/arm_read.py @@ -34,6 +34,7 @@ "voltage": "present_voltage", "temperature": "present_temperature", "torque": "torque_enable", + "offset": "homing_offset", } @@ -67,6 +68,18 @@ class JointReading: ``present_position``, ``present_load``, ``present_speed``, ``present_voltage``, ``present_temperature``, and ``torque_enable`` keys respectively. ``None`` when ``health == "failed"``. + offset: + The joint's encoder offset (``Ofs`` / ``Homing_Offset``, EEPROM addr + 31), **signed** — the bus decodes it from its sign-magnitude wire form, + so a servo holding ``-1073`` reads as ``-1073``, not as the raw ``3121`` + (which would look like a plausible *positive* offset). + + Surfaced read-only, for issue #35: ``elbow_flex``'s encoder wraps inside + its physical travel, and the fix is to re-zero it by writing this + register. Before anyone writes it, a human has to be able to SEE what it + currently holds. ``0`` on a factory servo; ``None`` when + ``health == "failed"`` — a hard ``0`` there would assert "this motor has + no offset", a claim a failed read has no evidence for. """ joint: str @@ -79,6 +92,7 @@ class JointReading: voltage: "int | None" = None temperature: "int | None" = None torque: "int | None" = None + offset: "int | None" = None def _read_joint_with_retries( diff --git a/arm101/hardware/arm_spec.py b/arm101/hardware/arm_spec.py index 4ceaed5..c9b68c6 100644 --- a/arm101/hardware/arm_spec.py +++ b/arm101/hardware/arm_spec.py @@ -24,6 +24,44 @@ differ per role (and per joint for the leader). Downstream modules (calibrate, setup_motors) should import from here instead of duplicating the joint→id map. + +``wrist_roll``'s soft limit — why a joint can need a SOFTWARE-only dead arc +---------------------------------------------------------------------------- +The STS3215 encoder is 12-bit: raw position ticks run ``[0, 4095]`` and then +**wrap** — commanding the servo past 4095 rolls it over to read back near 0, +and vice versa. Every piece of code that reasons about a joint's position as a +linear tick axis (``gentle_move``'s arrival check, ``arm explore``'s grid and +reachability map, a plain ``[min, max]`` range) silently assumes that axis +never crosses that wrap point — call it the **seam**. + +``docs/hardware-validation-arm-read-flex.md`` (t9, 2026-07-01) caught this +live: ``wrist_roll``, parked at raw position **4** (i.e. sitting almost +exactly *on* the seam) after an earlier mapping run, was commanded 300 ticks +to 304 — a linear-controller-trivial move — and instead ran its full 7.0 s +timeout, converging nowhere, because the straight-line path from 4 to 304 +never has to cross the seam but the servo's *actual* physical position that +session did, so the arrival check kept comparing ticks across a discontinuity +that isn't really there in angle-space. A later re-run *clear of* the seam +(3049 → 2749) converged in 2078 ms, exactly as expected. The seam, not the +joint, was the fault. + +``elbow_flex`` has real mechanical walls (a measured ~2020-tick span), so it +is fixed by an encoder **re-zero**: relocate the seam into the arc the joint +physically cannot reach, and every reachable tick is then on one side of it — +linear again. ``wrist_roll`` cannot take that path. Exploration found **no +wall anywhere** in its travel — measured free range ``[21, 4073]`` — meaning +it rotates freely all the way round. A re-zero only *relocates* the seam; +it can never *evict* it, because eviction requires an arc the joint cannot +reach, and a joint whose travel covers the whole circle has no such arc by +definition — every angle, including wherever the seam is moved to, is inside +its travel. So ``wrist_roll`` instead gets a **soft limit**: a SOFTWARE-only +restriction (never an EEPROM ``min_angle``/``max_angle`` write — see +:class:`SoftLimit` and :func:`dead_arc_contains_seam`) that shrinks its +*permitted* travel to strictly less than a full turn, carving out a **dead +arc** — ticks the joint is simply never commanded into — and placing the seam +inside that dead arc rather than inside the permitted range. A soft limit +whose dead arc does *not* contain the seam buys nothing: the permitted range +would still cross the wrap, and the exact failure above would still happen. """ from __future__ import annotations @@ -379,6 +417,609 @@ def role_motors(role: str) -> dict[str, MotorSpec]: return dict(ARM_SPEC[role]) +# --------------------------------------------------------------------------- +# Encoder wrap — software-only soft limits +# --------------------------------------------------------------------------- + +#: Encoder tick bounds shared by every SO-101 joint (STS3215, 12-bit encoder). +#: Every raw ``present_position``/``goal_position`` read or write is in this +#: range; the **seam** — the point that motivates this whole section — is the +#: wrap between :data:`TICK_MAX` and :data:`TICK_MIN`: driving a joint past +#: 4095 rolls it over to read back near 0, and driving it below 0 rolls it +#: over to read back near 4095. +TICK_MIN: int = 0 +TICK_MAX: int = 4095 + + +@dataclass(frozen=True) +class SoftLimit: + """A SOFTWARE-only travel restriction, expressed as a plain tick interval. + + This is deliberately the *only* shape a soft limit is ever expressed in: + a ``min_tick < max_tick`` interval within ``[TICK_MIN, TICK_MAX]`` — never + a "min greater than max means wrap around" encoding. That second, more + "clever" representation is exactly what this module exists to avoid: it + would let a range *cross* the seam by construction, silently reintroducing + the bug documented in the module docstring. Because every ``SoftLimit`` is + a plain interval, the excluded region — the **dead arc** — is always + ``[TICK_MIN, min_tick) ∪ (max_tick, TICK_MAX]``: the arc that runs + *through* the seam. See :func:`dead_arc_contains_seam`, which checks that + this dead arc is non-empty rather than assuming it. + + Attributes + ---------- + min_tick, max_tick : int + The permitted (non-dead) range, inclusive on both ends. + + Raises + ------ + ValueError + If the pair is not a well-ordered interval within + ``[TICK_MIN, TICK_MAX]`` (i.e. NOT + ``TICK_MIN <= min_tick < max_tick <= TICK_MAX``). + """ + + min_tick: int + max_tick: int + + def __post_init__(self) -> None: + if not (TICK_MIN <= self.min_tick < self.max_tick <= TICK_MAX): + raise ValueError( + f"Invalid soft limit ({self.min_tick}, {self.max_tick}): requires " + f"{TICK_MIN} <= min_tick < max_tick <= {TICK_MAX}." + ) + + @property + def dead_arc_ticks(self) -> int: + """Combined width, in ticks, of the excluded arc on both sides of the seam. + + ``(min_tick - TICK_MIN) + (TICK_MAX - max_tick)``. This is the + concrete number an operator is trading away for linearity — the exact + width is a tunable (see :data:`SOFT_LIMITS`'s docstring), but this + property makes "how much" legible rather than buried in two subtractions. + """ + return (self.min_tick - TICK_MIN) + (TICK_MAX - self.max_tick) + + def permits(self, tick: int) -> bool: + """``True`` iff *tick* falls inside the permitted range, inclusive.""" + return self.min_tick <= tick <= self.max_tick + + +def dead_arc_contains_seam(min_tick: int, max_tick: int) -> bool: + """Return ``True`` iff excluding ``[min_tick, max_tick]`` leaves a dead arc containing the seam. + + This is the enforceable version of the argument in the module docstring, + made concrete rather than left as a comment someone can outrun with an + edit. For any well-ordered interval ``TICK_MIN <= min_tick < max_tick <= + TICK_MAX``, the excluded region is exactly + ``[TICK_MIN, min_tick) ∪ (max_tick, TICK_MAX]`` — the arc that runs + through the ``TICK_MAX -> TICK_MIN`` wrap, i.e. through the seam. That + region is non-empty, and therefore genuinely contains the seam, iff + ``min_tick > TICK_MIN or max_tick < TICK_MAX``. The one case where it is + EMPTY is ``(min_tick, max_tick) == (TICK_MIN, TICK_MAX)``: the full turn, + nothing excluded, seam still fully reachable. That degenerate case is + exactly "a soft limit that still spans the seam" — it buys nothing, and + this function is what makes that failure mode fail a test instead of + quietly reintroducing the bug the next time someone "simplifies" a range. + + Parameters + ---------- + min_tick, max_tick : int + A candidate permitted range, in the plain-interval form + :class:`SoftLimit` always uses. + + Returns + ------- + bool + ``True`` if the dead arc is non-empty (contains the seam), ``False`` + only for the degenerate full-range case. + + Raises + ------ + ValueError + If ``min_tick``/``max_tick`` do not form a well-ordered interval + within ``[TICK_MIN, TICK_MAX]``. A malformed interval isn't a range + this predicate can classify one way or the other. + """ + if not (TICK_MIN <= min_tick < max_tick <= TICK_MAX): + raise ValueError( + f"({min_tick}, {max_tick}) is not a valid tick interval: requires " + f"{TICK_MIN} <= min_tick < max_tick <= {TICK_MAX}." + ) + return min_tick > TICK_MIN or max_tick < TICK_MAX + + +def _require_dead_arc_contains_seam(table: Mapping[str, SoftLimit]) -> None: + """Raise ``ValueError`` if any entry in *table* does not satisfy :func:`dead_arc_contains_seam`. + + Called once, below, against :data:`SOFT_LIMITS` at import time — so a + future edit that widens a joint's range back out to the full turn (even a + "harmless-looking" one-line change) fails LOUDLY the moment the module is + imported, for every caller and every test, rather than silently shipping + a soft limit that no longer excludes the seam. This is the difference + between the guarantee being enforced and the guarantee being merely + documented. + """ + for joint, limit in table.items(): + if not dead_arc_contains_seam(limit.min_tick, limit.max_tick): + raise ValueError( + f"Soft limit for {joint!r} is ({limit.min_tick}, {limit.max_tick}), " + f"which spans the full {TICK_MIN}-{TICK_MAX} encoder range — its dead " + "arc is empty, so it does not contain the encoder seam and the limit " + "buys nothing. Narrow the range so the dead arc contains the seam." + ) + + +#: Per-joint software travel restrictions for joints whose encoder wraps +#: within (or across the whole of) their physical travel. A joint absent from +#: this table has no soft limit — its full ``[TICK_MIN, TICK_MAX]`` range is +#: permitted, which is correct for every joint except the two wrapping ones +#: (see the module docstring). ``elbow_flex`` is the other wrapping joint, but +#: it takes the encoder RE-ZERO path instead (a different task/module): it +#: has real mechanical walls, so its seam can be relocated into an arc it +#: physically cannot reach, and it therefore needs no entry here. +#: +#: ``wrist_roll`` — the only entry — has none: exploration drove it across +#: its entire ``[21, 4073]``-tick measured free range with **no wall found +#: anywhere** (``docs/hardware-validation-arm-read-flex.md``), i.e. it +#: rotates freely all the way round, so re-zero cannot fix it even in +#: principle (see the module docstring's impossibility argument). The chosen +#: range, ``(100, 3995)``: +#: +#: * Splits a 200-tick dead arc evenly across the seam — 100 ticks +#: (``[0, 100)``) below :data:`TICK_MIN`'s side, 100 ticks +#: (``(3995, 4095]``) below :data:`TICK_MAX`'s side. 100 ticks is +#: comfortably larger than both encoder read jitter (a handful of ticks) +#: and ``gentle_move``'s default arrival tolerance of 12 ticks +#: (``arm101.hardware.gentle._DEFAULT_ARRIVAL_TOLERANCE``), so an arrival +#: check settling near either edge cannot land in the dead arc by accident. +#: * Lands with margin *inside* the empirically-confirmed free envelope +#: ``[21, 4073]`` — 79 ticks of margin above 21, 78 below 4073 — so the +#: permitted range never asks the servo to go anywhere exploration did not +#: already drive it without incident. +#: * Leaves 3895 of 4095 ticks (~95%) usable — the exclusion is real (this is +#: not "spans the seam" in disguise: see :func:`dead_arc_contains_seam`) but +#: deliberately small, since nothing measured suggests the joint needs more +#: room than that. +#: +#: The exact width is a tunable an operator may revisit with more hardware +#: data (e.g. if arrival tolerance or observed jitter ever changes); what must +#: never change is that the dead arc contains the seam — enforced immediately +#: below, not merely asserted here. +SOFT_LIMITS: dict[str, SoftLimit] = { + "wrist_roll": SoftLimit(min_tick=100, max_tick=3995), +} + +_require_dead_arc_contains_seam(SOFT_LIMITS) + + +# --------------------------------------------------------------------------- +# Encoder re-zero — EVICTING the seam from a joint's travel (issue #35) +# --------------------------------------------------------------------------- + +#: One full turn of the STS3215's 12-bit magnetic encoder, in ticks (4096). +#: Derived from :data:`TICK_MIN`/:data:`TICK_MAX` rather than typed, so the two +#: cannot drift apart. It is the modulus the corrected position is reduced by — +#: the arithmetic that makes an offset *relocate* the seam instead of merely +#: relabelling positions. +#: +#: Deliberately re-stated here rather than imported from +#: :mod:`arm101.hardware.bus` (``ENCODER_RESOLUTION``): this module imports no +#: bus, by design and by test (``test_arm_spec_module_never_imports_the_bus``), +#: because a table of physical facts must not depend on a serial port. The two +#: constants are pinned equal by a cross-module test instead. +ENCODER_TICKS: int = TICK_MAX - TICK_MIN + 1 + +#: Widest magnitude the servo's offset register (``Ofs``/``Homing_Offset``, +#: EEPROM addr 31) can hold: it is SIGN-MAGNITUDE on bit 11, so the magnitude +#: field is 11 bits and the representable range is ``[-2047, +2047]``. +#: (LeRobot ``encode_sign_magnitude``: ``max_magnitude = (1 << 11) - 1``; +#: confirmed the hard way on a real SO-101 — LeRobot issue #3193 raised +#: ``ValueError: Magnitude 2073 exceeds 2047``.) +#: +#: Modulo 4096 that covers **every** seam placement except exactly one: raw +#: 2048. (``-2047`` is congruent to ``2049``, so residues ``0..2047`` and +#: ``2049..4095`` are all reachable; neither ``+2048`` nor ``-2048`` is +#: representable.) See :func:`_offset_for_seam_at`. +#: +#: Mirrors :data:`arm101.hardware.bus.OFFSET_MAX_MAGNITUDE` — same reason as +#: :data:`ENCODER_TICKS`: same fact, stated in the module that must not import +#: the other, and pinned equal by a cross-module test. +MAX_ENCODER_OFFSET: int = 2047 + + +@dataclass(frozen=True) +class UnreachableArc: + """The contiguous arc of raw encoder ticks a joint physically CANNOT reach. + + The mirror image of a :class:`SoftLimit`, and the thing that makes an + encoder re-zero possible at all. A joint with real mechanical walls cannot + rotate all the way round, so some arc of the encoder's 4096 ticks is + permanently out of reach — and *that* is where the seam belongs. Move the + seam into an arc the joint can never visit and it can never be crossed; + every tick the joint can actually reach then lies on one side of it, and + the tick axis is linear again. A joint with no such arc (``wrist_roll``, + which turns freely through the whole circle) cannot be helped this way at + all — there is nowhere to put the seam. See :func:`rezero_refusal`. + + Expressed as the OPEN interval ``(low, high)`` in **raw** encoder ticks: + both endpoints are positions the joint *can* reach (they are its hard + walls, or the last positions measured before them), and everything strictly + between them is unreachable. Raw, not corrected — this arc describes the + magnet on the shaft, and it is the frame the offset is computed *in*, before + any offset exists. + + Attributes + ---------- + low, high : int + The reachable ticks bounding the unreachable arc, ``low < high``. + + Raises + ------ + ValueError + If ``(low, high)`` is not a well-ordered interval within + ``[TICK_MIN, TICK_MAX]``. + """ + + low: int + high: int + + def __post_init__(self) -> None: + if not (TICK_MIN <= self.low < self.high <= TICK_MAX): + raise ValueError( + f"Invalid unreachable arc ({self.low}, {self.high}): requires " + f"{TICK_MIN} <= low < high <= {TICK_MAX}." + ) + + @property + def width(self) -> int: + """Width of the unreachable arc in ticks (``high - low``).""" + return self.high - self.low + + @property + def travel_ticks(self) -> int: + """The joint's physical travel: everything the arc does NOT exclude. + + ``ENCODER_TICKS - width``. For ``elbow_flex`` this is 2202 ticks — a + **lower bound**, because its far wall has never been measured (``arm + explore`` cannot see across the seam, which is the whole problem). The + ``--verify`` sweep measures it for the first time. + """ + return ENCODER_TICKS - self.width + + @property + def midpoint(self) -> int: + """The tick dead-centre of the arc — where the seam gets the most clearance. + + Any tick strictly inside the arc would work; the midpoint maximises the + margin on both sides, so a slightly-wrong arc boundary (and + ``elbow_flex``'s far wall IS only a bound, not a measurement) still + leaves the seam safely out of reach. + """ + return (self.low + self.high) // 2 + + def contains(self, tick: int) -> bool: + """``True`` iff *tick* is strictly inside the arc — i.e. unreachable. + + Strict, because the endpoints are exactly the ticks the joint CAN + reach. A joint reporting a raw position for which this is ``True`` is + reporting a position it should be physically incapable of holding — the + arc is wrong, or the servo is not the one we think it is. + """ + return self.low < tick < self.high + + +def _offset_for_seam_at(tick: int) -> int: + """Return the SIGNED offset ``H`` that places the encoder seam at raw *tick*. + + With ``Present = (raw - H) mod 4096``, the reported value rolls 4095->0 + exactly where ``raw == H``, so the offset simply *is* the seam's raw tick — + but expressed in the signed form the register can hold. Ticks above + :data:`MAX_ENCODER_OFFSET` are unrepresentable as positive magnitudes and + are re-expressed as their negative congruent (``tick - 4096``): raw 3000 + becomes ``H = -1096``, which is the same residue and fits comfortably. + + Raw 2048 is the single seam placement the encoding cannot express at all + (``+2048`` overflows the 11-bit magnitude and ``-2048`` does too). It is not + silently rounded — :func:`_require_evictable_seam` rejects a table entry + whose midpoint lands there, loudly, at import time. + """ + return tick if tick <= MAX_ENCODER_OFFSET else tick - ENCODER_TICKS + + +#: Per-joint unreachable arcs — the joints an encoder re-zero can actually fix. +#: +#: ``elbow_flex`` is the only entry, and the only joint that needs one. Its +#: encoder WRAPS inside its physical travel (issue #35): driven far enough it +#: crosses the raw 4095->0 seam and reads back near zero, so its reported +#: position is **not monotonic with joint angle** and every position comparison +#: in this codebase — ``gentle_move``'s arrival check, ``clamp_goal``, the +#: reachability map's ``(min, max)`` ranges — is silently wrong for it. Worse, +#: sorting its two measured endpoints into a ``[min, max]`` pair yields exactly +#: the arc it CANNOT reach. +#: +#: The arc below is measured, not assumed (``docs/spikes/sts3215-offset-register.md`` +#: §3, from ``arm-explore-follower.map.json`` and the t9 run-log): +#: +#: * Hard wall driving *decreasing*: raw **2020**. +#: * Driven *increasing* it crossed the seam and read back ~1; it now rests at +#: raw **~126**, i.e. PAST its wrap. +#: * So its travel runs ``2020 -> 4095 -> |seam| -> 0 -> 126``: 2202 ticks, +#: and the arc ``(126, 2020)`` — 1894 ticks — is unreachable. +#: +#: The far wall has never been measured (nothing could see across the seam), so +#: 2202 is a LOWER bound on travel and 1894 an UPPER bound on the arc. That does +#: not threaten the fix — any travel under a full turn leaves somewhere to put +#: the seam, and 2202 is nowhere near 4096 — but the arc should be re-derived, +#: and the offset with it, once ``arm rezero elbow_flex --verify`` finally +#: measures the far wall. +#: +#: Midpoint 1073, which is where the seam lands: 947 ticks of clearance on each +#: side. Cross-check: LeRobot's own rule (park mid-travel, write ``H = pos - +#: 2047``) gives ``3121 - 2047 = 1074`` — one tick away, by a completely +#: different route. +REZERO_ARCS: dict[str, UnreachableArc] = { + "elbow_flex": UnreachableArc(low=126, high=2020), +} + + +def _require_evictable_seam(table: Mapping[str, UnreachableArc]) -> None: + """Raise ``ValueError`` if any arc in *table* cannot actually take the seam. + + Two ways a table entry can be nonsense, both caught at import time — for + every caller and every test — rather than discovered halfway through an + EEPROM write on a physical servo: + + 1. **The offset is unrepresentable.** The register holds ``[-2047, +2047]`` + (:data:`MAX_ENCODER_OFFSET`); the one seam placement it cannot express is + raw 2048. An arc whose midpoint lands there needs a human, not a rounding + rule. + 2. **The arc does not contain its own seam.** Vacuously true for the + midpoint of a well-ordered open interval — *unless* the arc is only one + tick wide (``high == low + 1``), in which case there is no tick strictly + inside it and the "seam goes here" claim is empty. A one-tick arc means a + joint whose travel is 4095 of 4096 ticks: essentially ``wrist_roll``, and + a re-zero is the wrong tool (see :func:`rezero_refusal`). + + This mirrors :func:`_require_dead_arc_contains_seam` for :data:`SOFT_LIMITS`, + and for the same reason: the guarantee is *enforced*, not merely documented, + so a future "harmless" edit to the table fails loudly instead of quietly + writing a useless offset into a servo's EEPROM. + """ + for joint, arc in table.items(): + seam = arc.midpoint + if not arc.contains(seam): + raise ValueError( + f"Unreachable arc for {joint!r} is ({arc.low}, {arc.high}), which has no " + "tick strictly inside it — there is nowhere to evict the seam TO. A joint " + "whose travel covers all but a sliver of the circle needs a soft limit, " + "not a re-zero." + ) + offset = _offset_for_seam_at(seam) + if abs(offset) > MAX_ENCODER_OFFSET: + raise ValueError( + f"Unreachable arc for {joint!r} is ({arc.low}, {arc.high}), whose midpoint " + f"{seam} needs an offset of {offset} — outside the register's " + f"[-{MAX_ENCODER_OFFSET}, +{MAX_ENCODER_OFFSET}] range. Raw 2048 is the one " + "seam placement the sign-magnitude encoding cannot express; pick another " + "tick inside the arc." + ) + + +_require_evictable_seam(REZERO_ARCS) + + +#: Why a joint that CANNOT be re-zeroed cannot be re-zeroed — keyed by joint. +#: +#: Only ``wrist_roll`` is here, and its reason is a genuine impossibility rather +#: than an omission, which is exactly why it is spelled out instead of being +#: left to a shrug. Every other ineligible joint gets +#: :data:`_REZERO_UNNECESSARY` instead: a completely different answer ("you +#: don't need one") that must never be confused with this one ("you can't have +#: one"). +_REZERO_IMPOSSIBLE: dict[str, str] = { + "wrist_roll": ( + "wrist_roll cannot be re-zeroed: a re-zero only RELOCATES the encoder seam, it " + "can never EVICT it. Eviction needs an arc the joint physically cannot reach, and " + "exploration found no wall anywhere in wrist_roll's travel (measured free range " + "[21, 4073]) — it turns freely all the way round, so every angle is reachable, " + "including whichever one the seam is moved to. Its unreachable arc is empty by " + "definition. wrist_roll is handled instead by a SOFT LIMIT (arm_spec.SOFT_LIMITS): " + "a software-only travel restriction that carves out a dead arc the joint is simply " + "never commanded into, and puts the seam in there. That is already in force." + ), +} + +#: The other, ordinary reason a joint is not offered a re-zero: it never needed +#: one. Four of the six joints do not wrap inside their travel at all, so there +#: is no seam in the way and nothing to evict. +_REZERO_UNNECESSARY = ( + "{joint} does not need a re-zero: its encoder does not wrap inside its travel, so its " + "reported position is already monotonic with joint angle and there is no seam to evict. " + "Only elbow_flex wraps mid-travel (issue #35). Re-zeroing a joint that does not need it " + "would shift its frame of reference for no benefit and invalidate every position " + "previously recorded for it." +) + + +def rezero_arc(joint: str) -> Optional[UnreachableArc]: + """Return *joint*'s :class:`UnreachableArc`, or ``None`` if it has none. + + ``None`` is the common answer (five of six joints) and means "this joint has + no measured unreachable arc **in this table**" — either because it does not + wrap and needs no re-zero, or because it wraps but cannot be re-zeroed at + all. :func:`rezero_refusal` is what tells those two apart. + + Raises + ------ + ValueError + If *joint* is not one of :data:`JOINTS`. + """ + if joint not in JOINTS: + raise ValueError(f"Unknown joint {joint!r}. Valid joints: {list(JOINTS)}.") + return REZERO_ARCS.get(joint) + + +def rezero_offset(joint: str) -> Optional[int]: + """Return the signed encoder offset that evicts *joint*'s seam, or ``None``. + + ``None`` means the joint is not re-zeroable — call :func:`rezero_refusal` + for the reason, which is never "no reason". + + The offset is DERIVED from :data:`REZERO_ARCS`, never typed: it is the + signed form (:func:`_offset_for_seam_at`) of the arc's midpoint. So + correcting the arc — which will happen the first time ``--verify`` measures + ``elbow_flex``'s far wall — automatically corrects the offset, and the two + cannot drift apart. For ``elbow_flex`` today: arc ``(126, 2020)``, midpoint + 1073, offset **+1073**. + + Raises + ------ + ValueError + If *joint* is not one of :data:`JOINTS`. + """ + arc = rezero_arc(joint) # validates *joint* + if arc is None: + return None + return _offset_for_seam_at(arc.midpoint) + + +def rezero_refusal(joint: str) -> Optional[str]: + """Return WHY *joint* cannot be re-zeroed, or ``None`` if it can be. + + A refusal is an explanation, not a shrug. Two structurally different + answers hide behind "no": + + * ``wrist_roll`` — **impossible**. Its travel covers the whole circle, so + there is no unreachable arc to evict the seam into; no offset can help, + and a soft limit already handles it (:data:`_REZERO_IMPOSSIBLE`). + * the other four — **unnecessary**. Their encoders do not wrap inside their + travel, so there is no seam in the way (:data:`_REZERO_UNNECESSARY`). + + Collapsing those into one message would teach the operator the wrong thing + about their arm — and would make "wrist_roll isn't supported yet" a + plausible reading of a limit that is permanent and provable. + + Raises + ------ + ValueError + If *joint* is not one of :data:`JOINTS`. + """ + if rezero_arc(joint) is not None: # validates *joint* + return None + impossible = _REZERO_IMPOSSIBLE.get(joint) + if impossible is not None: + return impossible + return _REZERO_UNNECESSARY.format(joint=joint) + + +def resolve_bounds(joint: str, eeprom_min: int, eeprom_max: int) -> tuple[int, int]: + """Return the travel bounds a move may actually use for *joint*. + + This is the function that makes :data:`SOFT_LIMITS` **bind**. Without it the + table is inert: every move in this codebase sources its bounds from the + servo's EEPROM ``min_angle``/``max_angle`` registers, and on this arm those + are the untouched factory ``0-4095`` on all six joints + (``docs/hardware-validation-arm-explore.md`` — the EEPROM knows nothing + about the arm's real travel). So ``arm flex wrist_roll --to 4090`` would + have been perfectly happy to drive the joint into the arc the soft limit + exists to exclude, straight across the encoder seam, reproducing the exact + hang documented in this module's docstring. A soft limit nobody reads + protects nothing. Every call site that turns a ``read_info`` result into + move bounds — ``arm flex``, ``arm explore``'s grid, the demo sweep — routes + through here. + + **Intersection, not replacement.** The soft limit says "never go outside + ``(min_tick, max_tick)``". It does NOT say "always permit + ``(min_tick, max_tick)``". If a servo's EEPROM limits are genuinely + narrower than the soft limit — an operator's calibration, a fixture, a + cable-routing constraint — those are a real physical constraint that a + software table has no business widening, and replacing them would drive the + joint somewhere the servo was explicitly configured not to go. So each end + independently takes the **tighter** of the two: the higher low bound, the + lower high bound. A joint with no soft limit (four of six — see + :func:`soft_limit`) gets its EEPROM bounds back verbatim; this function + must never quietly narrow a joint that never had a wrap problem. + + **Read-side only.** Nothing here — and nothing downstream of here — writes + the resolved range back to the servo. That is the standing spec boundary + for this whole line of work: measured and derived ranges are pose- and + environment-dependent, so they live in this module and in the reachability + map, never burnt into EEPROM where they would outlive the pose that + produced them. This module cannot violate that even by accident: it imports + no bus (pinned by ``test_arm_spec_module_never_imports_the_bus``). + + Parameters + ---------- + joint: + One of the six joint names in :data:`JOINTS`. + eeprom_min, eeprom_max: + The joint's angle limits as read from the servo, i.e. + ``bus.read_info(motor)["min_angle"]`` / ``["max_angle"]``. + + Returns + ------- + tuple[int, int] + ``(min_tick, max_tick)`` — the bounds to hand to ``clamp_goal`` / + ``compliant_move`` / ``gentle_move`` / ``GridSpec.bounds``. + + Raises + ------ + ValueError + If *joint* is unknown, or if the intersection is EMPTY — i.e. the + servo's configured range lies entirely inside the soft limit's dead + arc, so the servo says "only ever go here" about precisely the arc the + soft limit says "never go here" about. No pair of bounds honours both + constraints, and returning the inverted pair would surface downstream + as :func:`arm101.hardware.motion.clamp_goal`'s misleading "min/max were + swapped" error, several frames from the real cause. This module stays + free of CLI concerns — callers at the CLI/hardware layer translate this + into a :class:`~arm101.cli._errors.CliError`. Note that an *inverted* + EEPROM pair (``eeprom_min > eeprom_max``) is deliberately NOT caught + here: intersection preserves inversion, and ``clamp_goal`` already owns + that error with a message that names the real problem. + """ + limit = soft_limit(joint) # validates *joint* + if limit is None: + return (eeprom_min, eeprom_max) + + low = max(eeprom_min, limit.min_tick) + high = min(eeprom_max, limit.max_tick) + # Only an EEPROM range that is itself well-ordered can produce an empty + # intersection here; an inverted EEPROM pair stays inverted (low > high on + # both sides of the max/min) and is clamp_goal's error to report, not ours. + if eeprom_min <= eeprom_max and low > high: + raise ValueError( + f"Joint {joint!r} has no permitted travel: its servo angle limits " + f"({eeprom_min}, {eeprom_max}) lie entirely inside the soft limit's dead arc " + f"(permitted range is ({limit.min_tick}, {limit.max_tick})). Widen the servo's " + "angle limits or retune the soft limit — they currently contradict each other." + ) + return (low, high) + + +def soft_limit(joint: str) -> Optional[SoftLimit]: + """Return *joint*'s :class:`SoftLimit`, or ``None`` if it has no software travel restriction. + + ``None`` is a real, common answer — most joints (four of six) have no + wrap problem and no soft limit; ``None`` means "the full ``[TICK_MIN, + TICK_MAX]`` range is permitted", not "unknown". + + Parameters + ---------- + joint: + One of the six joint names in :data:`JOINTS`. + + Raises + ------ + ValueError + If *joint* is not one of :data:`JOINTS`. + """ + if joint not in JOINTS: + raise ValueError(f"Unknown joint {joint!r}. Valid joints: {list(JOINTS)}.") + return SOFT_LIMITS.get(joint) + + # --------------------------------------------------------------------------- # Internal helpers # --------------------------------------------------------------------------- diff --git a/arm101/hardware/bus.py b/arm101/hardware/bus.py index 420e071..69f07bb 100644 --- a/arm101/hardware/bus.py +++ b/arm101/hardware/bus.py @@ -18,7 +18,7 @@ from types import TracebackType from typing import TYPE_CHECKING -from arm101.cli._errors import EXIT_ENV_ERROR, CliError +from arm101.cli._errors import EXIT_ENV_ERROR, EXIT_USER_ERROR, CliError if TYPE_CHECKING: pass # No runtime imports needed for type hints here @@ -177,6 +177,161 @@ def load_magnitude(present_load: int) -> int: return present_load & _LOAD_MAGNITUDE_MASK +# --------------------------------------------------------------------------- +# Register addresses shared across methods (single source of truth) +# --------------------------------------------------------------------------- + +#: STS3215 ``Torque_Enable`` — SRAM, addr 40, 1 byte. Write 0 to relax, 1 to hold. +ADDR_TORQUE_ENABLE: int = 40 + +#: STS3215 ``Lock`` — EEPROM write-protect flag, addr 55, 1 byte. It must be +#: opened (0) around any EEPROM write and closed (1) afterwards; see +#: :meth:`FeetechBus._set_lock` and PR #21. +ADDR_LOCK: int = 55 + +#: STS3215 ``Ofs`` (Feetech) / ``Homing_Offset`` (LeRobot) — **EEPROM**, addr 31, +#: 2 bytes (31 = OFS_L, 32 = OFS_H). The servo subtracts it from the raw encoder +#: count before reporting: ``Present_Position = Actual_Position - Homing_Offset``. +#: +#: This is the register that fixes issue #35. ``elbow_flex``'s encoder WRAPS +#: inside its physical travel — driven far enough it crosses the raw 4095->0 +#: seam and reads back near zero — so its reported position is not monotonic +#: with joint angle, and every position comparison in this codebase (arrival +#: checks, ``clamp_goal``, ``ReachMap`` ranges) is silently wrong for it. +#: Writing an offset shifts the joint's zero so the seam falls in the arc the +#: joint *cannot reach*, which makes the linear-axis assumption TRUE rather than +#: merely assumed. +#: +#: It sits in the same EEPROM region as ID (5) and Baud_Rate (6) — the exact +#: registers that PR #21 proved revert on power-cycle when the Lock (addr 55) is +#: left closed — so it carries the identical persistence hazard. +#: +#: Research: ``docs/spikes/sts3215-offset-register.md`` (triple-sourced against +#: Feetech's SMS_STS.h, Feetech's Python SDK, and LeRobot's feetech/tables.py). +ADDR_HOMING_OFFSET: int = 31 + + +# --------------------------------------------------------------------------- +# Encoder-offset codec — SIGN-MAGNITUDE on bit 11 (NOT two's complement) +# --------------------------------------------------------------------------- + +#: Bit index of the SIGN in the offset register's encoding. The wire value is +#: ``(sign << 11) | magnitude`` — sign-magnitude, **not** two's complement. +#: (LeRobot: ``"Homing_Offset": 11`` in ``STS_SMS_SERIES_ENCODINGS_TABLE``.) +OFFSET_SIGN_BIT: int = 11 + +#: The sign bit itself (2048). Set = negative. +_OFFSET_SIGN_MASK: int = 1 << OFFSET_SIGN_BIT + +#: Bits 0-10 — the magnitude field (0-2047). +_OFFSET_MAGNITUDE_MASK: int = _OFFSET_SIGN_MASK - 1 + +#: Widest magnitude the encoding can hold: ``(1 << 11) - 1`` = **2047**. An +#: offset outside ``[-2047, +2047]`` is not representable — a real SO-101 hit +#: exactly this (LeRobot issue #3193: ``ValueError: Magnitude 2073 exceeds +#: 2047``) — so it is REJECTED, never truncated. Truncation would spill the +#: magnitude straight into the sign bit and turn +2048 into "-0". +OFFSET_MAX_MAGNITUDE: int = _OFFSET_MAGNITUDE_MASK + +#: The offset register occupies the low 12 bits of a 2-byte read; bits 12-15 are +#: not part of it. Mirrors :meth:`FeetechBus.read_position`'s ``& 0x0FFF``. +_OFFSET_REGISTER_MASK: int = 0xFFF + +#: One full turn of the 12-bit magnetic encoder. The corrected position is +#: reduced modulo this, which is what makes the seam *move* rather than merely +#: be relabelled (see :meth:`FakeBus._reported_position` for the caveat). +ENCODER_RESOLUTION: int = 4096 + + +def encode_offset(offset: int) -> int: + """Encode a signed *offset* into the STS3215's sign-magnitude wire value. + + The offset register (:data:`ADDR_HOMING_OFFSET`) is **sign-magnitude with + the sign on bit 11**, not two's complement:: + + wire = (sign << 11) | magnitude + + So ``+1073`` goes on the wire as ``1073``, and ``-1073`` as + ``2048 + 1073 == 3121``. A two's-complement encoder — the obvious wrong + guess, and the one every stdlib instinct pushes you toward — would send + ``-1073`` as ``0xFBCF`` (64463); the servo would read that as a nonsense + magnitude and every position it reported afterwards would be garbage. That + is why this function exists at module scope with its own round-trip test + rather than being three inline lines inside :meth:`FeetechBus.write_offset`. + + The bit index is deliberately **not** a parameter. Bit 11 belongs to this + register alone: ``Present_Position`` is sign-magnitude on bit **15**, and + ``Present_Load`` carries a *direction* flag on bit **10** + (:func:`load_magnitude`). A general-purpose codec would invite applying the + wrong width to the wrong register. + + Parameters + ---------- + offset: + Signed offset in ticks, ``[-2047, +2047]``. + + Returns + ------- + int + The 12-bit wire value to write to addr 31. + + Raises + ------ + CliError(EXIT_USER_ERROR) + If ``abs(offset) > 2047`` (:data:`OFFSET_MAX_MAGNITUDE`). Rejected + loudly rather than truncated: ``2048`` truncated to 11 bits is ``0`` + with the sign bit set — i.e. the servo would be told "negative zero" + when the caller asked for a two-thousand-tick shift, and the joint's + frame would be silently wrong forever after. + """ + magnitude = abs(offset) + if magnitude > OFFSET_MAX_MAGNITUDE: + raise CliError( + code=EXIT_USER_ERROR, + message=( + f"Encoder offset {offset} is out of range: magnitude {magnitude} " + f"exceeds {OFFSET_MAX_MAGNITUDE} (the STS3215 offset register is " + f"sign-magnitude on bit {OFFSET_SIGN_BIT}, so it can only hold " + f"-{OFFSET_MAX_MAGNITUDE}..+{OFFSET_MAX_MAGNITUDE})." + ), + remediation=( + f"Pass an offset between -{OFFSET_MAX_MAGNITUDE} and " + f"+{OFFSET_MAX_MAGNITUDE}. Any seam placement in 0..2047 is " + "reachable; only raw 2048 is not expressible." + ), + ) + if offset < 0: + return _OFFSET_SIGN_MASK | magnitude + return magnitude + + +def decode_offset(raw: int) -> int: + """Decode a raw addr-31 register read into a signed offset. + + The inverse of :func:`encode_offset`. Bit 11 set means negative; bits 0-10 + are the magnitude. Bits 12-15 of the 2-byte read are not part of the + register and are masked off (mirroring :meth:`FeetechBus.read_position`'s + ``& 0x0FFF``), so a stray high bit from a noisy read is discarded rather + than decoded into a nonsense magnitude. + + Returning the raw value instead of decoding is the failure this guards: a + servo holding ``-1073`` reports ``3121``, which reads as an entirely + plausible *positive* offset. Nothing downstream could tell the difference, + and every position comparison in the joint's frame would be off by 2146 + ticks. + + Note the encoding is not injective: wire ``0`` and wire ``2048`` + ("negative zero") both decode to ``0``. Both are values a servo can + genuinely hold, so the round-trip property is ``decode(encode(x)) == x`` — + never the reverse. :func:`encode_offset` always emits the canonical ``+0``. + """ + value = int(raw) & _OFFSET_REGISTER_MASK + magnitude = value & _OFFSET_MAGNITUDE_MASK + if value & _OFFSET_SIGN_MASK: + return -magnitude + return magnitude + + #: Shared remediation text for :class:`OverloadError`. _REMEDIATION_OVERLOAD = ( "The servo latched an overload fault (status error bit 5 / 0x20): load " @@ -525,6 +680,74 @@ def clear_overload(self, motor: int) -> None: If the bus has not been opened or the write fails. """ + @abc.abstractmethod + def read_offset(self, motor: int) -> int: + """Return *motor*'s encoder offset (``Ofs``/``Homing_Offset``, addr 31) as a SIGNED int. + + The value is decoded from the register's sign-magnitude encoding + (:func:`decode_offset`), so a servo holding ``-1073`` — which reports + ``3121`` on the wire — returns ``-1073``. The wire encoding never + escapes this module. + + Read-only: this touches no other register and commands no motion. + + Returns + ------- + int + Signed offset in encoder ticks, ``[-2047, +2047]``. ``0`` on a + factory servo. + + Raises + ------ + CliError(EXIT_ENV_ERROR) + If the bus has not been opened or the read fails. + """ + + @abc.abstractmethod + def write_offset(self, motor: int, offset: int) -> None: + """Write *motor*'s encoder offset to EEPROM (``Ofs``/``Homing_Offset``, addr 31). + + This re-zeros the joint: afterwards the servo reports + ``Present_Position = Actual_Position - offset``. It is the fix for + issue #35 — see :data:`ADDR_HOMING_OFFSET`. + + **This is a persistent EEPROM write.** Implementations MUST: + + 1. Reject an unrepresentable *offset* BEFORE touching the bus + (:func:`encode_offset`), so a bad value has no side effects at all. + 2. Disable torque (addr 40) FIRST. With torque on, the servo instantly + re-interprets its own position against the new offset and could + LURCH toward its standing goal. (Inferred, not documented — treated + as a hard safety rule.) + 3. Open the EEPROM Lock (addr 55 -> 0), write ONLY addr 31, then + re-lock (addr 55 -> 1) — including on the failure path, so a failed + write never strands the motor unlocked. Skipping the unlock makes + the write REVERT on the next power-cycle (PR #21). + 4. Write **address 31 and nothing else**. In particular NOT + ``Min_Position_Limit`` (9) / ``Max_Position_Limit`` (11), which + LeRobot's ``write_calibration`` also writes: in servo mode those + CLAMP goals, and narrowing them would shrink the very reachable set + the re-zero exists to recover. + + Parameters + ---------- + motor: + Motor ID (1-indexed, matching the Feetech servo ID). + offset: + Signed offset in encoder ticks, ``[-2047, +2047]``. + + Raises + ------ + CliError(EXIT_USER_ERROR) + If ``abs(offset) > 2047`` — before any register is touched. + CliError(EXIT_ENV_ERROR) + If the bus has not been opened, or any of the writes fail. + OverloadError + If the motor is latched in overload (it tags every response with + status bit 5, including the torque-off write's). The EEPROM is not + opened in that case; call :meth:`clear_overload` first. + """ + # ------------------------------------------------------------------ # Context-manager helpers (shared implementation) # ------------------------------------------------------------------ @@ -648,9 +871,8 @@ def _set_lock(self, motor: int, locked: bool) -> None: (``locked=True``) afterwards to restore write-protection. """ self._require_open() - _ADDR_LOCK = 55 result, error = self._packet_handler.write1ByteTxRx( # type: ignore[union-attr] - self._port_handler, motor, _ADDR_LOCK, 1 if locked else 0 + self._port_handler, motor, ADDR_LOCK, 1 if locked else 0 ) if result != 0 or error != 0: state = "re-lock" if locked else "unlock" @@ -735,10 +957,16 @@ def close(self) -> None: def read_position(self, motor: int) -> int: """Read the present-position register (address 56, 2 bytes) for *motor*. + The value the servo reports is already offset-corrected: + ``Present_Position = Actual_Position - Homing_Offset`` (see + :data:`ADDR_HOMING_OFFSET`). This method reads it as-is — it does not + add the offset back, because the corrected frame IS the frame goals are + commanded in. + Returns ------- int - Raw 12-bit encoder tick in ``[0, 4095]``. + 12-bit encoder tick in ``[0, 4095]``, as reported. """ self._require_open() @@ -752,7 +980,16 @@ def read_position(self, motor: int) -> int: raise self._status_error( motor, result, error, f"Read position failed for motor {motor}" ) - return int(value) & 0x0FFF # mask to 12 bits + # Mask to 12 bits. DELIBERATELY unchanged by the offset work (issue #35): + # Present_Position is itself sign-magnitude on bit 15, so this mask would + # SWALLOW a negative reading. That only matters if the firmware reports the + # corrected position as an unwrapped signed subtraction rather than reducing + # it modulo 4096 — the one unproven assumption behind the whole re-zero + # (docs/spikes/sts3215-offset-register.md §4). Every source points at the + # modular reading, in which case the value is always in [0, 4095] and this + # mask is a no-op. If the hardware test (step 10) says otherwise, the re-zero + # does not work at all and THIS mask is the next thing to fix. + return int(value) & 0x0FFF def write_id_baudrate(self, motor: int, new_id: int, baudrate: int) -> None: """Write servo ID and baud-rate to the motor's EEPROM. @@ -919,6 +1156,12 @@ def write_baudrate(self, motor: int, baudrate: int) -> None: # without this, build_plan would default lock_register to 0 on real # hardware even though the motor reports it. "lock_register": (55, 1), + # EEPROM encoder offset (addr 31). Surfaced so `arm read` can SHOW the + # re-zero before anyone writes it (issue #35) — the register that most + # needs inspecting would otherwise be the one register you cannot see. + # NOTE: read_info DECODES this one (see below); every other entry here + # is the raw register value. + "homing_offset": (ADDR_HOMING_OFFSET, 2), } def _read_register(self, motor: int, addr: int, length: int) -> int: @@ -959,12 +1202,29 @@ def scan(self, ids: "list[int] | None" = None) -> "list[int]": return sorted(found) def read_info(self, motor: int) -> "dict[str, int]": - """Return a full read-only register snapshot for *motor*.""" + """Return a full read-only register snapshot for *motor*. + + Every value is the raw register **except** ``homing_offset``, which is + decoded from its sign-magnitude wire form (:func:`decode_offset`) into a + signed int. That single exception is deliberate: the raw form is a trap. + A servo holding an offset of ``-1073`` reports ``3121``, which reads as + a perfectly plausible *positive* offset — so handing the raw value to + callers would let ``arm read`` display a confident, wrong number to the + human deciding whether to re-zero the joint. ``read_info["homing_offset"]`` + and :meth:`read_offset` therefore return the same one number with the + same one meaning, and the wire encoding never leaves this module. + + (Contrast ``present_load``, which IS raw, direction bit and all — see + :func:`load_magnitude`. The difference is that a raw load is merely + *large*; a raw offset is *plausible*.) + """ self._require_open() - return { + snapshot = { name: self._read_register(motor, addr, length) for name, (addr, length) in self._INFO_REGISTERS.items() } + snapshot["homing_offset"] = decode_offset(snapshot["homing_offset"]) + return snapshot def enable_torque(self, motor: int, on: bool) -> None: """Enable or disable torque for *motor*. @@ -974,10 +1234,8 @@ def enable_torque(self, motor: int, on: bool) -> None: """ self._require_open() - _ADDR_TORQUE_ENABLE = 40 - result, error = self._packet_handler.write1ByteTxRx( # type: ignore[union-attr] - self._port_handler, motor, _ADDR_TORQUE_ENABLE, 1 if on else 0 + self._port_handler, motor, ADDR_TORQUE_ENABLE, 1 if on else 0 ) if result != 0 or error != 0: state = "enable" if on else "disable" @@ -1025,10 +1283,8 @@ def read_lock(self, motor: int) -> int: """ self._require_open() - _ADDR_LOCK = 55 - value, result, error = self._packet_handler.read1ByteTxRx( # type: ignore[union-attr] - self._port_handler, motor, _ADDR_LOCK + self._port_handler, motor, ADDR_LOCK ) if result != 0 or error != 0: raise self._status_error( @@ -1148,9 +1404,8 @@ def clear_overload(self, motor: int) -> None: """ self._require_open() - _ADDR_TORQUE_ENABLE = 40 result, error = self._packet_handler.write1ByteTxRx( # type: ignore[union-attr] - self._port_handler, motor, _ADDR_TORQUE_ENABLE, 0 + self._port_handler, motor, ADDR_TORQUE_ENABLE, 0 ) # Mask off the overload bit — it is the very flag we are clearing. residual = error & ~_OVERLOAD_BIT @@ -1159,6 +1414,97 @@ def clear_overload(self, motor: int) -> None: motor, result, residual, f"Failed to clear overload for motor {motor}" ) + # ------------------------------------------------------------------ + # Encoder offset (Ofs / Homing_Offset) — EEPROM addr 31 + # ------------------------------------------------------------------ + + def read_offset(self, motor: int) -> int: + """Read *motor*'s encoder offset (addr 31, 2 bytes) and decode it to a signed int. + + Read-only — the counterpart to :meth:`write_offset` that lets a human + (or ``arm read``) inspect the current re-zero without risking one. See + :data:`ADDR_HOMING_OFFSET` for what the register does and why issue #35 + needs it. + """ + self._require_open() + return decode_offset(self._read_register(motor, ADDR_HOMING_OFFSET, 2)) + + def write_offset(self, motor: int, offset: int) -> None: + """Write *motor*'s encoder offset to EEPROM (addr 31, 2 bytes). + + The contract is :meth:`MotorBus.write_offset`; this is how it is met. + + The exact wire sequence, and why each step is where it is:: + + (40, 0) torque OFF + (55, 0) unlock EEPROM + (31, wire) the offset — SIGN-MAGNITUDE encoded, and nothing else + (55, 1) re-lock EEPROM + + **Validation first, wire traffic second.** :func:`encode_offset` runs + before anything is sent, so an out-of-range offset leaves the arm + exactly as it found it: torque still on, EEPROM still locked. Rejecting + after the torque-off would silently relax a joint as a side effect of a + *failed* call. + + **Torque off first.** With torque enabled the servo instantly + re-interprets its own present position against the newly written offset; + with a standing goal still latched, its position error jumps by the + offset in one step and it can LURCH. This is inferred rather than + documented, and is treated as a hard safety rule — enforced here in the + primitive, not left to the caller to remember. (It also means a motor + latched in overload raises :class:`OverloadError` from this very write, + BEFORE the EEPROM is opened — call :meth:`clear_overload` first.) + + **The Lock dance is mandatory, not decorative.** PR #21 of this repo + exists because id/baud writes appeared to work, read back correctly, and + then silently REVERTED on the next power-cycle: on fw 3.10 a write while + Lock=1 updates the live register but is never committed to EEPROM. + Addr 31 is in the same EEPROM region and fails the same way. The re-lock + is attempted on the failure path too (best-effort, preserving the + original error), because a motor stranded at Lock=0 is a motor whose id, + baud and offset any stray write can clobber. + + **Only addr 31.** LeRobot's ``write_calibration`` also writes + ``Min_Position_Limit`` (9) and ``Max_Position_Limit`` (11). In servo + mode the firmware CLAMPS goals to that window — so copying LeRobot + wholesale would narrow the reachable set that this re-zero exists to + recover. Our factory limits are the wide-open 0/4095 and stay that way. + ``tests/test_bus_offset.py`` pins the write surface to an exact + allow-list, so adding a register here is a deliberate act, not a slip. + """ + self._require_open() + + # Encode BEFORE any wire traffic: a rejected offset must have no + # side effects whatsoever (no torque-off, no unlock). + wire = encode_offset(offset) + + # Safety: the servo must be limp before its frame of reference moves + # under it. Raises (incl. OverloadError) if the motor will not relax — + # in which case we have not unlocked anything and never will. + self.enable_torque(motor, False) + + # Persistence: without the unlock the write reverts on power-cycle. + self._set_lock(motor, False) + try: + result, error = self._packet_handler.write2ByteTxRx( # type: ignore[union-attr] + self._port_handler, motor, ADDR_HOMING_OFFSET, wire + ) + if result != 0 or error != 0: + raise self._status_error( + motor, result, error, f"Write encoder offset failed for motor {motor}" + ) + except BaseException: + # Best-effort re-lock so a failed write never strands the motor at + # Lock=0; if the re-lock itself fails, preserve the ORIGINAL error — + # the operator needs to know why the OFFSET write failed, not to be + # sent hunting for a Lock problem that is a symptom, not the cause. + with contextlib.suppress(Exception): + self._set_lock(motor, True) + raise + else: + self._set_lock(motor, True) + # --------------------------------------------------------------------------- # In-memory FakeBus — for tests and offline development @@ -1173,8 +1519,21 @@ class FakeBus(MotorBus): Parameters ---------- positions: - Optional mapping of motor-id → initial encoder position. Motors not - present in the dict return :data:`_DEFAULT_POSITION` (2048). + Optional mapping of motor-id → initial **actual** encoder position (the + raw magnetic count on the shaft). Motors not present in the dict use + :data:`_DEFAULT_POSITION` (2048). What the bus *reports* is this minus + the motor's ``Homing_Offset`` — see :meth:`_reported_position`; with the + default zero offset the two are identical. + offsets: + Optional mapping of motor-id → **signed** encoder offset (``Ofs`` / + ``Homing_Offset``, addr 31). Motors not present read ``0`` (the factory + value). Seeds a servo that has already been re-zeroed, without having + to drive :meth:`write_offset` first. + offset_wraps: + Whether the corrected position is reduced modulo 4096 (the seam *moves*) + or reported as a plain signed subtraction (the seam *stays*). Defaults + to ``True``; see :meth:`_reported_position` for why this is a switch and + not a hard-coded assumption. Attributes ---------- @@ -1224,6 +1583,14 @@ class FakeBus(MotorBus): List of dicts, one per :meth:`write_torque_limit` call, in call order:: {"motor": int, "value": int} + offset_writes: + List of dicts, one per :meth:`write_offset` call, in call order:: + + {"motor": int, "offset": int} + + The **signed** offset, as the caller asked for it. The sign-magnitude + *wire* value lands in :attr:`register_writes` at addr 31, so a test can + pin both the intent and the encoding. register_writes: List of dicts, one per ACTUAL register write across every write method above (plus :meth:`write_torque_limit` / :meth:`clear_overload`), @@ -1256,6 +1623,8 @@ def __init__( lock_register: int = 0, torque_limits: "dict[int, int] | None" = None, overload_after_ops: "int | None" = None, + offsets: "dict[int, int] | None" = None, + offset_wraps: bool = True, ) -> None: self._positions: dict[int, int] = dict(positions) if positions else {} # Motor IDs the fake bus reports from scan(). Defaults to whatever the @@ -1280,6 +1649,11 @@ def __init__( self.register_writes: list[dict[str, int]] = [] self.overload_after_ops: "int | None" = overload_after_ops self._op_count: int = 0 + # Signed encoder offsets (addr 31). Motors absent from the dict hold the + # factory 0, i.e. reported position == actual position. + self._offsets: dict[int, int] = dict(offsets) if offsets else {} + self.offset_wraps: bool = offset_wraps + self.offset_writes: list[dict[str, int]] = [] self._open = False # ------------------------------------------------------------------ @@ -1347,6 +1721,74 @@ def _record_write(self, motor: int, addr: int, value: int) -> None: """ self.register_writes.append({"motor": motor, "addr": addr, "value": value}) + def _set_lock_fake(self, motor: int, locked: bool) -> None: + """Model the EEPROM Lock register (addr 55) write, and record it. + + The fake performs the unlock -> write -> re-lock dance for real (as a + recorded register write and a mutation of :attr:`lock_register`) rather + than skipping it, because a fake that omitted the dance would let a test + "prove" an EEPROM write that on hardware reads back correctly and then + silently REVERTS on the next power-cycle — the exact failure PR #21 was + written to kill. + """ + self.lock_register = 1 if locked else 0 + self._record_write(motor, ADDR_LOCK, self.lock_register) + + # ------------------------------------------------------------------ + # The encoder offset's EFFECT on what the servo reports + # ------------------------------------------------------------------ + + def _reported_position(self, motor: int, actual: int) -> int: + """Apply *motor*'s ``Homing_Offset`` to an *actual* count, as the servo does. + + ``Present_Position = Actual_Position - Homing_Offset``. This is the + single funnel through which every position this fake REPORTS passes + (:meth:`read_position` and :meth:`read_info` alike), because on the wire + they are the same register (addr 56). A fake in which one of them + applied the offset and the other did not would model a servo that does + not exist — and would let a test pass against behaviour no hardware can + produce. (The same lesson as ``clear_overload``/``enable_torque``: one + wire operation, one overridable method.) + + With no offset written this is the exact identity — not + ``actual % 4096`` — so a fixture that seeds a position outside + ``[0, 4095]`` still reads back byte-for-byte what it was given. + + **The modulo is the fake's one unproven assumption, and it is a switch.** + With ``offset_wraps=True`` (the default) the corrected value is reduced + modulo 4096, so the 4095->0 seam RELOCATES to ``raw == offset`` — which + is the entire premise of the issue-#35 re-zero. The alternative reading + (``offset_wraps=False``) is a plain signed subtraction: the offset then + merely *relabels* positions, the discontinuity stays pinned to the + physical angle where the magnet rolls over, and the re-zero achieves + nothing. Every source and LeRobot's shipped SO-101 calibration imply the + modular reading, but no primary Feetech source states the firmware's + formula (see ``docs/spikes/sts3215-offset-register.md`` §4), so the + other branch is modelled rather than assumed away. Hardware test step 10 + settles it. + """ + offset = self._offsets.get(motor, 0) + if offset == 0: + return actual + reported = actual - offset + return reported % ENCODER_RESOLUTION if self.offset_wraps else reported + + def _actual_position(self, motor: int, reported: int) -> int: + """Inverse of :meth:`_reported_position`: corrected frame -> raw encoder count. + + ``Actual = Present + Homing_Offset``. Needed because a *goal* is + commanded in the same corrected frame the servo reports in — if goals + and feedback lived in different frames the re-zero would be worse than + useless — so a fake that models the shaft in raw counts has to convert + an incoming goal back. Used by ``ServoModelBus``; harmless (identity) at + the default zero offset. + """ + offset = self._offsets.get(motor, 0) + if offset == 0: + return reported + actual = reported + offset + return actual % ENCODER_RESOLUTION if self.offset_wraps else actual + # ------------------------------------------------------------------ # MotorBus interface # ------------------------------------------------------------------ @@ -1360,7 +1802,10 @@ def close(self) -> None: self._open = False def read_position(self, motor: int) -> int: - """Return the preset position for *motor*, defaulting to 2048. + """Return the position *motor* REPORTS: its actual count minus its offset. + + Defaults to 2048 (:data:`_DEFAULT_POSITION`) with no offset written, in + which case reported == actual. See :meth:`_reported_position`. Raises ------ @@ -1379,7 +1824,7 @@ def read_position(self, motor: int) -> int: remediation=_FAKEBUS_NOT_OPEN_REMEDIATION, ) self._tick_and_maybe_overload(motor) - return self._positions.get(motor, _DEFAULT_POSITION) + return self._reported_position(motor, self._positions.get(motor, _DEFAULT_POSITION)) def write_id_baudrate(self, motor: int, new_id: int, baudrate: int) -> None: """Record an EEPROM write in :attr:`eeprom_writes`. @@ -1441,7 +1886,7 @@ def enable_torque(self, motor: int, on: bool) -> None: self._require_open_fake() self._tick_and_maybe_overload(motor) self.torque_writes.append({"motor": motor, "on": on}) - self._record_write(motor, 40, 1 if on else 0) + self._record_write(motor, ADDR_TORQUE_ENABLE, 1 if on else 0) def write_goal_position(self, motor: int, position: int) -> None: """Record a goal-position write in :attr:`position_writes`. @@ -1482,8 +1927,11 @@ def read_info(self, motor: int) -> "dict[str, int]": """Return a canned read-only register snapshot for *motor*. Defaults mimic a factory STS3215 (model 777, firmware 3.10, baud 1 Mbps, - 12.0 V, 38 deg C); ``present_position`` reflects the positions dict and - anything in the ``info`` constructor override wins. + 12.0 V, 38 deg C); ``present_position`` reflects the positions dict + (shifted by any written offset, exactly as the servo reports it — see + :meth:`_reported_position`), ``homing_offset`` is the **signed** offset + (matching :meth:`FeetechBus.read_info`, which decodes it), and anything + in the ``info`` constructor override wins. """ self._require_open_fake() self._tick_and_maybe_overload(motor) @@ -1496,12 +1944,15 @@ def read_info(self, motor: int) -> "dict[str, int]": "min_angle": 0, "max_angle": 4095, "torque_enable": 0, - "present_position": self._positions.get(motor, _DEFAULT_POSITION), + "present_position": self._reported_position( + motor, self._positions.get(motor, _DEFAULT_POSITION) + ), "present_speed": 0, "present_load": 0, "present_voltage": 120, "present_temperature": 38, "lock_register": self.lock_register, + "homing_offset": self._offsets.get(motor, 0), } snapshot.update(self._info_overrides.get(motor, {})) return snapshot @@ -1651,9 +2102,83 @@ def clear_overload(self, motor: int) -> None: # The latch rides on the response to the very write that clears it. # The byte DID land — record it, mirroring FeetechBus's masking. self.torque_writes.append({"motor": motor, "on": False}) - self._record_write(motor, 40, 0) + self._record_write(motor, ADDR_TORQUE_ENABLE, 0) self.overload_after_ops = None + # ------------------------------------------------------------------ + # Encoder offset (Ofs / Homing_Offset) — EEPROM addr 31 + # ------------------------------------------------------------------ + + def read_offset(self, motor: int) -> int: + """Return the signed encoder offset for *motor* (0 if never written). + + Raises + ------ + CliError(EXIT_ENV_ERROR) + If the bus has not been opened. + OverloadError + If the overload-simulation seam is armed and this call is the + Nth (or later) operation; see :meth:`fail_with_overload_on_op`. + """ + self._require_open_fake() + self._tick_and_maybe_overload(motor) + return self._offsets.get(motor, 0) + + def write_offset(self, motor: int, offset: int) -> None: + """Record an encoder-offset write, and apply its effect to reported positions. + + Faithfully reproduces :meth:`FeetechBus.write_offset`'s wire sequence — + torque-off (addr 40) -> unlock (55 -> 0) -> offset (31) -> re-lock + (55 -> 1) — into :attr:`register_writes`, with the SIGN-MAGNITUDE wire + value at addr 31, so a test asserting the sequence against the fake is + asserting something true of the hardware. Three properties are load-bearing + and each has a test: + + * The offset is validated (:func:`encode_offset`) **before any wire + traffic**, so a rejected offset does not leave the joint limp. + * The torque-off goes through :meth:`enable_torque` — one wire byte, one + overridable method — so a subclass that intercepts torque writes sees + this one too. + * A failed EEPROM write still re-locks, so the fake can never "prove" a + recovery path that leaves a real motor stranded at Lock=0. + + Once written, the offset changes what :meth:`read_position` and + :meth:`read_info` REPORT (``Present = Actual - Ofs``); the simulated + shaft itself does not move. See :meth:`_reported_position`. + + Raises + ------ + CliError(EXIT_USER_ERROR) + If ``abs(offset) > 2047`` — before anything is written. + CliError(EXIT_ENV_ERROR) + If the bus has not been opened. + OverloadError + If the overload-simulation seam is armed; on the torque-off it + fires before the EEPROM is opened, on the offset write it fires + after — and the re-lock still happens. + """ + self._require_open_fake() + + # Validate first: a rejected offset must have zero side effects. + wire = encode_offset(offset) + + # Safety rule, enforced not merely documented: the servo must be limp + # before its frame of reference shifts under it. + self.enable_torque(motor, False) + + self._set_lock_fake(motor, False) + try: + self._tick_and_maybe_overload(motor) # the EEPROM write itself + self._offsets[motor] = offset + self.offset_writes.append({"motor": motor, "offset": offset}) + self._record_write(motor, ADDR_HOMING_OFFSET, wire) + except BaseException: + with contextlib.suppress(Exception): + self._set_lock_fake(motor, True) + raise + else: + self._set_lock_fake(motor, True) + def _require_open_fake(self) -> None: from arm101.cli._errors import EXIT_ENV_ERROR, CliError diff --git a/arm101/hardware/demo.py b/arm101/hardware/demo.py index f36ec3d..0c178ad 100644 --- a/arm101/hardware/demo.py +++ b/arm101/hardware/demo.py @@ -11,18 +11,37 @@ move freely?" demo without ever risking a joint slamming into its mechanical limit or pressing through an obstruction. -Deliberately decoupled from calibration/spec concerns, same as ``motion`` and -``gentle``: callers are responsible for sourcing the joint-name -> motor-id -mapping (e.g. from ``arm_spec``) and pass it in explicitly. This module never -imports ``arm_spec`` or reads calibration files, and it never imports any CLI -module — it is a hardware-layer primitive only. +Still decoupled from calibration concerns, same as ``motion`` and ``gentle``: +callers are responsible for sourcing the joint-name -> motor-id mapping and +pass it in explicitly; this module reads no calibration file, and it never +imports any CLI module beyond the shared error type — it is a hardware-layer +primitive only. + +It does, however, import :mod:`arm101.hardware.arm_spec` for ONE thing: the +per-joint **soft limit** (:func:`~arm101.hardware.arm_spec.resolve_bounds`). +That import was deliberately absent until the soft limits had to actually bind, +and the reason it is here now is worth stating, because the obvious alternative +is worse. ``demo_sweep`` computes each joint's sub-range from the servo's +EEPROM ``[min_angle, max_angle]``, which on this arm is the untouched factory +``0-4095`` on every joint — so a sweep of ``wrist_roll`` starting near the +encoder seam (the t9 run found it parked at raw tick **4**) clamped its low +target to 0 and drove the joint straight through the wrap. The bounds could +instead have been injected by the caller, but a default that skips the soft +limit means the ONE caller that forgets drives a joint across the seam, and +"safe unless you remember" is not a safety property. ``demo_sweep`` already +receives joint NAMES, which is exactly the key the soft-limit table is keyed +by, so it resolves them itself and is safe by construction. ``arm_spec`` is a +pure data module (it imports no bus and cannot issue a register write), so this +buys the guarantee without widening what this module can do to the hardware. """ from __future__ import annotations from typing import TYPE_CHECKING -from arm101.cli._errors import EXIT_USER_ERROR, CliError +from arm101.cli._errors import EXIT_ENV_ERROR, EXIT_USER_ERROR, CliError +from arm101.hardware.arm_spec import JOINTS as _CANONICAL_JOINTS +from arm101.hardware.arm_spec import resolve_bounds from arm101.hardware.bus import OverloadError from arm101.hardware.gentle import _DEFAULT_LOAD_THRESHOLD, gentle_move from arm101.hardware.motion import clamp_goal @@ -45,6 +64,50 @@ ) +def _bounds_for(joint_name: str, info: "dict[str, int]") -> "tuple[int, int]": + """Resolve one joint's sweep bounds: EEPROM angle limits ∩ its soft limit. + + The soft limit (:func:`~arm101.hardware.arm_spec.resolve_bounds`) is what + keeps a sweep of ``wrist_roll`` out of the arc containing the encoder seam; + without it the factory ``0-4095`` EEPROM range permits the whole circle and + a sweep centred near the seam clamps a target straight through the wrap. + + *joint_name* is whatever key the CALLER put in the ``joints`` mapping, and + this module's contract is that the caller owns that mapping — the demo is + usable with a partial or ad-hoc set of joints. A name outside + :data:`~arm101.hardware.arm_spec.JOINTS` therefore cannot be keyed into the + soft-limit table at all and simply has no soft limit: it gets its EEPROM + bounds verbatim, exactly as before this function existed. That is not a + hole in the guarantee for the shipped CLI — ``arm flex --demo`` always + passes ``arm_spec.joint_ids(role)``, whose keys are the canonical six — it + is the price of keeping ``demo_sweep`` callable with a caller-chosen map + (and it is why a *typo'd* canonical name loses its soft limit; canonical + names are the ones you get by construction from ``arm_spec``). + + Raises + ------ + CliError(EXIT_ENV_ERROR) + If a canonical joint's EEPROM range and its soft limit do not overlap + at all — a hardware/configuration contradiction, surfaced before any + motion rather than as a confusing clamp error mid-sweep. + """ + eeprom_min = int(info["min_angle"]) + eeprom_max = int(info["max_angle"]) + if joint_name not in _CANONICAL_JOINTS: + return eeprom_min, eeprom_max + try: + return resolve_bounds(joint_name, eeprom_min, eeprom_max) + except ValueError as exc: + raise CliError( + code=EXIT_ENV_ERROR, + message=str(exc), + remediation=( + f"Check {joint_name}'s min_angle/max_angle with 'arm101 arm read --json'; " + "they contradict the joint's software travel limit, so no sweep is possible." + ), + ) from exc + + def _sweep_targets( bus: "MotorBus", motor: int, @@ -133,18 +196,25 @@ def demo_sweep( For each joint, in that order: - 1. ``info = bus.read_info(motor)`` supplies ``min_angle``, ``max_angle``, - and the joint's current ``present_position``. + 1. ``info = bus.read_info(motor)`` supplies the servo's EEPROM angle + limits and the joint's current ``present_position``. Those angle limits + are then intersected with the joint's SOFTWARE soft limit by + :func:`_bounds_for` to give the ``[min_angle, max_angle]`` this sweep + actually uses — on this arm the EEPROM is the factory ``0-4095`` on + every joint, so for a joint whose travel wraps the encoder seam + (``wrist_roll``) the EEPROM alone would permit the whole circle. 2. A safe sub-range is computed CENTRED ON THE CURRENT POSITION (not the calibrated midpoint, so the sweep explores from wherever the joint actually is): ``half_span = fraction * (max_angle - min_angle) / 2``, ``low = present_position - half_span``, ``high = present_position + half_span``. Both are rounded to the nearest tick and then passed through - :func:`arm101.hardware.motion.clamp_goal` against - ``[min_angle, max_angle]`` — the hard backstop that guarantees a - target NEVER exceeds the joint's calibrated bounds, regardless of - *fraction* or how close ``present_position`` sits to a limit. + :func:`arm101.hardware.motion.clamp_goal` against those resolved + ``[min_angle, max_angle]`` bounds — the hard backstop that guarantees a + target NEVER exceeds the joint's permitted travel, regardless of + *fraction* or how close ``present_position`` sits to a limit (and, for + a soft-limited joint, never lands in the dead arc even when the joint + is currently parked inside it). 3. ``gentle_move`` drives the joint to ``low``, then to ``high``, in that order, with ``allow_motion=True`` forced internally (the joint-level gate was already satisfied by the call into this function) and @@ -226,10 +296,17 @@ def demo_sweep( outcome for each call (see step 4 above), so a call that somehow reports both never also sets ``aborted_on_contact``/``aborted_joint``. + ``min_angle``/``max_angle`` in each joint report are the RESOLVED + bounds the sweep used (EEPROM ∩ soft limit), not the raw EEPROM + registers — they are the bounds that actually constrained the move. + Raises ------ CliError(EXIT_USER_ERROR) If ``allow_motion`` is not ``True`` — no bus calls are issued. + CliError(EXIT_ENV_ERROR) + If a joint's EEPROM angle limits and its soft limit do not overlap + (see :func:`_bounds_for`). CliError Propagated from the underlying ``gentle_move``/``bus`` calls (e.g. ``CliError(EXIT_ENV_ERROR)`` on a comms failure). @@ -260,8 +337,7 @@ def demo_sweep( aborted_on_overload = True overloaded_joint = joint_name break - min_angle = info["min_angle"] - max_angle = info["max_angle"] + min_angle, max_angle = _bounds_for(joint_name, info) start_position = info["present_position"] half_span = fraction * (max_angle - min_angle) / 2 diff --git a/arm101/hardware/rezero.py b/arm101/hardware/rezero.py new file mode 100644 index 0000000..6fd7b4e --- /dev/null +++ b/arm101/hardware/rezero.py @@ -0,0 +1,912 @@ +"""Encoder re-zero — the one EEPROM write that makes ``elbow_flex``'s tick axis linear. + +The problem (issue #35) +----------------------- +``elbow_flex``'s 12-bit encoder **wraps inside its own physical travel**. Driven +far enough it crosses the raw 4095->0 seam and reads back near zero, so its +reported position is *not monotonic with joint angle*: two different angles +report similar ticks, the joint's two measured endpoints sort into a ``[min, +max]`` pair that describes exactly the arc it CANNOT reach, and every position +comparison in this codebase — ``gentle_move``'s arrival check, ``clamp_goal``, +the reachability map's ranges — is silently wrong for it. It currently rests at +raw ~126, i.e. **past** its wrap. + +The fix is to shift the encoder's zero (``Ofs``/``Homing_Offset``, EEPROM addr +31) so the seam falls inside the arc the joint physically cannot reach +(:class:`~arm101.hardware.arm_spec.UnreachableArc`). Then every tick the joint +can actually reach lies on one side of the seam, and the linear-axis assumption +the whole codebase already makes becomes TRUE rather than merely assumed. + +The bootstrap problem — why nothing here commands motion +-------------------------------------------------------- +**The tool that makes the axis linear cannot itself rely on the axis being +linear.** That single sentence dictates the shape of this module. + +The obvious procedure — "drive the joint to mid-travel, then write the offset +that centres it" — is exactly the thing that must not happen. ``elbow_flex`` +rests at raw ~126, on the far side of its wrap. A goal of, say, 3121 (its +mid-travel) looks like a modest move in tick-space and is in fact a rotation +the *long way round*: the servo would drive from 126 down through 0, across the +whole 1894-tick arc it cannot reach, and into a wall. The commanded number is +sane; the physical consequence is not; and the discrepancy is precisely the +non-linearity this write exists to remove. So: + +* :func:`apply_rezero` **writes no goal position, ever** — the wire surface is + torque-off, unlock, addr 31, re-lock, and nothing else. It reads where the + joint physically is, computes the offset from the joint's *known unreachable + arc* (a table fact, not a live measurement), and writes it. +* :func:`sweep` also commands nothing: the joint is de-energised and a **human + hand** moves it. The one instrument that can prove the seam moved is a human + arm, because it is the only actuator in the building that does not need a + linear tick axis to work. + +Torque is off for the write (``bus.write_offset`` disables it first) and stays +off — a joint must not be *holding* when its own frame of reference changes +underneath it. + +The unproven assumption — and how ``--verify`` settles it +--------------------------------------------------------- +Everything above rests on one undocumented bit of firmware semantics +(``docs/spikes/sts3215-offset-register.md`` §4):: + + Present = (raw - Ofs) mod 4096 seam RELOCATES -> the fix works + Present = raw - Ofs (signed) seam STAYS -> the fix does NOTHING + +Under the second reading the offset merely *relabels* positions: the +discontinuity stays pinned to the physical angle where the magnet rolls over, +and the re-zero achieves nothing at all. Every source and LeRobot's shipped +SO-101 calibration imply the first, but no primary Feetech source states the +formula — so :class:`~arm101.hardware.bus.FakeBus` models BOTH +(``offset_wraps=True`` / ``False``) and this module is tested against both. + +**Reading the offset back only proves it was APPLIED. It does not prove the +seam MOVED.** Only a sweep does — which is why :func:`sweep` exists and why it +is not optional garnish on the write. A sweep that finds a discontinuity is a +STOP condition: the re-zero did not solve issue #35, and the plan has to go back +to the user for a re-decision. +""" + +from __future__ import annotations + +import time +from dataclasses import dataclass +from typing import TYPE_CHECKING, Callable, Optional, Sequence + +from arm101.cli._errors import EXIT_ENV_ERROR, EXIT_USER_ERROR, CliError +from arm101.hardware import arm_spec +from arm101.hardware.bus import FakeBus + +if TYPE_CHECKING: # pragma: no cover - typing only + from arm101.hardware.bus import MotorBus + + +# --------------------------------------------------------------------------- +# Sweep tuning — every number here is a claim about the hardware +# --------------------------------------------------------------------------- + +#: A single-sample position jump at or above this many ticks is a +#: **discontinuity**: the seam, crossed. It is not sensor noise and it is not a +#: fast hand. +#: +#: Sized against the three ways the seam can show up in a sweep, and against the +#: one thing it must never be confused with: +#: +#: * Seam still in the travel (no offset, or an offset that did nothing): the +#: report jumps 4095 -> 0, a delta of ~4095. +#: * Offset written, firmware does a plain signed subtraction, and +#: :meth:`~arm101.hardware.bus.FeetechBus.read_position`'s ``& 0x0FFF`` folds +#: the negative result back into range: the jump is ~1949 ticks for +#: ``H = 1073``. **This is the smallest discontinuity we can be shown**, and it +#: is why the threshold is not the tempting 2048. +#: * The same, unmasked (what :class:`~arm101.hardware.bus.FakeBus` reports with +#: ``offset_wraps=False``): the position goes NEGATIVE, a delta of ~4095, and +#: :attr:`SweepReport.out_of_range` catches it independently anyway. +#: +#: Against a human hand: the whole 2202-tick travel moved in a brisk 2 s, polled +#: every 50 ms, is ~55 ticks per sample. Even a yank is well under 500. The gap +#: between "fastest plausible hand" and "smallest possible seam crossing" is +#: nearly 4x, and 500 sits in the middle of it. +DISCONTINUITY_TICKS: int = 500 + +#: A direction reversal smaller than this is the operator's hand, not a signal. +#: Encoder read jitter is a few ticks; a human holding a joint still wobbles. +#: Matches ``gentle_move``'s arrival tolerance, which was sized against the same +#: noise floor. Used ONLY for the *descriptive* :attr:`SweepReport.monotonic` +#: flag — never for the verdict, which turns on continuity alone. +REVERSAL_TOLERANCE: int = 12 + +#: How much of the joint's expected travel the sweep must actually cover before +#: "no discontinuity" is allowed to mean anything. A sweep that moved the joint +#: 200 ticks and saw no seam has proved **nothing** — of course it saw no seam; +#: it never went near where the seam would be. Claiming a pass from that would +#: be the single most damaging thing this verb could do, because it would close +#: the open question with a lie. Hence :attr:`SweepReport.conclusive`. +MIN_COVERAGE: float = 0.8 + +#: Default wall-clock length of a ``--verify`` sweep, in seconds. Long enough +#: for a human to move one joint, hand-over-hand, from one hard stop to the +#: other without hurrying — hurrying is how you skip past the seam between two +#: samples. +DEFAULT_SWEEP_DURATION: float = 30.0 + +#: Seconds between position polls during a sweep. 50 ms gives ~600 samples over +#: the default duration and ~55 ticks/sample at a brisk hand speed — dense +#: enough that a seam crossing cannot hide between two reads. +DEFAULT_SWEEP_INTERVAL: float = 0.05 + +#: Ticks of slack allowed between the position predicted after a re-zero and the +#: one actually observed. The joint is limp while this is measured, so gravity, +#: backlash and a nudged cable all move it a little between the pre-write read +#: and the post-write read. Generous on purpose: this check exists to catch an +#: offset that did nothing (delta ~1073) or went the wrong way, not to police +#: encoder jitter. +POSITION_TOLERANCE: int = 30 + +#: Verdicts a :class:`SweepReport` can return. Deliberately four, not two — +#: "did not fail" is not the same claim as "proved the fix works", and a verb +#: that conflated them would report a pass for a sweep of a joint that was never +#: re-zeroed, or one the human barely moved. +VERDICT_SEAM_EVICTED = "seam-evicted" +VERDICT_SEAM_NOT_EVICTED = "seam-not-evicted" +VERDICT_SEAM_PRESENT_BASELINE = "seam-present-baseline" +VERDICT_INCONCLUSIVE = "inconclusive" + + +# --------------------------------------------------------------------------- +# Eligibility — answered with no hardware attached +# --------------------------------------------------------------------------- + + +def require_rezeroable(joint: str) -> "tuple[int, arm_spec.UnreachableArc]": + """Return *joint*'s ``(offset, arc)``, or raise explaining why there isn't one. + + Called FIRST by the CLI verb — before consent, before a port is resolved, + before a bus is opened — so that ``arm rezero wrist_roll`` answers the + question on a laptop with no arm plugged in. "Why can't I re-zero this + joint?" is a question about the arm's geometry, not about the servo in front + of you, and it deserves an answer that does not depend on one being there. + + Returning both halves together is deliberate: every caller that wants the + offset also wants the arc it was derived from (to check the joint is not + somewhere it cannot be, to size the sweep, to render the plan), and handing + them back as a pair means no caller has to re-look-up an + ``Optional[UnreachableArc]`` it has just proved is not ``None``. + + Parameters + ---------- + joint: + One of the six joint names in :data:`arm101.hardware.arm_spec.JOINTS`. + + Returns + ------- + tuple[int, UnreachableArc] + The signed encoder offset that evicts *joint*'s seam (``+1073`` for + ``elbow_flex``, the only re-zeroable joint on this arm), and the + unreachable arc it was derived from. + + Raises + ------ + CliError(EXIT_USER_ERROR) + If *joint* is not a joint name at all, or is a joint that cannot (or + need not) be re-zeroed. The message is + :func:`~arm101.hardware.arm_spec.rezero_refusal`'s — which distinguishes + "impossible" (``wrist_roll``: no unreachable arc exists, so no offset can + evict its seam; a soft limit handles it) from "unnecessary" (the other + four: their encoders do not wrap inside their travel at all). + """ + try: + offset = arm_spec.rezero_offset(joint) + arc = arm_spec.rezero_arc(joint) + except ValueError as exc: + raise CliError( + code=EXIT_USER_ERROR, + message=str(exc), + remediation=f"Valid joints: {', '.join(arm_spec.JOINTS)}.", + ) from exc + + if offset is None or arc is None: + refusal = arm_spec.rezero_refusal(joint) + raise CliError( + code=EXIT_USER_ERROR, + message=f"{joint} is not re-zeroable.\n\n{refusal}", + remediation=( + "Only elbow_flex wraps inside its travel, and only elbow_flex can be " + "re-zeroed: run 'arm101 arm rezero elbow_flex'. Inspect any joint's live " + "encoder offset (read-only) with 'arm101 arm read --json'." + ), + ) + return offset, arc + + +# --------------------------------------------------------------------------- +# The plan — read where the joint IS, decide nothing about where it should GO +# --------------------------------------------------------------------------- + + +@dataclass(frozen=True) +class RezeroPlan: + """Everything the write will do, derived from live reads and the arc table. + + Built by :func:`plan_rezero` from **reads only**. It is the answer to "what + is about to happen to this servo", and it is computed before anything + happens to it. + + Attributes + ---------- + joint, motor: + Which joint, and the servo id carrying it. + current_offset: + The signed offset the servo holds RIGHT NOW (0 on a factory servo). + target_offset: + The signed offset about to be written — derived from the joint's + unreachable arc, never typed. + reported_position: + What the servo reports today, i.e. already corrected by + *current_offset*. + raw_position: + Where the shaft physically is, in the encoder's own frame: + ``(reported + current_offset) mod 4096``. On a factory servo + (*current_offset* == 0) this is an identity and carries no assumption + at all — which is the case the whole procedure is designed around. + predicted_position: + What the servo will report once the write lands, IF the corrected + position is reduced modulo 4096: ``(raw - target) mod 4096``. This is + the first, cheapest test of the open question — see + :func:`describe_shift`. + already_applied: + The servo already holds *target_offset*. The write is a no-op; say so + rather than performing it again. + """ + + joint: str + motor: int + current_offset: int + target_offset: int + reported_position: int + raw_position: int + predicted_position: int + already_applied: bool + + def as_dict(self) -> dict[str, object]: + """JSON-serialisable form, for a verb's ``--json`` payload.""" + return { + "joint": self.joint, + "motor": self.motor, + "current_offset": self.current_offset, + "target_offset": self.target_offset, + "reported_position": self.reported_position, + "raw_position": self.raw_position, + "predicted_position": self.predicted_position, + "already_applied": self.already_applied, + } + + +def raw_from_reported(reported: int, offset: int) -> int: + """Recover the shaft's raw encoder count from what the servo REPORTS. + + ``Actual = (Present + Ofs) mod 4096`` — the inverse of the correction the + servo applies. With the factory offset of 0 this is the identity, and the + only case that matters for a first re-zero needs no inverse at all; the + function exists so that a servo which has ALREADY been re-zeroed can still + be located in the raw frame the arc table is written in (e.g. to re-run the + reachability checks, or to detect that the joint is somewhere it should not + physically be able to be). + """ + return (reported + offset) % arm_spec.ENCODER_TICKS + + +def plan_rezero(bus: "MotorBus", motor: int, joint: str) -> RezeroPlan: + """Read the joint's live state and work out exactly what the re-zero will write. + + **Reads only.** No torque write, no goal, no EEPROM. Two guards make this + more than a formatting exercise, and both refuse rather than guess: + + *An unknown frame.* If the servo already holds an offset that is neither the + factory ``0`` nor the target, we do not know what frame its reported + positions are in, so we cannot honestly convert them to raw ticks and cannot + honestly check anything below. Writing a new offset on top of an unknown one + would bury the problem instead of surfacing it. + + *A physically impossible position.* If the joint's raw position lands + strictly inside the arc it supposedly cannot reach, then either the arc is + wrong or this servo is not the joint we think it is. Either way the offset + about to be written is derived from a table that does not describe the + hardware in front of us, and writing it would put the seam somewhere the + joint CAN go — making issue #35 worse, not better, and doing it persistently + in EEPROM. + + Raises + ------ + CliError(EXIT_USER_ERROR) + If *joint* is not re-zeroable (see :func:`require_rezeroable`). + CliError(EXIT_ENV_ERROR) + If the servo holds an unrecognised offset, or reports a raw position + inside its own unreachable arc. + """ + target, arc = require_rezeroable(joint) + + current = bus.read_offset(motor) + reported = bus.read_position(motor) + + if current not in (0, target): + raise CliError( + code=EXIT_ENV_ERROR, + message=( + f"{joint} (motor {motor}) already holds an encoder offset of {current}, " + f"which is neither the factory 0 nor this joint's computed {target}. " + "Its reported positions are in a frame this tool did not set and cannot " + "interpret, so it will not write a new offset on top of it." + ), + remediation=( + "Inspect the live offset with 'arm101 arm read --json' (the 'offset' " + "column). A re-zero must start from a known frame: restore the servo's " + "offset register (EEPROM addr 31) to 0 — or to this joint's computed " + f"{target} — and re-run. If {current} was deliberate, the arc table in " + "arm101/hardware/arm_spec.py is what needs updating, not the servo." + ), + ) + + raw = raw_from_reported(reported, current) + + if arc.contains(raw): + raise CliError( + code=EXIT_ENV_ERROR, + message=( + f"{joint} (motor {motor}) reports raw encoder position {raw}, which is " + f"INSIDE the arc it is supposed to be physically unable to reach " + f"({arc.low}, {arc.high}). The joint cannot be where it says it is, so the " + "arc — and the offset derived from it — does not describe this hardware. " + "Refusing to write." + ), + remediation=( + "Check that motor " + f"{motor} really is {joint} ('arm101 arm read'), and that the arm is " + "assembled as the arc was measured on. If the joint's travel has genuinely " + "changed, re-measure its walls and correct REZERO_ARCS in " + "arm101/hardware/arm_spec.py — the offset is derived from that table, so " + "correcting the table corrects the offset." + ), + ) + + return RezeroPlan( + joint=joint, + motor=motor, + current_offset=current, + target_offset=target, + reported_position=reported, + raw_position=raw, + predicted_position=(raw - target) % arm_spec.ENCODER_TICKS, + already_applied=current == target, + ) + + +# --------------------------------------------------------------------------- +# The write — an EEPROM write, and NOT a move +# --------------------------------------------------------------------------- + + +def apply_rezero(bus: "MotorBus", motor: int, offset: int) -> int: + """Write *offset* to *motor*'s EEPROM and read it back. Commands NO motion. + + The whole write, in two lines, and both of them matter: + + 1. :meth:`~arm101.hardware.bus.MotorBus.clear_overload` — disables torque, + tolerating (and clearing) a latched overload. Not optional and not + merely defensive: ``write_offset``'s own first act is a plain + ``enable_torque(motor, False)``, which a servo latched in overload + answers with the overload bit still set — so the write would raise + :class:`~arm101.hardware.bus.OverloadError` before it ever opened the + EEPROM. A joint that has just been driven into a wall (which is how + ``elbow_flex``'s arc was measured in the first place) is exactly the + joint you are then asked to re-zero. + 2. :meth:`~arm101.hardware.bus.MotorBus.write_offset` — torque-off, unlock + (addr 55 -> 0), addr 31, re-lock. That primitive owns the Lock dance + (without it the write reads back fine and silently REVERTS on the next + power-cycle — PR #21), the sign-magnitude encoding, and the range check. + + What is NOT here is the point: **no ``write_goal_position``, at any stage.** + The joint is not driven anywhere before, during, or after. See the module + docstring — a linear command issued while the axis is still non-linear + rotates ``elbow_flex`` the long way round, through its whole travel, into a + wall. + + Returns + ------- + int + The offset read back from EEPROM. The caller MUST check it equals + *offset*: the read-back is what proves the write landed. (It does not + prove the write PERSISTS — only a power-cycle does — and it does not + prove the seam MOVED — only :func:`sweep` does.) + + Raises + ------ + CliError(EXIT_USER_ERROR) + If *offset* is outside the register's ``[-2047, +2047]`` — raised by + ``encode_offset`` before any wire traffic, so the joint is not even + de-energised by a rejected call. + CliError(EXIT_ENV_ERROR) + If the bus is not open or any write fails. + """ + bus.clear_overload(motor) + bus.write_offset(motor, offset) + return bus.read_offset(motor) + + +def describe_shift(plan: RezeroPlan, observed: int) -> dict[str, object]: + """Compare the position observed after the write against the one predicted. + + The cheapest possible probe of the open question, taken for free the moment + the write lands — and worth taking, because two of its outcomes are already + damning without waiting for a sweep: + + * ``observed`` equals the pre-write reading: the offset changed nothing + about what the servo reports. The register took the value (the read-back + says so) and the firmware ignored it. + * ``observed`` is outside ``[0, 4095]``: the corrected position is a plain + signed subtraction — the seam is pinned where it always was, and the + re-zero cannot work. + + It is a probe, not a proof. A reading consistent with the prediction shows + the offset is *applied*, in the *modular* sense, at *one* point. That the + seam actually MOVED — that the joint's whole travel is now continuous — is a + statement about every point in the travel, and only :func:`sweep` can make + it. + + Returns a dict for the verb's payload: the predicted and observed positions, + their difference, whether they agree within :data:`POSITION_TOLERANCE`, + whether the reading is even in range, and whether the report moved at all. + """ + delta = observed - plan.predicted_position + return { + "predicted_position": plan.predicted_position, + "observed_position": observed, + "delta": delta, + "as_predicted": abs(delta) <= POSITION_TOLERANCE, + "in_range": arm_spec.TICK_MIN <= observed <= arm_spec.TICK_MAX, + "unchanged": abs(observed - plan.reported_position) <= POSITION_TOLERANCE, + } + + +# --------------------------------------------------------------------------- +# The sweep — the only thing that can prove the seam MOVED +# --------------------------------------------------------------------------- + + +@dataclass(frozen=True) +class SweepReport: + """What a torque-off, hand-driven sweep of one joint actually measured. + + Reports what it saw, then draws a conclusion — and keeps the two apart. The + measurements (:attr:`samples`, :attr:`minimum`, :attr:`maximum`, + :attr:`largest_jump`) are facts. The :attr:`verdict` is an interpretation, + and it is allowed to be "I cannot tell" — see :attr:`conclusive`. + + Attributes + ---------- + joint, motor: + What was swept. + offset_in_force, expected_offset: + The offset the servo actually held during the sweep, and the one that + evicts this joint's seam. When they differ the sweep is a **baseline**, + not a proof — a perfectly useful thing to run (it SHOWS you the seam + before you fix it), but not the same claim. + samples: + Every position read, in order. The measurement record; every number + below is derived from it. + minimum, maximum, span: + The extent the joint was actually moved through. ``span`` is what + :attr:`conclusive` is judged on — and, incidentally, the first + measurement anyone has ever made of ``elbow_flex``'s far wall. + monotonic: + The reported position never both rose and fell by more than + :data:`REVERSAL_TOLERANCE`. **Descriptive, not decisive**: a human hand + that backs up mid-sweep makes this ``False`` without anything being + wrong. The verdict turns on :attr:`continuous`, which a hand cannot fake + either way. + largest_jump, largest_jump_at: + The biggest single-sample change, and the sample index it happened at. + A seam crossing is ~1949-4095 ticks; sensor noise and a human hand are + tens of ticks. The gap is not subtle. + discontinuities: + ``(index, before, after)`` for every jump at or above + :data:`DISCONTINUITY_TICKS`. Non-empty means the seam is still inside + the joint's travel. + out_of_range: + Any sample outside ``[0, 4095]``. A position register cannot hold such + a value — seeing one means the corrected position is an unwrapped signed + subtraction, which independently proves the re-zero cannot work. + expected_travel: + The joint's travel in ticks, from its unreachable arc — the yardstick + :attr:`conclusive` measures :attr:`span` against. + """ + + joint: str + motor: int + offset_in_force: int + expected_offset: int + samples: tuple[int, ...] + minimum: int + maximum: int + monotonic: bool + largest_jump: int + largest_jump_at: int + discontinuities: tuple[tuple[int, int, int], ...] + out_of_range: tuple[int, ...] + expected_travel: int + + @property + def span(self) -> int: + """Ticks between the extremes reached — how far the joint was actually moved.""" + return self.maximum - self.minimum + + @property + def rezeroed(self) -> bool: + """The joint was carrying the seam-evicting offset while it was swept.""" + return self.offset_in_force == self.expected_offset + + @property + def continuous(self) -> bool: + """No discontinuity and no impossible reading — the seam was never crossed.""" + return not self.discontinuities and not self.out_of_range + + @property + def conclusive(self) -> bool: + """The sweep covered enough travel for its answer to mean anything. + + A sweep that barely moved the joint and saw no seam has proved nothing — + of course it saw no seam, it never went near where the seam would be. + Requires the span to reach :data:`MIN_COVERAGE` of the joint's expected + travel. A DISCONTINUOUS sweep is always conclusive, whatever its span: + seeing the seam is proof it is there, and no amount of extra travel can + un-see it. + """ + if not self.continuous: + return True + return self.span >= self.expected_travel * MIN_COVERAGE + + @property + def seam_evicted(self) -> bool: + """The one claim the whole plan hangs on, and it is only true three ways at once. + + The joint was re-zeroed, the sweep found no discontinuity anywhere, and + the sweep actually covered the travel. Drop any one and the claim is not + earned. + """ + return self.rezeroed and self.continuous and self.conclusive + + @property + def verdict(self) -> str: + """One of the four :data:`VERDICT_SEAM_EVICTED` constants. + + * :data:`VERDICT_SEAM_EVICTED` — re-zeroed, continuous, and covered. The + fix works; issue #35 is settled on hardware. + * :data:`VERDICT_SEAM_NOT_EVICTED` — re-zeroed and STILL discontinuous. + **The stop condition.** The corrected position is not modularly + reduced; the re-zero achieves nothing; the plan goes back to the user. + * :data:`VERDICT_SEAM_PRESENT_BASELINE` — not re-zeroed, discontinuous. + Exactly what an un-fixed ``elbow_flex`` should look like: this is the + bug, photographed. Useful, expected, not a failure. + * :data:`VERDICT_INCONCLUSIVE` — continuous, but either the joint was + not re-zeroed (so there was no seam to evict and nothing was tested) + or the sweep did not cover enough travel to have met one. + """ + if not self.continuous: + return VERDICT_SEAM_NOT_EVICTED if self.rezeroed else VERDICT_SEAM_PRESENT_BASELINE + if self.seam_evicted: + return VERDICT_SEAM_EVICTED + return VERDICT_INCONCLUSIVE + + @property + def failed(self) -> bool: + """``True`` iff this sweep is the STOP condition — a re-zero that did nothing.""" + return self.verdict == VERDICT_SEAM_NOT_EVICTED + + def describe(self) -> str: + """A verdict a human standing at the arm can act on, without a legend.""" + headline = { + VERDICT_SEAM_EVICTED: ( + "PASS — the seam is GONE from this joint's travel. The sweep ran " + f"{self.span} ticks with no discontinuity: the corrected position IS " + "reduced modulo 4096, the re-zero works, and issue #35 is fixed for " + f"{self.joint}." + ), + VERDICT_SEAM_NOT_EVICTED: ( + "*** STOP — THE RE-ZERO DID NOT WORK. ***\n\n" + f"{self.joint} carries the seam-evicting offset ({self.offset_in_force}) " + "and its reported position STILL jumps discontinuously mid-travel " + f"(largest jump: {self.largest_jump} ticks, at sample " + f"{self.largest_jump_at}). The servo therefore does NOT reduce the " + "corrected position modulo 4096 — it reports a plain signed subtraction, " + "so the offset only RELABELS positions and the discontinuity stays pinned " + "to the physical angle where the magnet rolls over.\n\n" + "The re-zero cannot fix issue #35. Do not build anything on top of it. " + "This needs a decision from the user, not a workaround." + ), + VERDICT_SEAM_PRESENT_BASELINE: ( + f"BASELINE — the seam is present, as expected. {self.joint} is not " + f"re-zeroed (offset in force: {self.offset_in_force}) and its position " + f"jumps {self.largest_jump} ticks at sample {self.largest_jump_at}. That " + "jump IS issue #35. Write the offset " + f"('arm101 arm rezero {self.joint} --apply'), power-cycle, and sweep again " + "— the jump should be gone." + ), + VERDICT_INCONCLUSIVE: ( + "INCONCLUSIVE — this sweep proves nothing." + self._why_inconclusive() + ), + }[self.verdict] + + lines = [ + headline, + "", + f"- joint : {self.joint} (motor {self.motor})", + f"- offset in force : {self.offset_in_force}" + f" (seam-evicting offset for this joint: {self.expected_offset})", + f"- samples : {len(self.samples)}", + f"- range reached : {self.minimum} .. {self.maximum} (span {self.span} ticks" + f", expected travel ~{self.expected_travel})", + f"- monotonic : {self.monotonic}", + f"- largest jump : {self.largest_jump} ticks (at sample {self.largest_jump_at})", + f"- discontinuities : {len(self.discontinuities)}" + f" (threshold {DISCONTINUITY_TICKS} ticks)", + ] + if self.out_of_range: + lines.append( + f"- IMPOSSIBLE READS : {len(self.out_of_range)} sample(s) outside [0, 4095]" + f" (e.g. {self.out_of_range[0]}) — the position register cannot hold these," + " so the corrected position is an UNWRAPPED signed subtraction." + ) + if self.rezeroed and self.continuous and self.conclusive: + lines += [ + "", + f"Far wall measured for the first time: {self.joint}'s travel spans " + f"{self.span} ticks ({self.minimum} .. {self.maximum} in the corrected " + "frame). The arc table in arm101/hardware/arm_spec.py was built on a " + f"LOWER BOUND of {self.expected_travel}; this is the real number.", + ] + return "\n".join(lines) + + def _why_inconclusive(self) -> str: + """Spell out which of the two ways this sweep failed to test anything.""" + if not self.rezeroed: + return ( + f" {self.joint} was NOT re-zeroed during it (offset in force: " + f"{self.offset_in_force}, expected {self.expected_offset}) and no " + "discontinuity was seen — so either the sweep never reached the seam, or " + "this joint does not wrap where we think it does. It does NOT show the " + "re-zero working, because no re-zero was in force." + ) + return ( + f" The joint moved only {self.span} ticks of its ~{self.expected_travel}-tick " + f"travel ({MIN_COVERAGE:.0%} required). Seeing no seam across a fraction of the " + "travel proves nothing — the seam may simply be in the part you did not visit. " + "Move the joint from one hard stop ALL THE WAY to the other and sweep again." + ) + + def as_dict(self) -> dict[str, object]: + """JSON-serialisable form, for a verb's ``--json`` payload.""" + return { + "joint": self.joint, + "motor": self.motor, + "offset_in_force": self.offset_in_force, + "expected_offset": self.expected_offset, + "rezeroed": self.rezeroed, + "samples": len(self.samples), + "minimum": self.minimum, + "maximum": self.maximum, + "span": self.span, + "expected_travel": self.expected_travel, + "monotonic": self.monotonic, + "continuous": self.continuous, + "conclusive": self.conclusive, + "seam_evicted": self.seam_evicted, + "largest_jump": self.largest_jump, + "largest_jump_at": self.largest_jump_at, + "discontinuity_threshold": DISCONTINUITY_TICKS, + "discontinuities": [ + {"index": i, "before": before, "after": after} + for i, before, after in self.discontinuities + ], + "out_of_range": list(self.out_of_range), + "verdict": self.verdict, + "failed": self.failed, + } + + +def analyse_sweep( + positions: Sequence[int], + *, + joint: str, + motor: int, + offset_in_force: int, + expected_offset: int, + expected_travel: int, +) -> SweepReport: + """Turn a list of polled positions into a :class:`SweepReport`. Pure — no bus. + + Split out from :func:`sweep` so the *judgement* can be tested exhaustively + against hand-written position sequences (a clean sweep, a 4095->0 wrap, the + ~1949-tick masked-signed jump, negative readings, a two-sample nothing) + without a bus, a clock, or a servo anywhere near it. + + Raises + ------ + CliError(EXIT_ENV_ERROR) + If fewer than two positions were sampled. One sample has no deltas, so + it cannot be continuous OR discontinuous — there is no sweep to judge, + and returning a cheerful "no discontinuities found" for it would be the + exact false pass this module is built to prevent. + """ + if len(positions) < 2: + raise CliError( + code=EXIT_ENV_ERROR, + message=( + f"The sweep collected {len(positions)} position sample(s) — too few to " + "judge. Continuity is a statement about the change BETWEEN samples, so at " + "least two are needed before anything can be concluded." + ), + remediation=( + "Re-run with a longer --duration, and check the joint is answering: " + "'arm101 arm read'." + ), + ) + + samples = tuple(int(p) for p in positions) + deltas = [b - a for a, b in zip(samples, samples[1:])] + + rose = any(d > REVERSAL_TOLERANCE for d in deltas) + fell = any(d < -REVERSAL_TOLERANCE for d in deltas) + + largest_jump_at = max(range(len(deltas)), key=lambda i: abs(deltas[i])) + discontinuities = tuple( + (i, samples[i], samples[i + 1]) + for i, d in enumerate(deltas) + if abs(d) >= DISCONTINUITY_TICKS + ) + out_of_range = tuple(p for p in samples if not (arm_spec.TICK_MIN <= p <= arm_spec.TICK_MAX)) + + return SweepReport( + joint=joint, + motor=motor, + offset_in_force=offset_in_force, + expected_offset=expected_offset, + samples=samples, + minimum=min(samples), + maximum=max(samples), + monotonic=not (rose and fell), + largest_jump=abs(deltas[largest_jump_at]), + largest_jump_at=largest_jump_at, + discontinuities=discontinuities, + out_of_range=out_of_range, + expected_travel=expected_travel, + ) + + +def _needs_pacing(bus: "MotorBus") -> bool: + """Does this bus need real time to pass between samples? + + A physical servo does — the human's hand advances it by wall-clock, so the + loop must wait between reads or it samples the same tick six hundred times. + A simulated bus advances *per read*, so pacing it would only make the suite + sleep. Same seam, same reasoning, as + :func:`arm101.hardware.gentle._needs_pacing`. + """ + return not isinstance(bus, FakeBus) + + +def sweep( + bus: "MotorBus", + motor: int, + joint: str, + *, + samples: int, + interval: float = DEFAULT_SWEEP_INTERVAL, + on_sample: Optional[Callable[[int, int], None]] = None, +) -> SweepReport: + """De-energise *motor* and poll its position while a HUMAN hand-moves the joint. + + The proof step, and the only one that can settle the open question. It + commands nothing: the joint goes limp and the operator walks it from one + hard stop to the other while this watches. That is not a fallback for + missing automation — it is the *right* instrument, because a human arm is + the only actuator available that does not need a linear tick axis to work, + and a linear tick axis is precisely what is in doubt. + + Torque is disabled first, via + :meth:`~arm101.hardware.bus.MotorBus.clear_overload` (which also clears a + latch left by a previous run), and is **never re-enabled** — the verb + deliberately ends with the joint limp, because the operator's hand is on it. + The caller wraps this in a + :func:`~arm101.hardware.safety.torque_guard` anyway, so an abnormal exit + mid-sweep cannot leave a motor hot. + + Parameters + ---------- + bus: + An open :class:`~arm101.hardware.bus.MotorBus`. + motor, joint: + The servo id, and the joint name it carries (used to look up the + expected offset and travel). + samples: + How many positions to poll. With *interval*, this sets the wall-clock + length of the sweep. + interval: + Seconds between polls on real hardware. Skipped entirely for a + :class:`~arm101.hardware.bus.FakeBus` (see :func:`_needs_pacing`), whose + simulated shaft advances per read rather than per second. + on_sample: + Optional ``(index, position)`` callback, invoked after every poll — the + verb uses it to show the operator the position moving in real time, so + they can see they are actually driving the joint and not merely holding + it. + + Raises + ------ + CliError(EXIT_USER_ERROR) + If *samples* is less than 2 (see :func:`analyse_sweep`). + CliError(EXIT_ENV_ERROR) + If the bus is not open or a read fails. + """ + if samples < 2: + raise CliError( + code=EXIT_USER_ERROR, + message=f"A sweep needs at least 2 samples to have any deltas, got {samples}.", + remediation="Increase the sweep duration so it collects at least two samples.", + ) + + expected_offset, arc = require_rezeroable(joint) + + # Limp FIRST. A joint the human is about to hand-move must not be holding — + # and one that fought a wall on the way here may be latched in overload, in + # which case a plain enable_torque(False) would raise instead of releasing. + bus.clear_overload(motor) + + # Read the offset AFTER de-energising and BEFORE the first sample, so the + # report says which frame the samples below are actually in — rather than + # assuming the one we hoped for. + offset_in_force = bus.read_offset(motor) + + pace = interval if _needs_pacing(bus) else 0.0 + positions: list[int] = [] + for index in range(samples): + position = bus.read_position(motor) + positions.append(position) + if on_sample is not None: + on_sample(index, position) + if pace: + time.sleep(pace) + + return analyse_sweep( + positions, + joint=joint, + motor=motor, + offset_in_force=offset_in_force, + expected_offset=expected_offset, + expected_travel=arc.travel_ticks, + ) + + +def samples_for(duration: float, interval: float = DEFAULT_SWEEP_INTERVAL) -> int: + """How many polls fit in *duration* seconds at *interval* seconds apart. + + Raises + ------ + CliError(EXIT_USER_ERROR) + If *duration* is not long enough to collect two samples — the minimum + for any delta to exist at all. + """ + count = int(duration / interval) + if count < 2: + raise CliError( + code=EXIT_USER_ERROR, + message=( + f"A sweep of {duration}s at {interval}s per sample collects {count} " + "sample(s) — too few to detect a discontinuity, which is a change BETWEEN " + "two samples." + ), + remediation=( + f"Pass a longer --duration (at least {2 * interval:.2f}s; the default " + f"{DEFAULT_SWEEP_DURATION:.0f}s is sized for a human to walk the joint " + "from one hard stop to the other without hurrying)." + ), + ) + return count diff --git a/docs/hardware-rezero-procedure.md b/docs/hardware-rezero-procedure.md new file mode 100644 index 0000000..51f429d --- /dev/null +++ b/docs/hardware-rezero-procedure.md @@ -0,0 +1,321 @@ +# Hardware procedure — re-zeroing `elbow_flex`'s encoder (issue #35) + +**Who this is for:** a human standing at the follower arm, with one hand free +for the joint and one for the keyboard. It should be followable start to finish +without reading anything else. + +**What you are doing:** writing one number into one servo's EEPROM, so that the +encoder's 4095→0 **seam** falls in the arc `elbow_flex` physically cannot reach, +instead of in the middle of its travel. Then *proving* it worked — which takes +your hand, not the motor. + +**How long:** about 15 minutes, most of it the two sweeps. + +--- + +## Before you start + +| Thing | Value | +| --- | --- | +| Arm | follower | +| Port | `/dev/ttyACM1` | +| Joint | `elbow_flex` | +| Motor id | **3** | +| Register | `Ofs` / `Homing_Offset` — EEPROM **addr 31**, 2 bytes | +| Offset to be written | **+1073** (wire value `1073`) | +| Where the seam ends up | raw tick 1073 — dead centre of the arc `(126, 2020)` the joint cannot reach, with 947 ticks of clearance on each side | + +You do **not** type any of those numbers. They come from +`arm101/hardware/arm_spec.py` (`REZERO_ARCS`), and the offset is *derived* from +the measured arc rather than typed, so it cannot drift away from it. + +Two things to have ready: + +- **A way to cut and restore power to the servo bus** — not just unplugging USB. + Step 5 needs a real power-cycle, and there is no way around it (see + [Why the power-cycle is not optional](#why-the-power-cycle-is-not-optional)). +- **A hand on the elbow.** Steps 3 and 6 de-energise the joint. If the arm is + holding a pose it **will sag** when torque drops. Support it. + +### Sanity check the arm is alive + +```bash +uv run arm101 arm read +``` + +Every joint should read `ok`. Note `elbow_flex`'s `position` and `offset` +columns — the `offset` should be **0** on a servo that has never been re-zeroed. +If it is not 0, stop and read [If something looks +wrong](#if-something-looks-wrong). + +--- + +## Step 1 — Look at the plan (writes nothing, opens no bus) + +```bash +uv run arm101 arm rezero elbow_flex +``` + +This is a dry-run. It prints the exact register writes it *would* perform: + +```text +write1ByteTxRx(addr=40, value=0) # Torque_Enable OFF +write1ByteTxRx(addr=55, value=0) # Lock OPEN +write2ByteTxRx(addr=31, value=1073) # Ofs/Homing_Offset = +1073 +write1ByteTxRx(addr=55, value=1) # Lock CLOSED +(no goal position is ever written — this verb commands NO motion) +``` + +Read that last line twice. **This procedure never commands the joint to move.** +That is not a simplification, it is the safety property — see +[Why nothing here moves the joint](#why-nothing-here-moves-the-joint). + +`arm101 arm rezero wrist_roll` will refuse and explain why (it cannot be +re-zeroed at all, and that is permanent, not a missing feature). Try it if you +want to understand the shape of the problem. + +--- + +## Step 2 — (Optional but recommended) Photograph the bug first + +Sweep the joint **before** you change anything. This shows you the seam with +your own eyes, so that when it is gone in step 6 you know it was actually you +who moved it. + +```bash +uv run arm101 arm rezero elbow_flex --verify --apply +``` + +Follow the on-screen instructions (they are the same as +[step 6](#step-6--prove-the-seam-actually-moved)). Expect the verdict +**`seam-present-baseline`** and a jump of roughly **4000 ticks** somewhere in the +middle of the travel. That jump *is* issue #35. The command exits 0 — a baseline +is not a failure. + +--- + +## Step 3 — Write the offset + +Support the elbow. Torque is about to drop. + +```bash +uv run arm101 arm rezero elbow_flex --apply +``` + +At a terminal it prompts; type `yes`. What it does, in order: + +1. Reads where the joint physically is, and checks that position is somewhere it + is actually *able* to be. (If not, it refuses — the table would not describe + your arm.) +2. Clears any latched overload and **disables torque**. A servo must not be + *holding* while its own frame of reference moves underneath it. +3. Opens the EEPROM lock, writes **addr 31 = 1073**, closes the lock. +4. Reads the offset back and reports it. + +You should see: + +```text +- offset written : +1073 (wire value 1073, EEPROM addr 31) +- offset read back : 1073 <- the write LANDED +- reported before : 126 +- reported after : 3149 (predicted 3149, delta 0) +``` + +The `reported after` value is the first evidence: the servo's own report jumped +from ~126 to ~3149, which is `(126 − 1073) mod 4096`. That is the modular +correction working. + +**If you see a WARNING here** — the position did not change, or it went negative +— the offset was applied but the firmware is not using it the way we need. +Continue to step 6 anyway; the sweep is what settles it, and its verdict is the +one that counts. + +--- + +## Step 4 — Confirm the joint did not move + +Look at the arm. `elbow_flex` should be exactly where it was. + +Nothing commanded it to move, and nothing should have. The *reported number* +changed by 3023 ticks; the *physical joint* changed by nothing. If the joint +moved, stop and report it — something is commanding motion that should not be. + +--- + +## Step 5 — POWER-CYCLE the servo + +**Cut power to the servo bus and restore it.** Not the USB cable — the *bus +power*. Then re-read: + +```bash +uv run arm101 arm read --json +``` + +`elbow_flex`'s `offset` must **still be 1073**. + +If it reverted to **0**, the write did not persist, and the EEPROM Lock dance +failed. Stop and see [If something looks wrong](#if-something-looks-wrong). + +### Why the power-cycle is not optional + +PR #21 of this repo exists because exactly this went wrong before. Servo `id` and +`baudrate` writes *appeared* to work — they read back correctly, every time — and +then silently **reverted on the next power-up**, because the EEPROM Lock register +(addr 55) had not been opened around the write. On this firmware a write while +`Lock=1` updates the *live* register but is never committed to EEPROM. + +`addr 31` sits in the same EEPROM region and fails the same way. The read-back in +step 3 proves the servo **accepted** the value. Only the power-cycle proves it +**kept** it. These are different claims, and only one of them survives the night. + +--- + +## Step 6 — Prove the seam actually moved + +This is the step the whole exercise exists for. **Do not skip it, and do not +substitute step 3's read-back for it.** + +Reading the offset back proves it was *applied*. It does **not** prove the seam +*moved*. Only a sweep does — see [The one thing nobody +knows](#the-one-thing-nobody-knows). + +Support the elbow. Torque is about to drop and **stay** dropped. + +```bash +uv run arm101 arm rezero elbow_flex --verify --apply +``` + +Then, for the 30 seconds it gives you: + +1. **Put one hand on the elbow link.** It is limp. +2. **Move it slowly, all the way to one hard stop.** Gently — you are looking for + the wall, not fighting it. +3. **Move it slowly, all the way back to the other hard stop.** *This is the part + that matters.* You must traverse the **entire** travel — the whole way, end to + end. A sweep that covers only part of the travel proves nothing, and the tool + will tell you so rather than pretend otherwise. +4. **Do not hurry.** Hurrying is how a seam crossing hides between two samples. + +You will see the position ticking past on screen as you move it. That feed is +there so you can tell "the tool is watching me" from "the tool has wedged and I +am wobbling a dead arm". + +### Reading the verdict + +| Verdict | Exit | What it means | +| --- | --- | --- | +| **`seam-evicted`** | 0 | **Done.** The joint's reported position climbed steadily across its whole travel with no jump. The seam is out of the travel, the tick axis is linear, and issue #35 is fixed. | +| **`seam-not-evicted`** | 2 | **STOP.** See below. | +| `inconclusive` | 0 | You did not move the joint through enough of its travel (or the offset was not in force). Nothing was proved either way. Just do it again, and go end to end. | +| `seam-present-baseline` | 0 | The joint was not re-zeroed when you swept it. If you got this *after* step 5, the offset did not persist — go back to step 5. | + +The passing report also tells you something nobody has ever known: **the real +span of `elbow_flex`'s travel.** Its far wall has never been measured, because +nothing could see across the seam. The arc table's 2202 ticks is a *lower bound*; +your sweep's `span` is the real number. Write it down. + +### If the verdict is `seam-not-evicted` + +**Stop. Do not work around this, and do not build anything on top of it.** + +The offset is in force, and the joint's reported position *still* jumps +discontinuously in the middle of its travel. That means the servo does **not** +reduce the corrected position modulo 4096 — it reports a plain signed +subtraction, so the offset only *relabels* positions and the seam stays pinned to +the physical angle where the magnet rolls over. + +The re-zero cannot fix issue #35. The command exits 2 and says so. Take the +report to the user: this needs a decision (a software soft limit, as `wrist_roll` +uses, or unwrapping the encoder in software), not a retry. + +--- + +## Background + +### Why nothing here moves the joint + +**The tool that makes the tick axis linear cannot itself rely on the tick axis +being linear.** + +The natural procedure would be "drive the joint to mid-travel, then centre it +there". It is exactly the procedure that must not run. `elbow_flex` currently +rests at raw tick **~126** — which is *past* its wrap. A goal of 3121 (its +mid-travel) looks like a modest move in tick-space, and is in fact a rotation +**the long way round**: from 126 down through 0, across the whole 1894-tick arc +the joint cannot reach, and into a wall. The commanded number is sane; the +physical consequence is not — and that gap *is* the bug being fixed. + +So the verb reads where the joint physically **is**, computes the offset from the +joint's known unreachable arc, and writes it. No goal position is written at any +point, on any path. The only thing that moves the joint in this entire procedure +is **your hand**, in step 6 — and a human arm is the right instrument precisely +because it is the one actuator in the building that does not need a linear tick +axis to work. + +### The one thing nobody knows + +One bit of firmware behaviour decides whether this whole fix works, and Feetech +have never documented it: + +```text +Present = (raw − Ofs) mod 4096 the seam RELOCATES → the fix works +Present = raw − Ofs (signed) the seam STAYS → the fix does NOTHING +``` + +Every source we could find — Feetech's own SDK, LeRobot, and LeRobot's shipped +SO-101 calibration procedure, which is *literally this fix* — implies the first. +None of them states it. The evidence is strong and entirely circumstantial. + +That is why step 6 exists, and why it cannot be replaced by a read-back. Your +sweep is the experiment that settles it. Full write-up: +`docs/spikes/sts3215-offset-register.md`, section 4. + +### Why `wrist_roll` is not on this list + +A re-zero only ever **relocates** a seam. It can never **evict** one. + +Eviction needs an arc the joint physically cannot reach — somewhere to put the +seam where the joint will never follow it. `elbow_flex` has one: real mechanical +walls, and a 1894-tick arc between them that it cannot enter. `wrist_roll` has +none: exploration drove it right around and found **no wall anywhere** (measured +free range `[21, 4073]`). Every angle is reachable, including whichever one you +move the seam to. + +So `wrist_roll` gets a **soft limit** instead — a software-only travel +restriction that carves out a dead arc the joint is simply never *commanded* +into, and puts the seam in there. That is already in force +(`arm_spec.SOFT_LIMITS`). It is a different fix for a genuinely different +problem, and no amount of re-zeroing would have helped. + +--- + +## If something looks wrong + +| Symptom | What it means | What to do | +| --- | --- | --- | +| `arm read` shows `offset` ≠ 0 before you start | The servo already holds an offset. If it is **1073**, you have already done this — skip to step 5 or 6. | If it is anything else, the verb will refuse: its reported positions are in a frame this tool did not set and cannot interpret. Restore addr 31 to 0 and start again. | +| `"reports raw encoder position N, which is INSIDE the arc"` | The joint says it is somewhere it should be physically unable to be. The arc table does not describe this arm — wrong motor, or the travel has genuinely changed. | Check motor 3 really is `elbow_flex` (`arm read`). Do **not** override it. If the travel changed, re-measure the walls and correct `REZERO_ARCS` in `arm101/hardware/arm_spec.py`; the offset is derived from that table. | +| `"The encoder offset did NOT take"` | The servo accepted the write and is not holding the value. | The Lock register (addr 55) is being re-closed by something else, or motor 3 is not the servo you think it is. This is PR #21's failure mode. | +| Offset reverted to 0 after the power-cycle | The write did not persist — the EEPROM Lock dance failed. | Re-run step 3. If it reverts again, that is a real bug in the Lock handling; capture it and report it. | +| The arm sagged when you ran step 3 or 6 | Working as designed. Both steps de-energise the joint deliberately. | Support the arm before running them. | +| The sweep says `inconclusive` | You did not move the joint far enough — over 80% of the travel is required before "no seam" means anything. | Run it again and go from one hard stop **all the way** to the other. | + +--- + +## What to record + +When you are done, note these in the run log — the first three are numbers +nobody has: + +- The verdict, and the **span** the sweep measured. This is `elbow_flex`'s real + travel, and the first measurement of its far wall. +- The `minimum` and `maximum` reported positions across the sweep. Together with + the span they give the joint's true reachable interval in the *corrected* + frame — which is now a single interval, which is the whole point. +- The **largest single-sample jump**. On a pass this should be tens of ticks + (your hand). Anything in the thousands is the seam. +- Whether the offset survived the power-cycle. + +If the verdict was `seam-evicted`, the arc table in `arm101/hardware/arm_spec.py` +can now be corrected from a lower bound to a measured fact — and the offset, +being derived from it, will follow automatically. diff --git a/docs/spikes/sts3215-offset-register.md b/docs/spikes/sts3215-offset-register.md new file mode 100644 index 0000000..1920d21 --- /dev/null +++ b/docs/spikes/sts3215-offset-register.md @@ -0,0 +1,375 @@ +# STS3215 offset register — spike (issue #35, task t5) + +**VERDICT: GO-WITH-CAVEAT.** + +The mechanism exists and is exactly what we hoped for: a **2-byte, EEPROM-backed, +sign-magnitude offset register at address 31** (`Ofs` / `Homing_Offset`), applied by +the servo to the position it *reports*. Its ±2047 range is roughly **twice** what +`elbow_flex` needs (we need ≈1073). Feetech's own SDK, Feetech's own Python library, +and LeRobot — which drives this exact arm — all agree on the facts below. + +**But one load-bearing assumption is undocumented and must be proven on hardware +before t7 writes anything:** that the corrected `Present_Position` is reduced +**modulo 4096** (so the seam *relocates* to `raw == Ofs`), rather than reported as a +plain signed subtraction (which would leave the discontinuity pinned at the physical +raw 4095→0 point and make the re-zero **useless**). Every source is consistent with +the modular reading and LeRobot's shipped SO-101 calibration silently depends on it, +but no primary source states the firmware's formula. See +[The caveat](#4-the-caveat--what-must-be-proven-on-hardware) and +[the hardware test](#5-the-hardware-persistence--seam-eviction-test). + +This is a spike, not a build. Nothing was written to any servo; no Python file was +touched. + +--- + +## 1. Does an offset mechanism exist? + +Yes — and there are **two** of them. + +`arm101/hardware/bus.py` implements neither. Confirmed by reading it: the only +addresses it knows are 0, 1, 3 (read-only ident), 5 (ID), 6 (Baud_Rate), 9/11 +(Min/Max_Angle_Limit), 40 (Torque_Enable), 41 (Acceleration), 42 (Goal_Position), +46 (Goal_Speed), 48 (Torque_Limit), 55 (Lock), 56 (Present_Position), 58/60/62/63 +(present speed/load/voltage/temperature). **Address 31 appears nowhere in the +codebase.** + +### Mechanism A — write the offset register directly (address 31) + +The one t7 should build on: it gives arbitrary, exact control over where the seam +lands. + +### Mechanism B — firmware midpoint calibration (write `128` to address 40) + +Feetech's SDK exposes `CalibrationOfs(id)`, which is literally one byte: + +```cpp +int SMS_STS::CalibrationOfs(u8 ID) +{ + return writeByte(ID, SMS_STS_TORQUE_ENABLE, SMS_STS_CALIBRATION_CMD); +} +// #define SMS_STS_TORQUE_ENABLE 40 +// #define SMS_STS_CALIBRATION_CMD 128 // Command value for midpoint calibration +``` + +The firmware reads the current physical position, computes the offset that makes it +the centre, and commits that to EEPROM itself. Feetech's own example documents the +procedure: *"Physically position servo shaft to desired center/zero point → run this +program → CalibrationOfs() reads current position and calculates offset → Offset is +written to EEPROM (persists across power cycles) → All future position commands are +relative to this new center."* + +Mechanism B is a genuinely attractive **fallback**, because it sidesteps a +chicken-and-egg problem: it needs no wrap-aware move and no arithmetic — a human +parks the joint by hand (torque off) at mid-travel and sends one byte. Its downsides +are that it always centres on 2048 (no control over the exact offset) and that +whether its firmware-internal EEPROM commit honours the Lock register is **untested**. + +Sources: [`SMS_STS.h`][smsh] (Feetech memory table + `SMS_STS_CALIBRATION_CMD`), +[`SMS_STS.cpp`][smscpp] (`CalibrationOfs` body), +[`CalibrationOfs.cpp` example][smsex]. + +--- + +## 2. Register facts (every fact cited) + +| Fact | Value | Source | +| --- | --- | --- | +| Name | `Ofs` (Feetech) / `Homing_Offset` (LeRobot) | [`SMS_STS.h`][smsh], [LeRobot `tables.py`][lrtables] | +| Address | **31** (`OFS_L`), 32 (`OFS_H`) — one 2-byte register at 31 | `#define SMS_STS_OFS_L 31` / `SMS_STS_OFS_H 32` ([`SMS_STS.h`][smsh]); `SMS_STS_OFS_L = 31` ([Feetech `sms_sts.py`][ftpy]); `"Homing_Offset": (31, 2)` ([LeRobot][lrtables]) | +| Width | **2 bytes** | as above (L/H pair; LeRobot's `(31, 2)`) | +| Encoding | **SIGN-MAGNITUDE, sign bit = bit 11.** Wire value = `(sign << 11) \| magnitude`. **NOT two's complement.** | `"Homing_Offset": 11` in `STS_SMS_SERIES_ENCODINGS_TABLE` ([LeRobot][lrtables]) + `encode_sign_magnitude` ([LeRobot][lrenc]) | +| Usable range | **−2047 … +2047** | `max_magnitude = (1 << sign_bit_index) - 1` = `(1 << 11) - 1` = **2047** ([LeRobot][lrenc]); confirmed empirically by a real SO-101 failure: `ValueError: Magnitude 2073 exceeds 2047 (max for sign_bit_index=11)` ([LeRobot issue #3193][lr3193]) | +| **EEPROM or RAM** | **EEPROM — persistent.** It sits under the `//-------EEPROM (Read and Write)--------` heading; the `//-------SRAM (Read and Write)--------` region does not begin until address 40. | [`SMS_STS.h`][smsh] | +| Lock (addr 55) | **Unlock → write → re-lock required.** Same EEPROM region as ID (5) and Baud_Rate (6) — the exact registers PR #21 proved revert on power-cycle when Lock is left closed. | [`SMS_STS.h`][smsh]; LeRobot's `disable_torque` writes `Torque_Enable=0` **and `Lock=0`**, `enable_torque` writes `Torque_Enable=1` **and `Lock=1`**, and `write_calibration` (which writes `Homing_Offset`) runs between them ([LeRobot `feetech.py`][lrfeetech]) | +| **Effect on `Present_Position` (addr 56)** | **YES — the servo applies it to what it reports.** `Present_Position = Actual_Position − Homing_Offset` | LeRobot `_get_half_turn_homings` docstring ([LeRobot `feetech.py`][lrfeetech]); independently, an unrelated STS3215 spec memo: *"補正後の現在位置 = 生のエンコーダ値 − Homing_Offset"* (corrected present position = raw encoder value − Homing_Offset) ([Zenn][zenn]) | + +Two further facts worth carrying into t7: + +- **`Present_Position` is itself sign-magnitude on bit 15.** Feetech's `ReadPos` + decodes it as `scs_tohost(value, 15)` ([Feetech `sms_sts.py`][ftpy]); LeRobot lists + `"Present_Position": 15` in its encodings table ([LeRobot][lrtables]). Our + `bus.py:read_position` currently does `int(value) & 0x0FFF`, which discards that + sign bit. In servo mode with limits 0/4095 that mask is harmless, but it is exactly + the mask that would *hide* a negative reading — see the caveat. +- **The SDK this repo already depends on ships the codec but no register table.** + `scservo_sdk` (`feetech-servo-sdk` 1.0.0) contains `SCS_TOHOST(a, b)` / + `SCS_TOSCS(a, b)` in `scservo_def.py` — a sign-magnitude codec where bit `b` is the + sign — but defines **no control-table constants at all**. So t7 must supply address + 31 itself; there is nothing to import. + +--- + +## 3. Range arithmetic for `elbow_flex`, step by step + +**Measured facts** (follower on `/dev/ttyACM1`, 2026-07-12 — from +`arm-explore-follower.map.json` and issue #35): + +- Hard wall (driving *decreasing*) at raw **2020**. +- Recorded contiguous, wrap-free region: **[2040, 4060]** (span 2020 ticks — this is + the "~2020-tick span", and it is a *lower bound* on true travel: the map note says + it deliberately stops short of the wrap). +- Driven *increasing*, it crossed the 4095→0 seam and read back as **~1**. +- It currently **rests at raw ~126** — *past* its wrap (issue #35: *"elbow_flex is + currently resting at ~126, i.e. past its wrap. A linear command will rotate it the + long way round."*). + +**Step 1 — reconstruct the true travel arc in raw ticks.** Going up from the wall: + +```text +2020 → 4095 →│seam│→ 0 → ~126 +``` + +**Step 2 — arc length.** + +```text +travel = (4096 − 2020) + 126 + = 2076 + 126 + = 2202 ticks (a LOWER BOUND — the far wall was never measured) +``` + +**Step 3 — the unreachable arc**, which is where we must evict the seam to: + +```text +unreachable = 4096 − 2202 = 1894 ticks, spanning raw (126, 2020) +``` + +**Step 4 — where the seam lands.** With `Present = (raw − H) mod 4096`, the reported +value wraps 4095→0 exactly where `raw == H`. So we need: + +```text +H ∈ (126, 2020) +``` + +**Step 5 — pick the midpoint for maximum margin.** + +```text +H* = (126 + 2020) / 2 = 1073 +``` + +**Step 6 — does it fit in ±2047?** + +```text +|H*| = 1073 ≤ 2047 ✓ head-room = 2047 − 1073 = 974 ticks +``` + +**Yes — with ~2× margin.** The answer to "if the offset is limited to ±2047, can it +move the seam clear of a ~2020-tick travel?" is **comfortably yes**, because the +required offset is bounded by the *unreachable* arc's position (≈1073), not by the +travel's length. + +**Step 7 — verify the result is monotonic.** With `H = 1073`: + +| raw | reported = `(raw − 1073) mod 4096` | +| --- | --- | +| 2020 (wall) | **947** | +| 4060 | 2987 | +| 4095 | 3022 | +| 0 | 3023 | +| 126 (rest) | **3149** | + +Reported values along the travel: `947 → 2987 → 3022 → 3023 → 3149` — **strictly +increasing, no discontinuity.** The reachable set becomes the single interval +**[947, 3149]**, which a `(min, max)` pair *can* honestly describe. The seam sits at +raw 1073, dead centre of the 1894-tick unreachable arc, with **947 ticks of clearance +on each side**. + +**Step 8 — cross-check against LeRobot's rule.** LeRobot's `set_half_turn_homings` +computes `H = pos − 2047` with the joint parked at mid-range. Mid-travel here is +`2020 + 2202/2 = 3121`, giving `H = 3121 − 2047 = 1074` — within **one tick** of the +1073 derived independently above. Two different routes, same answer, both far inside +±2047. + +**Generality of the ±2047 bound.** Sign-magnitude on bit 11 yields +`H ∈ {−2047 … +2047}`. Modulo 4096 that covers **every residue except 2048** +(since `−2047 ≡ 2049`, and neither `+2048` nor `−2048` is representable). So there is +exactly *one* seam placement the encoding cannot express — raw 2048 — and it is not +one we need. Our required window `(126, 2020)` lies wholly inside `0…2047`. + +**Why this cannot rescue `wrist_roll`** (handled separately, not this task): its +travel covers the whole circle, so the unreachable arc is *empty* and step 3 yields +nothing to evict the seam into. No choice of `H` can remove a seam from a travel that +includes every angle. That is a soft-limit problem, not a re-zero problem. + +--- + +## 4. The caveat — what must be proven on hardware + +Steps 4 and 7 above both assume: + +```text +Present_Position = (raw − Ofs) mod 4096 ← seam MOVES to raw == Ofs ✅ fix works +``` + +The alternative is: + +```text +Present_Position = raw − Ofs (plain signed) ← seam STAYS at raw 4095→0 ❌ fix is useless +``` + +Under the second reading the offset merely *relabels* positions: the discontinuity +remains pinned to the physical angle where the magnet's raw count rolls over, and +re-zeroing achieves **nothing**. This is not a paranoid hypothetical — `Present_Position` +really is decoded as sign-magnitude on bit 15 by both Feetech's SDK and LeRobot, which +means the register *can* carry negative values, which is precisely what an unwrapped +signed subtraction would produce. + +**Evidence for the modular (correct-for-us) reading — strong, but circumstantial:** + +- LeRobot's SO-101 calibration procedure is *literally the fix we are proposing*: + *"First you need to move the robot to the position where all joints are in the middle + of their ranges"*, then it writes homing offsets ([LeRobot SO-101 docs][lr101]). +- The community framing of the identical bug is that centring **prevents** the wrap: + *"The wrist axis uses almost the entire motor rotation, so if it's not properly + centered, you may encounter encoder overflow / underflow"* ([LeRobot issue + #3193][lr3193]). Centring can only prevent overflow if the offset **relocates** the + seam. +- Feetech's own example: *"All future position commands are relative to this new + center"* ([`CalibrationOfs.cpp`][smsex]). + +**Evidence I could not obtain:** any primary Feetech statement of the firmware's +actual formula, or of whether the corrected value is reduced mod 4096 in servo mode. +Waveshare's ST3215 wiki (which publishes the full memory table with min/max columns) +returned HTTP 403, and the Feetech PDF was not machine-readable. + +Because the entire re-zero rests on this one bit of semantics, it is a **caveat, not a +GO**. It is cheap to settle — step 10 of the test below settles it in about two +minutes with the torque off. + +--- + +## 5. The hardware persistence + seam-eviction test + +Run on the follower (`/dev/ttyACM1`), `elbow_flex` = **motor id 3**. PR #21 exists +precisely because an EEPROM write *appeared* to work and then silently reverted on +power-cycle, so persistence is proven by power-cycling, not by reading back. + +Encoding reminder: for a **positive** `H`, the wire value is just `H` (sign bit +clear). For a **negative** `H` it is `(1 << 11) | abs(H)` = `2048 + abs(H)`. For +`H = 1073` the wire value is simply **1073**. + +1. **Baseline.** Read addr 31 (2 bytes) on motor 3 → record it (expect `0` from + factory). Read addr 56 → record (expect ~126, the rest position). +2. **Torque off.** Write addr 40 = `0`. *(Do not write the offset with torque on — + the servo would instantly re-interpret its own position and could lurch toward its + goal. This is inferred, not documented; treat it as a hard rule.)* +3. **Unlock EEPROM.** Write addr 55 = `0` (this repo's `_set_lock(motor, False)`). +4. **Write the offset.** `write2ByteTxRx(id=3, addr=31, value=1073)`. +5. **Re-lock.** Write addr 55 = `1`. +6. **Read back addr 31** → must be `1073`. *(Proves the write landed.)* +7. **Read addr 56** → must now read **~3149**, i.e. shifted by −1073 mod 4096 from the + ~126 baseline. It must **not** still read ~126 (offset ignored) and must **not** + read negative / ~64589 (unwrapped signed — see the caveat). +8. **POWER-CYCLE the servo.** Cut and restore *bus power* — not merely close and + reopen the serial port. +9. **Re-read addr 31** → must **still** be `1073`. If it reverted to `0`, the Lock + dance failed exactly as id/baud did before PR #21. Re-read addr 56 → still ~3149. +10. **The seam-eviction proof (settles the caveat).** With torque **off**, hand-move + `elbow_flex` slowly through its **entire** travel, from the wall to the far stop, + polling addr 56 throughout. The reported value must climb **monotonically** from + ~947 to ~3149 with **no 4095→0 jump anywhere**. If a jump appears, the corrected + position is not modularly reduced, the re-zero does **not** solve issue #35, and + wave 2a must **STOP** and return to the user. + - Bonus, free: this sweep also measures `elbow_flex`'s **far wall** for the first + time (see unknown 5 below). + +**Do not skip step 10 for step 7.** Step 7 shows the offset is *applied*; only step 10 +shows the seam actually *moved*, and that is the thing the whole plan depends on. + +--- + +## 6. What I could NOT determine + +Stated plainly — these are the point of the spike. + +1. **Whether the corrected `Present_Position` is reduced modulo 4096** (seam moves) or + reported as an unwrapped signed value (seam stays). *The decisive unknown.* All + circumstantial evidence points to modular; no primary source states it. Settled by + test step 10. +2. **Whether `Ofs` is applied to `Goal_Position` (addr 42) as well as to + `Present_Position`.** If the servo corrected what it *reports* but not what it + *accepts*, our commands and its feedback would live in different frames and the + re-zero would be worse than useless. Strong inference that both are corrected — + LeRobot commands goals in the corrected frame on this exact arm and it works — but + I found no primary statement. A move commanded after step 6 that lands where + expected would confirm it. +3. **Feetech's own documented min/max for register 31.** The ±2047 bound is derived + from the bit-11 sign-magnitude encoding (LeRobot) and confirmed empirically by the + `ValueError` in issue #3193 — but I never saw the datasheet's own min/max column. + Waveshare's ST3215 wiki (which has it) 403'd. The bound is well-attested; its + *provenance* is second-hand. +4. **Whether Mechanism B (`128` → addr 40) needs the Lock open to persist.** Feetech's + own example does no unlock/relock and still claims EEPROM persistence, which + suggests the firmware commits it internally — but PR #21 is a standing warning + about exactly this class of assumption. +5. **`elbow_flex`'s far wall.** We know the travel reaches *at least* raw ~126; the + upper mechanical stop has never been measured, because `arm explore` cannot see + across the seam. So the 2202-tick travel is a **lower bound** and the 1894-tick + unreachable arc is an **upper bound**. This does not threaten the verdict (the + re-zero works for any travel < 4096, and 2202 ≪ 4096) but `H` should be re-derived + once the far wall is known. Test step 10 measures it as a side-effect. +6. **Whether the STS3215 requires torque off for EEPROM writes in general.** Assumed + (and made a hard rule above) rather than confirmed. + +--- + +## 7. Guidance for t7, if the hardware test passes + +- Write **only** address 31. **Do not** copy LeRobot's `write_calibration`, which also + writes `Min_Position_Limit` (9) and `Max_Position_Limit` (11): in servo mode the + firmware clamps goals to that window, and our factory values are the wide-open + 0/4095 that we *want* to keep. Narrowing them would clamp the reachable set we are + trying to recover. +- Reuse the existing `_set_lock` unlock→write→relock pattern from + `bus.py:write_id_baudrate` verbatim, including its best-effort re-lock on the + failure path — address 31 is in the same EEPROM region and carries the same + power-cycle-revert hazard. +- `read_offset` / `write_offset` must encode/decode **sign-magnitude on bit 11**, not + two's complement. Guard the magnitude: `abs(H) > 2047` must raise a `CliError`, not + silently corrupt bit 11 into the sign. +- Note that `bus.py:read_position` masks `& 0x0FFF`. Revisit that mask deliberately: + it is what would silently swallow a negative reading if unknown 1 resolves the wrong + way. + +--- + +## Sources + +- [`SMS_STS.h`][smsh] — Feetech SMS/STS memory table: `SMS_STS_OFS_L 31` / + `SMS_STS_OFS_H 32` under the **EEPROM (Read and Write)** heading; SRAM starts at 40; + `SMS_STS_LOCK 55`; `SMS_STS_CALIBRATION_CMD 128`. +- [`SMS_STS.cpp`][smscpp] — `CalibrationOfs`, `unLockEeprom`, `LockEeprom` bodies. +- [`CalibrationOfs.cpp`][smsex] — Feetech's official midpoint-calibration example and + its EEPROM-persistence claim. +- [Feetech `sms_sts.py`][ftpy] — Feetech's official Python SDK: `SMS_STS_OFS_L = 31`, + `SMS_STS_LOCK = 55`, `ReadPos` decoding via `scs_tohost(value, 15)`. +- [LeRobot `feetech/tables.py`][lrtables] — `"Homing_Offset": (31, 2)`; + `STS_SMS_SERIES_ENCODINGS_TABLE = {"Present_Load": 10, "Homing_Offset": 11, + "Goal_Position": 15, …, "Present_Position": 15}`. +- [LeRobot `feetech/feetech.py`][lrfeetech] — `Present_Position = Actual_Position − + Homing_Offset`; `enable_torque`/`disable_torque` writing `Lock` 1/0 around the + calibration write. +- [LeRobot `encoding_utils.py`][lrenc] — `encode_sign_magnitude`: + `max_magnitude = (1 << sign_bit_index) - 1`. +- [LeRobot issue #3193][lr3193] — real SO-101 `wrist_roll` failure: + `ValueError: Magnitude 2073 exceeds 2047 (max for sign_bit_index=11)`; encoder + overflow/underflow when not centred. +- [LeRobot SO-101 docs][lr101] — calibration procedure: park all joints mid-range, + then write homing offsets. +- [STS3215 spec memo (Zenn, ja)][zenn] — independent confirmation: + corrected present position = raw encoder value − `Homing_Offset`; address 31 in + EEPROM. +- `arm101/hardware/bus.py` (this repo) — confirmed: no offset register implemented. +- `arm-explore-follower.map.json` + issue #35 (this repo) — the measured `elbow_flex` + numbers used in the arithmetic. + +[smsh]: https://github.com/adityakamath/SCServo_Linux/blob/main/include/scservo/SMS_STS.h +[smscpp]: https://github.com/adityakamath/SCServo_Linux/blob/main/src/SMS_STS.cpp +[smsex]: https://github.com/adityakamath/SCServo_Linux/blob/main/examples/SMS_STS/CalibrationOfs/CalibrationOfs.cpp +[ftpy]: https://github.com/ftservo/FTServo_Python/blob/main/scservo_sdk/sms_sts.py +[lrtables]: https://github.com/huggingface/lerobot/blob/main/src/lerobot/motors/feetech/tables.py +[lrfeetech]: https://github.com/huggingface/lerobot/blob/main/src/lerobot/motors/feetech/feetech.py +[lrenc]: https://github.com/huggingface/lerobot/blob/main/src/lerobot/motors/encoding_utils.py +[lr3193]: https://github.com/huggingface/lerobot/issues/3193 +[lr101]: https://huggingface.co/docs/lerobot/so101 +[zenn]: https://zenn.dev/usagi1975/articles/2026-05-16-000_sts3215-spec diff --git a/pyproject.toml b/pyproject.toml index 19b08b5..fb926be 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,6 +1,6 @@ [project] name = "arm101-cli" -version = "0.20.1" +version = "0.21.0" description = "Agent and CLI for controlling SO-ARM101 robotic arm grippers" readme = "README.md" license = "MIT" diff --git a/tests/_fakes.py b/tests/_fakes.py index adc3cbc..8e46726 100644 --- a/tests/_fakes.py +++ b/tests/_fakes.py @@ -258,9 +258,18 @@ def place_obstacle(self, motor: int, position: int) -> "ServoModelBus": # ------------------------------------------------------------------ def write_goal_position(self, motor: int, position: int) -> None: - """Record a GOAL. The servo does not move — that takes polls (time).""" + """Record a GOAL. The servo does not move — that takes polls (time). + + *position* arrives in the **corrected frame** — the same frame the servo + reports in — so it is converted back to a raw encoder count for the + simulation, which runs on actual shaft positions + (``Actual = Present + Homing_Offset``). Goals and feedback living in + different frames is precisely the failure that would make an encoder + re-zero worse than useless, so the fake refuses to model it. At the + default zero offset this is the identity and nothing changes. + """ super().write_goal_position(motor, position) - self._goals[motor] = position + self._goals[motor] = self._actual_position(motor, position) def read_info(self, motor: int) -> dict: """Advance the simulation one poll interval, then report what it reads. @@ -268,15 +277,23 @@ def read_info(self, motor: int) -> dict: The advance happens *after* ``super().read_info`` so that an armed overload seam (which raises from there) leaves the simulated shaft untouched: a read that failed observed nothing. + + The simulated position is in raw encoder counts, so it goes back out + through the same offset funnel every other reported position uses + (:meth:`~arm101.hardware.bus.FakeBus._reported_position`). Skipping that + would make ``read_info``'s ``present_position`` disagree with + ``read_position`` — two views of ONE register (addr 56) — and a test + could then pass against a servo that cannot exist. """ snapshot = super().read_info(motor) position, load, state = self._poll(motor) - snapshot["present_position"] = position + reported = self._reported_position(motor, position) + snapshot["present_position"] = reported snapshot["present_load"] = load self.poll_log.append( { "motor": motor, - "present_position": position, + "present_position": reported, "present_load": load, "state": state, } diff --git a/tests/test_arm_read.py b/tests/test_arm_read.py index aa76cd9..2515c50 100644 --- a/tests/test_arm_read.py +++ b/tests/test_arm_read.py @@ -236,11 +236,65 @@ def test_read_arm_empty_joints_returns_empty_list(): def test_joint_reading_is_a_dataclass_with_expected_fields(): - """JointReading exposes joint, motor_id, health, and the six register fields.""" + """JointReading exposes joint, motor_id, health, and the register fields.""" r = JointReading(joint="gripper", motor_id=6, health="ok") assert r.joint == "gripper" assert r.motor_id == 6 assert r.health == "ok" - for field in ("position", "load", "speed", "voltage", "temperature", "torque"): + for field in ("position", "load", "speed", "voltage", "temperature", "torque", "offset"): assert hasattr(r, field) assert getattr(r, field) is None + + +# --------------------------------------------------------------------------- +# Encoder offset (Ofs / Homing_Offset, addr 31) — read-only visibility (#35) +# --------------------------------------------------------------------------- + + +def test_read_arm_exposes_the_encoder_offset_signed(): + """Each joint carries its ``Homing_Offset``, decoded to a signed int. + + Issue #35: ``elbow_flex``'s encoder wraps mid-travel, and the fix is to + re-zero it by writing addr 31. Before anyone writes that register a human + must be able to SEE what it currently holds — and see it as ``-1073``, not + as the raw sign-magnitude wire value ``3121``, which would read as a + plausible positive offset. + """ + bus = FakeBus(positions={i: 1000 + i for i in range(1, 7)}, offsets={3: -1073, 5: 400}) + bus.open() + + by_joint = {r.joint: r for r in read_arm(bus, JOINTS)} + + assert by_joint["elbow_flex"].offset == -1073 + assert by_joint["wrist_roll"].offset == 400 + assert by_joint["shoulder_pan"].offset == 0 # factory default + + +def test_read_arm_failed_joint_has_no_offset(): + """A joint that never answered reports ``offset=None``, not a misleading 0. + + A hard 0 would say "this motor has no offset" — a claim the snapshot has no + evidence for. + """ + bus = FlakyBus( + positions={i: 1000 + i for i in range(1, 7)}, + offsets={3: -1073}, + fail_counts={3: float("inf")}, + ) + bus.open() + + by_joint = {r.joint: r for r in read_arm(bus, JOINTS, retries=2)} + + assert by_joint["elbow_flex"].health == "failed" + assert by_joint["elbow_flex"].offset is None + + +def test_read_arm_writes_nothing_while_reading_the_offset(): + """``arm read`` stays read-only: surfacing addr 31 must not write addr 31.""" + bus = FakeBus(positions={i: 2048 for i in range(1, 7)}, offsets={3: 1073}) + bus.open() + + read_arm(bus, JOINTS) + + assert bus.register_writes == [] + assert bus.offset_writes == [] diff --git a/tests/test_arm_read_flex.py b/tests/test_arm_read_flex.py index 781f403..d8230ad 100644 --- a/tests/test_arm_read_flex.py +++ b/tests/test_arm_read_flex.py @@ -209,6 +209,49 @@ def _open(port: str) -> FakeBus: assert seen["port"] == "/dev/ttyACM7" +# --------------------------------------------------------------------------- +# arm read — the encoder offset (Ofs / Homing_Offset, addr 31) is INSPECTABLE +# --------------------------------------------------------------------------- + + +def test_read_json_exposes_the_encoder_offset(monkeypatch, capsys) -> None: + """Issue #35: a human must be able to see addr 31 before anyone writes it. + + Reported signed (-1073), never as the raw sign-magnitude wire value (3121). + """ + fake = FakeBus(positions={i: 1000 + i for i in range(1, 7)}, offsets={3: -1073}) + _patch_bus(monkeypatch, fake) + + arm_cmd.cmd_arm_read(_read_args(json_mode=True)) + + payload = json.loads(capsys.readouterr().out) + by = {j["joint"]: j for j in payload["joints"]} + assert by["elbow_flex"]["offset"] == -1073 + assert by["shoulder_pan"]["offset"] == 0 + + +def test_read_text_renders_an_offset_column(monkeypatch, capsys) -> None: + fake = FakeBus(positions={i: 1000 + i for i in range(1, 7)}, offsets={3: -1073}) + _patch_bus(monkeypatch, fake) + + arm_cmd.cmd_arm_read(_read_args()) + + out = capsys.readouterr().out + assert "offset" in out + assert "-1073" in out + + +def test_read_is_read_only_even_with_the_offset_surfaced(monkeypatch) -> None: + """Surfacing addr 31 must not write addr 31 — ``arm read`` has no consent gate.""" + fake = FakeBus(positions={i: 1000 + i for i in range(1, 7)}, offsets={3: -1073}) + _patch_bus(monkeypatch, fake) + + arm_cmd.cmd_arm_read(_read_args()) + + assert fake.register_writes == [] + assert fake.offset_writes == [] + + # =========================================================================== # arm flex — validation # =========================================================================== diff --git a/tests/test_arm_rezero_cli.py b/tests/test_arm_rezero_cli.py new file mode 100644 index 0000000..77e8fd2 --- /dev/null +++ b/tests/test_arm_rezero_cli.py @@ -0,0 +1,841 @@ +"""Tests for the ``arm rezero`` CLI verb — the gated EEPROM write for issue #35, +and the ``--verify`` sweep that proves it actually worked. + +All hardware is a :class:`~arm101.hardware.bus.FakeBus` injected through the +``arm._open_bus`` / ``arm._candidate_ports`` seam (the same seam ``read`` / +``flex`` / ``explore`` use), so no serial port is ever opened. + +Two properties are asserted over and over, because they are the two a hardware +run cannot forgive: + +1. **No motion, on any path.** ``elbow_flex`` rests at raw ~126, PAST its wrap, so + a linear goal would rotate it the long way round through its whole travel and + into a wall. The verb must never write a goal position and must never energise + the joint. +2. **``--verify`` fails under ``offset_wraps=False``.** That is the pessimistic + reading of the one undocumented firmware behaviour the re-zero rests on, and + under it the re-zero achieves nothing. The verb must say so, loudly, with a + non-zero exit — that failure is what stands between the operator and shipping + on top of a fix that did nothing. +""" + +from __future__ import annotations + +import argparse +import json +import sys + +import pytest + +from arm101.cli._commands import arm as arm_cmd +from arm101.cli._errors import EXIT_ENV_ERROR, EXIT_USER_ERROR, CliError +from arm101.hardware import rezero +from arm101.hardware.bus import ADDR_HOMING_OFFSET, FakeBus, OverloadError +from tests.test_rezero import ELBOW, ELBOW_MOTOR, EXPECTED_OFFSET, HandMovedBus + +#: STS3215 Goal_Position — the register this verb must never, ever write. +ADDR_GOAL_POSITION = 42 + + +# --------------------------------------------------------------------------- +# Helpers +# --------------------------------------------------------------------------- + + +class _FakeStdin: + """Scripted stdin controlling ``isatty()`` and ``readline()``.""" + + def __init__(self, lines: "list[str]", tty: bool = True) -> None: + self._lines = list(lines) + self._tty = tty + + def isatty(self) -> bool: + return self._tty + + def readline(self) -> str: + return self._lines.pop(0) if self._lines else "" + + +def _patch_bus(monkeypatch, fake: FakeBus, port: str = "/dev/ttyACM_fake") -> None: + fake.open() + monkeypatch.setattr(arm_cmd, "_candidate_ports", lambda: [port]) + monkeypatch.setattr(arm_cmd, "_open_bus", lambda _p: fake) + + +def _args( + joint: str = ELBOW, + verify: bool = False, + duration: "float | None" = None, + role: str = "follower", + port: "str | None" = None, + apply: bool = False, + json_mode: bool = False, +): + return argparse.Namespace( + joint=joint, + verify=verify, + duration=duration, + role=role, + port=port, + apply=apply, + json=json_mode, + ) + + +#: A sweep short enough to run instantly against a FakeBus (which needs no +#: pacing) but long enough to walk the hand-moved shaft across the seam: +#: 2.5s / 0.05s = 50 samples x 50 ticks = 2500 raw ticks, comfortably past the +#: 2202-tick travel. +SWEEP_SECONDS = 2.5 +SWEEP_STEP = 50 + + +def _swept_bus(*, offset_wraps: bool = True, rezeroed: bool = True) -> HandMovedBus: + bus = HandMovedBus( + ticks_per_read=SWEEP_STEP, + offsets={ELBOW_MOTOR: EXPECTED_OFFSET} if rezeroed else None, + offset_wraps=offset_wraps, + ) + return bus + + +# --------------------------------------------------------------------------- +# Registration +# --------------------------------------------------------------------------- + + +def test_rezero_is_registered_under_the_arm_noun(): + from arm101.cli import _build_parser + + args = _build_parser().parse_args(["arm", "rezero", ELBOW]) + assert args.func is arm_cmd.cmd_arm_rezero + assert args.joint == ELBOW + assert args.verify is False + assert args.apply is False + assert args.json is False + assert args.role == "follower" + + +def test_rezero_flags_parse(): + from arm101.cli import _build_parser + + args = _build_parser().parse_args( + ["arm", "rezero", ELBOW, "--verify", "--duration", "45", "--apply", "--json"] + ) + assert (args.verify, args.duration, args.apply, args.json) == (True, 45.0, True, True) + + +def test_arm_overview_lists_rezero(capsys): + arm_cmd.cmd_arm_overview(argparse.Namespace(json=True)) + payload = json.loads(capsys.readouterr().out) + assert "rezero" in payload["verbs"] + + +# --------------------------------------------------------------------------- +# Eligibility — answered with NO hardware attached +# --------------------------------------------------------------------------- + + +def test_wrist_roll_is_refused_with_the_reason_and_no_bus_is_opened(monkeypatch): + """ "Why can't I re-zero this joint?" is a question about the arm's GEOMETRY. + + It deserves an answer on a laptop, with no servo plugged in — so the + eligibility check runs before consent, before a port is resolved, and before + a bus is opened. If it needed hardware, the one operator who most needs the + explanation (the one who does not understand the arm yet) is the one least + able to get it. + """ + + def _explode(_port): # pragma: no cover - must never be reached + raise AssertionError("a bus was opened for a joint that cannot be re-zeroed") + + monkeypatch.setattr(arm_cmd, "_open_bus", _explode) + monkeypatch.setattr(arm_cmd, "_candidate_ports", lambda: []) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + with pytest.raises(CliError) as exc: + arm_cmd.cmd_arm_rezero(_args(joint="wrist_roll", apply=True)) + + assert exc.value.code == EXIT_USER_ERROR + assert "RELOCATES" in exc.value.message + assert "EVICT" in exc.value.message + assert "SOFT LIMIT" in exc.value.message + + +@pytest.mark.parametrize("joint", ["shoulder_pan", "shoulder_lift", "wrist_flex", "gripper"]) +def test_a_joint_that_never_wraps_is_refused_as_unnecessary(joint, monkeypatch): + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + with pytest.raises(CliError) as exc: + arm_cmd.cmd_arm_rezero(_args(joint=joint)) + assert exc.value.code == EXIT_USER_ERROR + assert "does not need a re-zero" in exc.value.message + + +def test_an_unknown_joint_is_a_user_error(monkeypatch): + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + with pytest.raises(CliError) as exc: + arm_cmd.cmd_arm_rezero(_args(joint="knee")) + assert exc.value.code == EXIT_USER_ERROR + + +# --------------------------------------------------------------------------- +# Consent gate — dry-run / TTY prompt / --apply +# --------------------------------------------------------------------------- + + +def test_dry_run_prints_the_exact_writes_and_opens_NO_bus(monkeypatch, capsys): + """Zero writes AND zero bus access — like ``flex``'s and ``explore``'s dry-runs. + + Everything a plan can honestly say about a re-zero is already known without a + servo (the offset is derived from the arc table, the wire value from the + offset). Everything it cannot say offline is a live fact, checked at apply + time where it can actually be acted on. + """ + + def _explode(_port): # pragma: no cover - must never be reached + raise AssertionError("dry-run opened a bus") + + monkeypatch.setattr(arm_cmd, "_open_bus", _explode) + monkeypatch.setattr(arm_cmd, "_candidate_ports", lambda: ["/dev/ttyACM_fake"]) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + arm_cmd.cmd_arm_rezero(_args()) + + out = capsys.readouterr().out + assert "Dry-run plan: arm rezero elbow_flex" in out + assert "addr=31, value=1073" in out # the exact wire value + assert "addr=55, value=0" in out and "addr=55, value=1" in out # the Lock dance + assert "COMMANDS NO MOTION" in out + assert "no goal position is ever written" in out + + +def test_dry_run_json_carries_the_whole_plan(monkeypatch, capsys): + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + monkeypatch.setattr(arm_cmd, "_candidate_ports", lambda: ["/dev/ttyACM_fake"]) + + arm_cmd.cmd_arm_rezero(_args(json_mode=True)) + + plan = json.loads(capsys.readouterr().out)["plan"] + assert plan["joint"] == ELBOW + assert plan["motor"] == ELBOW_MOTOR + assert plan["target_offset"] == EXPECTED_OFFSET + assert plan["wire_value"] == EXPECTED_OFFSET + assert plan["unreachable_arc"] == [126, 2020] + assert plan["seam_moves_to_raw_tick"] == EXPECTED_OFFSET + assert plan["mode"] == "write" + + +def test_verify_dry_run_warns_that_the_arm_will_SAG(monkeypatch, capsys): + """De-energising a joint that is holding a pose is a physical hazard in itself. + + ``--verify`` is not "read-only and therefore safe": it drops the torque and + leaves it dropped. An operator who does not know that is an operator standing + under a falling arm. + """ + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + monkeypatch.setattr(arm_cmd, "_candidate_ports", lambda: ["/dev/ttyACM_fake"]) + + arm_cmd.cmd_arm_rezero(_args(verify=True, json_mode=True)) + + plan = json.loads(capsys.readouterr().out)["plan"] + assert plan["mode"] == "verify" + assert "sag" in plan["note"] + assert "DE-ENERGISES" in plan["note"] + assert plan["writes"] == ["write1ByteTxRx(addr=40, value=0) # Torque_Enable OFF"] + + +def test_tty_prompt_confirms_before_the_write(monkeypatch, capsys): + bus = FakeBus(positions={ELBOW_MOTOR: 126}) + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin(["yes\n"], tty=True)) + + arm_cmd.cmd_arm_rezero(_args()) + + captured = capsys.readouterr() + assert "PERSISTENT" in captured.err + assert bus.offset_writes == [{"motor": ELBOW_MOTOR, "offset": EXPECTED_OFFSET}] + + +def test_tty_prompt_declined_writes_NOTHING(monkeypatch, capsys): + bus = FakeBus(positions={ELBOW_MOTOR: 126}) + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin(["no\n"], tty=True)) + + arm_cmd.cmd_arm_rezero(_args()) + + assert bus.register_writes == [] + assert "Aborted" in capsys.readouterr().out + + +def test_non_tty_without_apply_never_writes(monkeypatch): + bus = FakeBus(positions={ELBOW_MOTOR: 126}) + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + arm_cmd.cmd_arm_rezero(_args(apply=False)) + + assert bus.register_writes == [] + + +# --------------------------------------------------------------------------- +# The write path +# --------------------------------------------------------------------------- + + +def test_apply_writes_the_offset_and_reports_the_read_back(monkeypatch, capsys): + bus = FakeBus(positions={ELBOW_MOTOR: 126}) + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + arm_cmd.cmd_arm_rezero(_args(apply=True, json_mode=True)) + + payload = json.loads(capsys.readouterr().out) + assert payload["read_back_offset"] == EXPECTED_OFFSET + assert payload["applied"] is True + assert payload["plan"]["raw_position"] == 126 + assert payload["shift"]["observed_position"] == 3149 # the spike's prediction + assert payload["shift"]["as_predicted"] is True + # Applied is not persistent, and applied is not evicted. Neither is claimed. + assert payload["persistence_proven"] is False + assert payload["seam_eviction_proven"] is False + + +def test_the_write_path_COMMANDS_NO_MOTION(monkeypatch): + """The one thing a hardware run cannot forgive, pinned at the register level. + + ``elbow_flex`` rests PAST its wrap: any linear goal rotates it the long way + round, through its whole travel, into a wall. Asserted here on the CLI verb's + complete write surface — not just on the primitive it calls. + """ + bus = FakeBus(positions={ELBOW_MOTOR: 126}) + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + arm_cmd.cmd_arm_rezero(_args(apply=True)) + + assert bus.position_writes == [] + assert all(w["addr"] != ADDR_GOAL_POSITION for w in bus.register_writes) + assert bus.speed_writes == [] + assert bus.accel_writes == [] + # Torque was touched, and only ever downward. + assert bus.torque_writes and all(w["on"] is False for w in bus.torque_writes) + + +def test_the_result_TELLS_the_operator_to_power_cycle(monkeypatch, capsys): + """PR #21 exists because an EEPROM write read back fine and reverted anyway. + + The read-back proves the value was APPLIED. Only a power-cycle proves it + PERSISTS. A result that ended on a success line would let the operator walk + away with neither fact established. + """ + bus = FakeBus(positions={ELBOW_MOTOR: 126}) + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + arm_cmd.cmd_arm_rezero(_args(apply=True)) + + out = capsys.readouterr().out + assert "POWER-CYCLE" in out + assert "BUS POWER" in out + assert "--verify" in out + assert "does NOT prove the seam" in out + + +def test_a_second_run_on_an_already_re_zeroed_joint_writes_nothing(monkeypatch, capsys): + """The procedure sends the operator away to power-cycle. Coming back is normal.""" + bus = FakeBus(positions={ELBOW_MOTOR: 126}, offsets={ELBOW_MOTOR: EXPECTED_OFFSET}) + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + arm_cmd.cmd_arm_rezero(_args(apply=True, json_mode=True)) + + payload = json.loads(capsys.readouterr().out) + assert payload["written"] is False + assert payload["reason"] == "already-applied" + assert bus.register_writes == [] + + +def test_a_write_that_does_not_stick_is_an_ENV_error(monkeypatch, capsys): + """The servo took the packet and is not holding the value — that is PR #21's ghost.""" + + class _AmnesiacBus(FakeBus): + """Accepts the offset write, then reports the factory 0 forever after.""" + + def read_offset(self, motor: int) -> int: + return 0 + + bus = _AmnesiacBus(positions={ELBOW_MOTOR: 126}) + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + with pytest.raises(CliError) as exc: + arm_cmd.cmd_arm_rezero(_args(apply=True)) + + assert exc.value.code == EXIT_ENV_ERROR + assert "did NOT take" in exc.value.message + assert "Lock register" in exc.value.remediation + + +def test_the_write_path_WARNS_when_the_position_does_not_move_as_predicted(monkeypatch, capsys): + """The free, early probe: under the pessimistic firmware reading it fires at once. + + ``offset_wraps=False`` makes the servo report −947 from rest — a value the + position register cannot hold. The write itself still succeeded (it read + back), so this is a warning and not an error; the STOP verdict belongs to + ``--verify``. But the operator is told, 30 seconds before the sweep could + tell them. + """ + bus = FakeBus(positions={ELBOW_MOTOR: 126}, offset_wraps=False) + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + arm_cmd.cmd_arm_rezero(_args(apply=True)) + + out = capsys.readouterr().out + assert "WARNING" in out + assert "OUTSIDE [0, 4095]" in out + assert "UNWRAPPED signed subtraction" in out + + +def test_a_servo_holding_an_unknown_offset_is_refused(monkeypatch): + bus = FakeBus(positions={ELBOW_MOTOR: 126}, offsets={ELBOW_MOTOR: 777}) + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + with pytest.raises(CliError) as exc: + arm_cmd.cmd_arm_rezero(_args(apply=True)) + + assert exc.value.code == EXIT_ENV_ERROR + assert bus.offset_writes == [] + + +def test_a_joint_reporting_an_impossible_position_is_refused(monkeypatch): + bus = FakeBus(positions={ELBOW_MOTOR: 1500}) # inside its own unreachable arc + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + with pytest.raises(CliError) as exc: + arm_cmd.cmd_arm_rezero(_args(apply=True)) + + assert exc.value.code == EXIT_ENV_ERROR + assert "INSIDE the arc" in exc.value.message + assert bus.offset_writes == [] + + +# --------------------------------------------------------------------------- +# Overload recovery while PLANNING — a latched motor can still be re-zeroed +# --------------------------------------------------------------------------- +# +# ``plan_rezero`` is reads-only (``read_offset`` then ``read_position``), but a +# read is not exempt from the overload latch: ``FeetechBus._read_register`` +# raises through ``_status_error``, which returns ``OverloadError`` off the +# STATUS BYTE the servo replies with — not off which direction the packet that +# tripped it was. So a motor latched in overload fails ``plan_rezero`` exactly +# as it would fail a write, and the verb used to abort before it ever reached +# ``apply_rezero`` (the only call site that clears the latch). That is not a +# corner case: ``elbow_flex``'s unreachable arc was measured by driving the +# joint into a wall, which is precisely how a Feetech servo latches an +# overload — so an operator running ``arm rezero`` right after that +# measurement, exactly the order ``docs/hardware-rezero-procedure.md`` +# describes, hit this every single time. + + +class _ClearOverloadSpyBus(FakeBus): + """A :class:`FakeBus` that counts calls to :meth:`clear_overload`. + + Distinguishes the pre-existing ``clear_overload`` call inside + :func:`rezero.apply_rezero` (expected on every real write) from the NEW + one this fix adds around ``plan_rezero`` (expected ONLY when the plan + read actually raised ``OverloadError``). A plain write-writes count of + ``register_writes``/``torque_writes`` cannot tell those two apart; a call + counter on the method itself can. + """ + + def __init__(self, *args, **kwargs) -> None: + super().__init__(*args, **kwargs) + self.clear_overload_calls = 0 + + def clear_overload(self, motor: int) -> None: + self.clear_overload_calls += 1 + super().clear_overload(motor) + + +def test_a_latched_motor_can_still_be_re_zeroed(monkeypatch, capsys): + """THE test this bug exists for. + + ``fail_with_overload_on_op(1)`` makes the very FIRST bus operation raise + ``OverloadError`` — ``plan_rezero``'s own ``read_offset``, before this verb + has done anything else. Without the fix this aborts the whole verb with an + ``OverloadError`` and never reaches the write. With it: the latch is + cleared, the plan is re-read, and the offset is written exactly as it + would be on an un-latched motor. + """ + bus = FakeBus(positions={ELBOW_MOTOR: 126}).fail_with_overload_on_op(1) + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + arm_cmd.cmd_arm_rezero(_args(apply=True, json_mode=True)) + + payload = json.loads(capsys.readouterr().out) + assert payload["read_back_offset"] == EXPECTED_OFFSET + assert payload["applied"] is True + assert bus.offset_writes == [{"motor": ELBOW_MOTOR, "offset": EXPECTED_OFFSET}] + # The simulated latch was actually disarmed by clear_overload, not merely + # dodged — a real servo's latch does not clear itself. + assert bus.overload_after_ops is None + + +def test_the_operator_is_told_the_joint_went_limp_when_a_latch_is_cleared(monkeypatch, capsys): + """A joint going limp mid-command must never be a silent side effect. + + ``clear_overload`` disables torque as its mechanism for clearing the + latch — the operator standing at the arm needs to know THAT happened and + WHY, on stderr, before the write proceeds. + """ + bus = FakeBus(positions={ELBOW_MOTOR: 126}).fail_with_overload_on_op(1) + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + arm_cmd.cmd_arm_rezero(_args(apply=True, json_mode=True)) + + err = capsys.readouterr().err + assert "latched" in err.lower() + assert "LIMP" in err + assert ELBOW in err + + +def test_the_happy_path_issues_NO_recovery_clear_overload_call(monkeypatch): + """Pins "only recover when actually overloaded". + + A healthy, un-latched motor must plan on the FIRST read. If the fix + unconditionally cleared overload ahead of every plan — rather than only in + response to a caught ``OverloadError`` — a joint that was holding fine + would be silently de-energised for no reason connected to anything that + went wrong. ``apply_rezero`` itself calls ``clear_overload`` once, always + (that is pre-existing, correct behaviour, guarding the write itself) — + this asserts the total stays at exactly that one call, i.e. the new + recovery path around ``plan_rezero`` never fired. + """ + bus = _ClearOverloadSpyBus(positions={ELBOW_MOTOR: 126}) + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + arm_cmd.cmd_arm_rezero(_args(apply=True)) + + assert bus.clear_overload_calls == 1 # apply_rezero's own guard, and nothing more + + +def test_the_already_applied_noop_path_does_not_de_energise_the_joint(monkeypatch, capsys): + """A second run against an already-re-zeroed joint must leave it exactly alone. + + This is the path the procedure sends operators back to on purpose (see + ``test_a_second_run_on_an_already_re_zeroed_joint_writes_nothing``), and it + is the clearest possible case for why the overload recovery must be + conditional: nothing here is latched, nothing is written, and NOTHING + should touch torque — a joint mid-way through being held in position by + an operator must not go limp because a re-zero happened to be re-run + against it. + """ + bus = _ClearOverloadSpyBus(positions={ELBOW_MOTOR: 126}, offsets={ELBOW_MOTOR: EXPECTED_OFFSET}) + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + arm_cmd.cmd_arm_rezero(_args(apply=True, json_mode=True)) + + payload = json.loads(capsys.readouterr().out) + assert payload["written"] is False + assert payload["reason"] == "already-applied" + assert bus.clear_overload_calls == 0 + assert bus.torque_writes == [] + assert bus.register_writes == [] + + +def test_a_retry_that_still_raises_overload_propagates_and_does_not_loop(monkeypatch): + """The negative control: recovery is ONE retry, not a loop. + + Models a fault ``clear_overload`` cannot actually clear — every read stays + latched no matter how many times torque is cycled. The fix must not spin + forever against a servo that is never going to answer differently; it + retries exactly once and then lets the second ``OverloadError`` propagate. + """ + + class _StubbornlyLatchedBus(FakeBus): + """A servo whose overload never releases, however many times it is cleared.""" + + def read_offset(self, motor: int) -> int: + raise OverloadError( + motor=motor, + error_byte=32, + message=f"FakeBus: motor {motor} refuses to un-latch.", + ) + + bus = _StubbornlyLatchedBus(positions={ELBOW_MOTOR: 126}) + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + with pytest.raises(OverloadError): + arm_cmd.cmd_arm_rezero(_args(apply=True)) + + assert bus.offset_writes == [] # the write was never reached + + +# --------------------------------------------------------------------------- +# --verify: the seam-eviction proof, under BOTH firmware readings +# --------------------------------------------------------------------------- + + +def test_verify_PASSES_when_the_offset_wraps(monkeypatch, capsys): + """The world we hope we live in — and the run that finally settles the caveat.""" + bus = _swept_bus(offset_wraps=True) + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + arm_cmd.cmd_arm_rezero(_args(verify=True, duration=SWEEP_SECONDS, apply=True, json_mode=True)) + + sweep = json.loads(capsys.readouterr().out)["sweep"] + assert sweep["verdict"] == rezero.VERDICT_SEAM_EVICTED + assert sweep["seam_evicted"] is True + assert sweep["failed"] is False + assert sweep["continuous"] is True + assert sweep["conclusive"] is True + assert sweep["discontinuities"] == [] + # It also measured the far wall — a number nobody has ever had. + assert sweep["span"] >= sweep["expected_travel"] + + +def test_verify_FAILS_LOUDLY_when_the_offset_does_NOT_wrap(monkeypatch, capsys): + """THE test this whole task exists for. + + Under ``offset_wraps=False`` the firmware reports a plain signed subtraction: + the offset only relabels positions, the seam stays pinned where it always + was, and the re-zero achieves NOTHING. The verb must not shrug at that. It + must fail, with a non-zero exit, in words the operator cannot misread — and + it must still hand them the numbers, because those numbers are what the + re-decision will be made on. + """ + bus = _swept_bus(offset_wraps=False) + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + with pytest.raises(CliError) as exc: + arm_cmd.cmd_arm_rezero(_args(verify=True, duration=SWEEP_SECONDS, apply=True)) + + # A stop condition, and it exits non-zero so no script can read it as success. + assert exc.value.code == EXIT_ENV_ERROR + assert "SEAM NOT EVICTED" in exc.value.message + assert "does NOT reduce the corrected position modulo 4096" in exc.value.message + assert "STOP" in exc.value.remediation + assert "re-decision" in exc.value.remediation + + # ...and the report still went to stdout, BEFORE the raise. The numbers are + # exactly as valuable when the answer is "the fix does not work". + out = capsys.readouterr().out + assert "STOP — THE RE-ZERO DID NOT WORK" in out + assert "largest jump" in out + + +def test_verify_failure_emits_the_full_report_as_json_before_raising(monkeypatch, capsys): + bus = _swept_bus(offset_wraps=False) + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + with pytest.raises(CliError): + arm_cmd.cmd_arm_rezero( + _args(verify=True, duration=SWEEP_SECONDS, apply=True, json_mode=True) + ) + + sweep = json.loads(capsys.readouterr().out)["sweep"] + assert sweep["verdict"] == rezero.VERDICT_SEAM_NOT_EVICTED + assert sweep["failed"] is True + assert sweep["out_of_range"] # positions no register could hold + + +def test_verify_on_a_FACTORY_joint_is_a_BASELINE_and_exits_zero(monkeypatch, capsys): + """Running it before the write photographs the bug — useful, and not a failure.""" + bus = _swept_bus(rezeroed=False) + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + arm_cmd.cmd_arm_rezero( + _args(verify=True, duration=SWEEP_SECONDS, apply=True, json_mode=True) + ) # must NOT raise + + sweep = json.loads(capsys.readouterr().out)["sweep"] + assert sweep["verdict"] == rezero.VERDICT_SEAM_PRESENT_BASELINE + assert sweep["failed"] is False + assert sweep["rezeroed"] is False + assert sweep["largest_jump"] > 4000 # the raw seam, in the open + + +def test_verify_COMMANDS_NO_MOTION_and_leaves_the_joint_LIMP(monkeypatch): + """The verb deliberately ENDS with the joint limp — a human's hand is on it.""" + bus = _swept_bus() + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + arm_cmd.cmd_arm_rezero(_args(verify=True, duration=SWEEP_SECONDS, apply=True)) + + assert bus.position_writes == [] + assert all(w["addr"] != ADDR_GOAL_POSITION for w in bus.register_writes) + assert bus.torque_writes and all(w["on"] is False for w in bus.torque_writes) + # And it wrote no EEPROM: --verify is a measurement, not a write. + assert all(w["addr"] != ADDR_HOMING_OFFSET for w in bus.register_writes) + assert bus.offset_writes == [] + + +def test_verify_tells_the_operator_to_start_moving_the_joint(monkeypatch, capsys): + """A human hand-moving a limp joint with no feedback cannot tell "the tool is + watching me" from "the tool wedged and I am wobbling a dead arm".""" + bus = _swept_bus() + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + arm_cmd.cmd_arm_rezero(_args(verify=True, duration=SWEEP_SECONDS, apply=True)) + + err = capsys.readouterr().err + assert "Torque is now OFF" in err + assert "ENTIRE travel" in err + assert "sample" in err # the live position feed + + +def test_verify_progress_goes_to_stderr_and_never_pollutes_the_json_stdout(monkeypatch, capsys): + """Under ``--json`` stdout must carry exactly ONE document — the report. + + A progress line interleaved into stdout would wedge a partial object between + the reader and the result, and an agent parsing it would see neither. + """ + bus = _swept_bus() + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + arm_cmd.cmd_arm_rezero(_args(verify=True, duration=SWEEP_SECONDS, apply=True, json_mode=True)) + + captured = capsys.readouterr() + json.loads(captured.out) # exactly one document; must not raise + assert "sample" in captured.err + for line in captured.err.strip().splitlines(): + if line.startswith("{"): + json.loads(line) # structured diagnostics stay structured + + +def test_a_duration_too_short_to_have_a_delta_is_a_user_error(monkeypatch): + """Caught BEFORE the consent prompt — do not make the operator say yes to nothing.""" + bus = _swept_bus() + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin(["yes\n"], tty=True)) + + with pytest.raises(CliError) as exc: + arm_cmd.cmd_arm_rezero(_args(verify=True, duration=0.01, apply=True)) + + assert exc.value.code == EXIT_USER_ERROR + assert bus.register_writes == [] # not even the torque-off + + +def test_verify_declined_at_the_prompt_does_nothing(monkeypatch, capsys): + bus = _swept_bus() + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin(["no\n"], tty=True)) + + arm_cmd.cmd_arm_rezero(_args(verify=True, duration=SWEEP_SECONDS)) + + assert bus.register_writes == [] + assert "Aborted" in capsys.readouterr().out + + +# --------------------------------------------------------------------------- +# Torque ownership + failure hygiene +# --------------------------------------------------------------------------- + + +def test_a_bus_failure_mid_sweep_releases_torque_and_raises_a_structured_error(monkeypatch, capsys): + """The joint must never be left hot by a crash — and no traceback may leak.""" + + class _DyingBus(HandMovedBus): + def read_position(self, motor: int) -> int: + if len(self.torque_writes) and self._op_count > 6: + raise CliError( + code=EXIT_ENV_ERROR, + message="simulated serial failure mid-sweep", + remediation="retry", + ) + return super().read_position(motor) + + bus = _DyingBus(ticks_per_read=SWEEP_STEP, offsets={ELBOW_MOTOR: EXPECTED_OFFSET}) + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + with pytest.raises(CliError) as exc: + arm_cmd.cmd_arm_rezero(_args(verify=True, duration=SWEEP_SECONDS, apply=True)) + + assert exc.value.code == EXIT_ENV_ERROR + assert "simulated serial failure" in exc.value.message + # The torque guard fired and announced it on stderr. + assert "Torque released" in capsys.readouterr().err + assert all(w["on"] is False for w in bus.torque_writes) + + +def test_the_torque_guard_owns_only_the_joint_being_re_zeroed(monkeypatch, capsys): + """One joint is touched, so one joint is claimed — no crying wolf over five others.""" + owned: "list[tuple[int, ...]]" = [] + real_guard = arm_cmd.torque_guard + + def _spy(bus, motors=(), **kwargs): + owned.append(tuple(motors)) + return real_guard(bus, motors, **kwargs) + + monkeypatch.setattr(arm_cmd, "torque_guard", _spy) + bus = FakeBus(positions={ELBOW_MOTOR: 126}) + _patch_bus(monkeypatch, bus) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + arm_cmd.cmd_arm_rezero(_args(apply=True)) + + assert owned == [(ELBOW_MOTOR,)] + + +def test_no_port_is_an_env_error_not_a_traceback(monkeypatch): + monkeypatch.setattr(arm_cmd, "_candidate_ports", lambda: []) + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + + with pytest.raises(CliError) as exc: + arm_cmd.cmd_arm_rezero(_args(apply=True)) + + assert exc.value.code == EXIT_ENV_ERROR + + +def test_the_leader_role_resolves_the_same_motor_id(monkeypatch, capsys): + """Both roles carry elbow_flex on id 3 (LeRobot: follower == leader for ids).""" + monkeypatch.setattr(sys, "stdin", _FakeStdin([], tty=False)) + monkeypatch.setattr(arm_cmd, "_candidate_ports", lambda: ["/dev/ttyACM_fake"]) + + arm_cmd.cmd_arm_rezero(_args(role="leader", json_mode=True)) + + plan = json.loads(capsys.readouterr().out)["plan"] + assert plan["role"] == "leader" + assert plan["motor"] == ELBOW_MOTOR + + +# --------------------------------------------------------------------------- +# Catalog lockstep +# --------------------------------------------------------------------------- + + +def test_explain_arm_rezero_resolves_and_names_the_stop_condition(): + from arm101.explain.catalog import ENTRIES + + entry = ENTRIES[("arm", "rezero")] + assert "seam-not-evicted" in entry + assert "wrist_roll" in entry + assert "commands no motion" in entry.lower() + assert "bootstrap problem" in entry.lower() + + +def test_learn_and_overview_both_mention_rezero(): + from arm101.cli._commands.learn import _TEXT, _as_json_payload + from arm101.cli._commands.overview import _VERBS + + assert "arm rezero" in _TEXT + assert any("rezero" in v for v in _VERBS) + payload = _as_json_payload() + assert ["arm", "rezero"] in [c["path"] for c in payload["commands"]] + assert "arm rezero" in payload["hardware"]["verbs"] diff --git a/tests/test_arm_spec.py b/tests/test_arm_spec.py index ea96371..3e4de4c 100644 --- a/tests/test_arm_spec.py +++ b/tests/test_arm_spec.py @@ -9,6 +9,8 @@ * Leader's six (servo_model, gear_ratio) pairs match the Seeed SO-101 wiki BOM exactly. * Accessor helpers (roles, joint_ids, motor_spec, role_motors) work correctly. * Unknown role raises ValueError; unknown joint raises ValueError. +* wrist_roll's SoftLimit, dead_arc_contains_seam, and soft_limit() — the + encoder-wrap dead-arc machinery (issue #35 / plan task t9). """ import pytest @@ -18,12 +20,19 @@ DEFAULT_BAUDRATE, DEFAULT_CONTACT_THRESHOLDS, JOINTS, + SOFT_LIMITS, + TICK_MAX, + TICK_MIN, MotorSpec, + SoftLimit, + _require_dead_arc_contains_seam, + dead_arc_contains_seam, joint_ids, motor_spec, resolve_contact_thresholds, role_motors, roles, + soft_limit, ) from arm101.hardware.gentle import _CONTACT_TORQUE_LIMIT @@ -368,3 +377,291 @@ def test_resolve_unknown_joint_in_per_joint_raises(): def test_resolve_unknown_joint_in_from_file_raises(): with pytest.raises(ValueError, match="Unknown joint"): resolve_contact_thresholds(from_file={"not_a_joint": 100}) + + +# --------------------------------------------------------------------------- +# Encoder wrap — TICK_MIN / TICK_MAX +# --------------------------------------------------------------------------- + + +def test_tick_bounds_are_12_bit_encoder_range(): + """The STS3215 is a 12-bit encoder: ticks run [0, 4095].""" + assert TICK_MIN == 0 + assert TICK_MAX == 4095 + + +# --------------------------------------------------------------------------- +# SoftLimit — structural validation +# --------------------------------------------------------------------------- + + +def test_soft_limit_accepts_a_well_ordered_interval(): + limit = SoftLimit(min_tick=100, max_tick=3995) + assert limit.min_tick == 100 + assert limit.max_tick == 3995 + + +@pytest.mark.parametrize( + "min_tick,max_tick", + [ + (100, 100), # min == max: zero-width, not a valid interval + (200, 100), # min > max: the "wrap means min>max" encoding this module bans + (-1, 100), # below TICK_MIN + (100, 4096), # above TICK_MAX + (0, 4095), # structurally valid (full range) — SoftLimit alone allows this + ], +) +def test_soft_limit_rejects_malformed_intervals(min_tick, max_tick): + if (min_tick, max_tick) == (0, 4095): + # The full range is a well-formed interval structurally — SoftLimit's + # own validation is purely "is this a well-ordered interval", not the + # seam-containment guarantee. That guarantee is a SEPARATE check (see + # dead_arc_contains_seam below) precisely so the two concerns are + # independently testable. + SoftLimit(min_tick=min_tick, max_tick=max_tick) + else: + with pytest.raises(ValueError): + SoftLimit(min_tick=min_tick, max_tick=max_tick) + + +def test_soft_limit_dead_arc_ticks_matches_both_excluded_sides(): + limit = SoftLimit(min_tick=100, max_tick=3995) + # low side [0, 100) = 100 ticks, high side (3995, 4095] = 100 ticks + assert limit.dead_arc_ticks == 200 + + +def test_soft_limit_dead_arc_ticks_zero_for_full_range(): + limit = SoftLimit(min_tick=TICK_MIN, max_tick=TICK_MAX) + assert limit.dead_arc_ticks == 0 + + +def test_soft_limit_permits_checks_inclusive_bounds(): + limit = SoftLimit(min_tick=100, max_tick=3995) + assert limit.permits(100) is True + assert limit.permits(3995) is True + assert limit.permits(2000) is True + assert limit.permits(99) is False + assert limit.permits(3996) is False + assert limit.permits(0) is False + assert limit.permits(4095) is False + + +# --------------------------------------------------------------------------- +# dead_arc_contains_seam — the enforceable predicate +# --------------------------------------------------------------------------- + + +def test_dead_arc_contains_seam_true_for_a_genuine_restriction(): + """Any interval narrower than the full turn excludes an arc through the seam.""" + assert dead_arc_contains_seam(100, 3995) is True + + +def test_dead_arc_contains_seam_true_when_only_the_low_side_is_excluded(): + assert dead_arc_contains_seam(50, TICK_MAX) is True + + +def test_dead_arc_contains_seam_true_when_only_the_high_side_is_excluded(): + assert dead_arc_contains_seam(TICK_MIN, 4000) is True + + +def test_dead_arc_contains_seam_false_for_the_full_range(): + """The degenerate case: nothing excluded, so the seam is not contained anywhere. + + This is the exact "a soft limit that still spans the seam buys nothing" + case the task exists to guard against. If a future edit ever widens + wrist_roll's SOFT_LIMITS entry back to (TICK_MIN, TICK_MAX), this is the + predicate that must say False — see + test_widening_the_soft_limit_to_the_full_range_fails_validation below for + the module-load-time enforcement built on top of it. + """ + assert dead_arc_contains_seam(TICK_MIN, TICK_MAX) is False + + +@pytest.mark.parametrize( + "min_tick,max_tick", + [ + (100, 100), + (200, 100), + (-1, 100), + (100, 4096), + ], +) +def test_dead_arc_contains_seam_rejects_malformed_intervals(min_tick, max_tick): + with pytest.raises(ValueError): + dead_arc_contains_seam(min_tick, max_tick) + + +# --------------------------------------------------------------------------- +# _require_dead_arc_contains_seam — the import-time guard, and how a future +# widening of the range is caught +# --------------------------------------------------------------------------- + + +def test_require_dead_arc_contains_seam_passes_the_real_soft_limits_table(): + """The table this module actually ships must satisfy its own guarantee.""" + _require_dead_arc_contains_seam(SOFT_LIMITS) # must not raise + + +def test_widening_the_soft_limit_to_the_full_range_fails_validation(): + """A future edit that widens a joint's range back to the full turn must be + rejected — not silently accepted — which is exactly what would happen if + SOFT_LIMITS["wrist_roll"] were ever "simplified" back to (TICK_MIN, TICK_MAX). + + This directly exercises the guard :func:`_require_dead_arc_contains_seam` + runs at import time against the real :data:`SOFT_LIMITS`, using a + reconstructed table so the test does not have to mutate (and restore) the + module-level constant to prove the point. + """ + widened = {"wrist_roll": SoftLimit(min_tick=TICK_MIN, max_tick=TICK_MAX)} + with pytest.raises(ValueError, match="spans the full"): + _require_dead_arc_contains_seam(widened) + + +def test_require_dead_arc_contains_seam_empty_table_is_fine(): + _require_dead_arc_contains_seam({}) # must not raise + + +# --------------------------------------------------------------------------- +# SOFT_LIMITS / soft_limit() — the shipped wrist_roll value +# --------------------------------------------------------------------------- + + +def test_soft_limits_has_exactly_one_entry_wrist_roll(): + """Only wrist_roll gets a soft limit; elbow_flex takes the re-zero path instead.""" + assert set(SOFT_LIMITS.keys()) == {"wrist_roll"} + + +def test_wrist_roll_soft_limit_shipped_values(): + limit = SOFT_LIMITS["wrist_roll"] + assert (limit.min_tick, limit.max_tick) == (100, 3995) + + +def test_wrist_roll_soft_limit_dead_arc_contains_the_seam(): + """The load-bearing property, checked directly against the shipped value.""" + limit = SOFT_LIMITS["wrist_roll"] + assert dead_arc_contains_seam(limit.min_tick, limit.max_tick) is True + + +def test_wrist_roll_soft_limit_is_narrower_than_a_full_turn(): + """A genuine dead arc exists — the range is strictly narrower than [0, 4095].""" + limit = SOFT_LIMITS["wrist_roll"] + assert limit.dead_arc_ticks > 0 + assert (limit.min_tick, limit.max_tick) != (TICK_MIN, TICK_MAX) + + +def test_wrist_roll_soft_limit_lands_inside_the_measured_free_envelope(): + """The permitted range never asks the servo to go where exploration didn't + already drive it without incident (measured free range [21, 4073]).""" + limit = SOFT_LIMITS["wrist_roll"] + assert limit.min_tick >= 21 + assert limit.max_tick <= 4073 + + +def test_soft_limit_returns_none_for_joints_without_a_wrap_problem(): + for joint in JOINTS: + if joint == "wrist_roll": + continue + assert soft_limit(joint) is None + + +def test_soft_limit_returns_the_soft_limit_for_wrist_roll(): + result = soft_limit("wrist_roll") + assert isinstance(result, SoftLimit) + assert result is SOFT_LIMITS["wrist_roll"] + + +def test_soft_limit_unknown_joint_raises(): + with pytest.raises(ValueError, match="Unknown joint"): + soft_limit("not_a_joint") + + +# --------------------------------------------------------------------------- +# Acceptance criterion: a sweep across the permitted range is monotonic in +# ticks, and never enters the dead arc / crosses the seam. +# --------------------------------------------------------------------------- + + +def test_wrist_roll_soft_limit_sweep_is_monotonic_in_ticks(): + """Sweeping wrist_roll's permitted range end-to-end never jumps through the seam. + + This is the direct proof the task calls for: a sweep across the whole + permitted range, in ascending tick order, is strictly increasing (no + 4095->0 jump anywhere inside it) and never visits a dead-arc tick. If + wrist_roll's soft limit still spanned the seam (the exact bug this task + fixes), the analogous sweep would have to wrap through 0 to cover the + joint's full travel and could not be expressed as one monotonic tick + sequence at all. + """ + limit = SOFT_LIMITS["wrist_roll"] + sweep = list(range(limit.min_tick, limit.max_tick + 1, 37)) + if sweep[-1] != limit.max_tick: + sweep.append(limit.max_tick) + + assert len(sweep) > 2, "sweep must cover more than just the two endpoints" + + for previous, current in zip(sweep, sweep[1:]): + assert current > previous, "sweep must be strictly increasing" + + for tick in sweep: + assert limit.permits(tick), f"tick {tick} outside the permitted range" + assert TICK_MIN <= tick <= TICK_MAX + + # The defining property: no tick in the sweep falls in the dead arc, i.e. + # the sweep never has to cross [TICK_MIN, min_tick) or (max_tick, TICK_MAX]. + dead_low = range(TICK_MIN, limit.min_tick) + dead_high = range(limit.max_tick + 1, TICK_MAX + 1) + assert not any(tick in dead_low for tick in sweep) + assert not any(tick in dead_high for tick in sweep) + + +def test_full_range_sweep_would_have_to_cross_the_seam_to_be_monotonic(): + """Contrast case: sweeping the WHOLE [TICK_MIN, TICK_MAX] range in one + direction never needs to wrap either — because a single unbroken sweep + from 0 to 4095 doesn't have to come back around. The seam only bites a + controller that must go from a point near one edge to a point near the + other by the SHORT way (e.g. 4 -> 304, the real hardware failure in + docs/hardware-validation-arm-read-flex.md). This test documents why + dead_arc_contains_seam — not "can I enumerate 0..4095 in order" — is the + right predicate: monotonicity of a single full sweep is necessary but not + sufficient, whereas an explicit dead arc rules out the wrap entirely, + for every pair of permitted ticks, not just an end-to-end sweep. + """ + full_sweep = list(range(TICK_MIN, TICK_MAX + 1, 500)) + for previous, current in zip(full_sweep, full_sweep[1:]): + assert current > previous + # And yet the full range does NOT contain a dead arc around the seam: + assert dead_arc_contains_seam(TICK_MIN, TICK_MAX) is False + + +# --------------------------------------------------------------------------- +# Spec boundary: nothing in this module writes to a servo register. +# --------------------------------------------------------------------------- + + +def test_arm_spec_module_never_imports_the_bus(): + """arm_spec stays a pure data module — no EEPROM/register-writing capability. + + The soft limit is a SOFTWARE bound only (this task's spec boundary): the + servo's EEPROM min_angle/max_angle registers stay at the factory 0-4095. + Parsing the module's own AST for any import mentioning "bus" is a coarse + but effective guardrail — a module with no import of + arm101.hardware.bus (or MotorBus) has no handle to issue a register write + with in the first place, so this rules the whole category out rather than + checking for one specific call site. + """ + import ast + import inspect + + import arm101.hardware.arm_spec as arm_spec_module + + tree = ast.parse(inspect.getsource(arm_spec_module)) + imported_names = set() + for node in ast.walk(tree): + if isinstance(node, ast.Import): + imported_names.update(alias.name for alias in node.names) + elif isinstance(node, ast.ImportFrom) and node.module: + imported_names.add(node.module) + + assert not any( + "bus" in name.lower() for name in imported_names + ), f"arm_spec must not import a bus module; found {imported_names}" diff --git a/tests/test_bus_offset.py b/tests/test_bus_offset.py new file mode 100644 index 0000000..fead351 --- /dev/null +++ b/tests/test_bus_offset.py @@ -0,0 +1,794 @@ +"""Tests for the STS3215 encoder-offset primitives (``Ofs`` / ``Homing_Offset``, addr 31). + +TDD: written before ``read_offset`` / ``write_offset`` / ``encode_offset`` / +``decode_offset`` existed, and they drive the implementation. + +Why this register matters (issue #35): ``elbow_flex``'s encoder WRAPS inside its +physical travel — driven far enough it crosses the raw 4095->0 seam and reads +back near zero — so its encoder value is not monotonic with joint angle and +every position comparison in the codebase (arrival checks, ``clamp_goal``, +``ReachMap`` ranges) is wrong for it. Writing addr 31 shifts the joint's zero so +the seam lands in the arc the joint CANNOT reach, which makes the linear-axis +assumption true rather than merely assumed. + +Three hardware failures are pinned here, each with its own test: + +1. **Sign-magnitude, not two's complement.** The offset's sign lives in BIT 11 + (``(sign << 11) | magnitude``), so ``-1073`` goes on the wire as ``3121``, + NOT as ``0xFBCF``. A two's-complement encoder would silently write a + ~64000-tick garbage offset. +2. **The EEPROM Lock dance.** PR #21 of this repo exists because id/baud writes + silently REVERTED on the next power-cycle when the Lock register (addr 55) + was never opened. Addr 31 is in the same EEPROM region and carries the same + hazard, so the unlock -> write -> relock order is asserted on the recorded + register-write sequence, not merely on the end state. +3. **The LeRobot trap.** LeRobot's ``write_calibration`` writes + ``Min_Position_Limit`` (addr 9) and ``Max_Position_Limit`` (addr 11) + alongside the homing offset. In servo mode those CLAMP goals — narrowing the + very reachable set the re-zero is meant to recover. ``write_offset`` writes + addr 31 and nothing else, and that is pinned explicitly. + +See ``docs/spikes/sts3215-offset-register.md`` for the sourced research. +""" + +from __future__ import annotations + +import pytest + +from arm101.cli._errors import EXIT_ENV_ERROR, EXIT_USER_ERROR, CliError +from arm101.hardware.bus import ( + ADDR_HOMING_OFFSET, + ADDR_LOCK, + ADDR_TORQUE_ENABLE, + OFFSET_MAX_MAGNITUDE, + OFFSET_SIGN_BIT, + FakeBus, + FeetechBus, + OverloadError, + decode_offset, + encode_offset, +) + +#: Position-limit registers LeRobot's write_calibration also writes. Nothing in +#: this repo may ever write them — in servo mode they clamp goals. +ADDR_MIN_POSITION_LIMIT = 9 +ADDR_MAX_POSITION_LIMIT = 11 + +#: The offset issue #35 needs for elbow_flex: the midpoint of its unreachable +#: arc (126, 2020). Used throughout so the tests read as the real scenario. +ELBOW_FLEX_OFFSET = 1073 + +#: ``encode_offset(-1073)`` -> ``2048 + 1073``. Spelled out because getting this +#: number wrong is the whole point of the codec tests. +ELBOW_FLEX_OFFSET_NEGATIVE_WIRE = 3121 + + +# --------------------------------------------------------------------------- +# Recording packet-handler stubs (FeetechBus wire-level assertions) +# --------------------------------------------------------------------------- + + +class _RecordingPacket: + """Records every 1- and 2-byte register write into ONE ordered event list. + + One list, not two, because the assertion that matters is the *interleaving*: + torque-off (1 byte, addr 40) then unlock (1 byte, addr 55) then the offset + (2 bytes, addr 31) then relock (1 byte, addr 55). Separate per-width lists + could not express that order, and it is exactly the order the EEPROM write + depends on. + + Parameters + ---------- + fail_addrs: + Addresses whose write should report a comms failure (``result=1``). + error_addrs: + Addresses whose write should report a servo status *error* byte + (e.g. ``{40: 32}`` for a motor latched in overload). + reads: + Canned ``addr -> value`` map for :meth:`read2ByteTxRx`. + """ + + def __init__( + self, + fail_addrs: "set[int] | None" = None, + error_addrs: "dict[int, int] | None" = None, + reads: "dict[int, int] | None" = None, + ) -> None: + self.writes: list[tuple[int, int, int]] = [] + self.reads: list[tuple[int, int]] = [] + self._fail_addrs = set(fail_addrs or set()) + self._error_addrs = dict(error_addrs or {}) + self._canned = dict(reads or {}) + + def _result(self, addr: int) -> "tuple[int, int]": + return (1 if addr in self._fail_addrs else 0, self._error_addrs.get(addr, 0)) + + def write1ByteTxRx(self, port, motor, addr, val): # noqa: N802 - SDK spelling + self.writes.append((motor, addr, val)) + return self._result(addr) + + def write2ByteTxRx(self, port, motor, addr, val): # noqa: N802 - SDK spelling + self.writes.append((motor, addr, val)) + return self._result(addr) + + def read2ByteTxRx(self, port, motor, addr): # noqa: N802 - SDK spelling + self.reads.append((motor, addr)) + result, error = self._result(addr) + return self._canned.get(addr, 0), result, error + + def read1ByteTxRx(self, port, motor, addr): # noqa: N802 - SDK spelling + self.reads.append((motor, addr)) + result, error = self._result(addr) + return self._canned.get(addr, 0), result, error + + +def _open_feetech(packet: _RecordingPacket) -> FeetechBus: + """A FeetechBus wired to *packet*, marked open, with no serial port involved.""" + bus = FeetechBus(port="/dev/ttyUSB_fake") + bus._packet_handler = packet + bus._port_handler = object() + bus._open = True + return bus + + +# =========================================================================== +# 1. The codec — sign-magnitude on bit 11 (pure, hardware-free) +# =========================================================================== + + +def test_offset_constants_match_the_datasheet(): + """Address 31, sign bit 11, max magnitude 2047 — the three facts everything rests on.""" + assert ADDR_HOMING_OFFSET == 31 + assert OFFSET_SIGN_BIT == 11 + assert OFFSET_MAX_MAGNITUDE == (1 << 11) - 1 == 2047 + + +@pytest.mark.parametrize("offset", [0, 1, 2, 1073, 2046, 2047]) +def test_encode_offset_positive_is_the_magnitude_itself(offset): + """A positive offset goes on the wire unchanged — the sign bit stays clear.""" + assert encode_offset(offset) == offset + assert encode_offset(offset) & (1 << OFFSET_SIGN_BIT) == 0 + + +@pytest.mark.parametrize( + ("offset", "wire"), + [ + (-1, 2049), # 2048 | 1 + (-1073, ELBOW_FLEX_OFFSET_NEGATIVE_WIRE), # 2048 | 1073 + (-2047, 4095), # 2048 | 2047 — the widest negative + ], +) +def test_encode_offset_negative_sets_bit_11(offset, wire): + """A negative offset is 2048 + |offset|: sign in bit 11, magnitude below it.""" + assert encode_offset(offset) == wire + + +def test_encode_offset_is_not_twos_complement(): + """The regression this whole module exists to prevent. + + Two's complement would put -1 on the wire as 0xFFFF (65535) and -1073 as + 0xFBCF (64463). The STS3215 would read those as an absurd magnitude and the + joint's reported position would be garbage. Sign-magnitude gives 2049 and + 3121. + """ + assert encode_offset(-1) == 2049 + assert encode_offset(-1) != 0xFFFF + assert encode_offset(-1073) == 3121 + assert encode_offset(-1073) != 0xFBCF + + +@pytest.mark.parametrize( + ("wire", "offset"), + [ + (0, 0), + (1, 1), + (1073, 1073), + (2047, 2047), + (2049, -1), + (ELBOW_FLEX_OFFSET_NEGATIVE_WIRE, -1073), + (4095, -2047), + ], +) +def test_decode_offset_reads_sign_magnitude(wire, offset): + """Bit 11 set means negative; the low 11 bits are the magnitude.""" + assert decode_offset(wire) == offset + + +def test_decode_offset_negative_zero_is_zero(): + """Wire 2048 is "negative zero" — a real encoding the servo can hold; it decodes to 0. + + The encoding is therefore NOT injective on the wire (0 and 2048 both mean an + offset of zero), which is why the round-trip property is stated as + ``decode(encode(x)) == x`` and never the other way round. + """ + assert decode_offset(2048) == 0 + assert encode_offset(0) == 0 # we always write the canonical +0 + + +def test_decode_offset_ignores_bits_above_the_12th(): + """A 2-byte read returns 16 bits; only the low 12 are the register. + + Mirrors ``read_position``'s ``& 0x0FFF``. Without this mask a stray high bit + from a noisy read would decode into a nonsense magnitude rather than being + discarded. + """ + assert decode_offset(0xF000 | ELBOW_FLEX_OFFSET_NEGATIVE_WIRE) == -1073 + assert decode_offset(0x1000 | 5) == 5 + + +def test_offset_round_trip_across_the_entire_range(): + """decode(encode(x)) == x for every representable offset, -2047 .. +2047.""" + for offset in range(-OFFSET_MAX_MAGNITUDE, OFFSET_MAX_MAGNITUDE + 1): + assert decode_offset(encode_offset(offset)) == offset + + +def test_every_encoded_offset_fits_the_12_bit_register(): + """No representable offset can ever overflow into bits 12-15.""" + for offset in range(-OFFSET_MAX_MAGNITUDE, OFFSET_MAX_MAGNITUDE + 1): + assert 0 <= encode_offset(offset) <= 0xFFF + + +@pytest.mark.parametrize("offset", [2048, -2048, 2073, -2073, 4096, -100000]) +def test_encode_offset_rejects_unrepresentable_magnitudes(offset): + """|offset| > 2047 cannot be encoded — reject loudly, never truncate. + + Truncating would corrupt the magnitude straight into the sign bit: 2048 + silently becomes "-0". LeRobot issue #3193 is a real SO-101 hitting exactly + this (``ValueError: Magnitude 2073 exceeds 2047``); 2073 is in the parameter + list for that reason. + """ + with pytest.raises(CliError) as exc: + encode_offset(offset) + assert exc.value.code == EXIT_USER_ERROR + assert "2047" in exc.value.message + + +def test_encode_offset_boundaries_are_inclusive(): + """+/-2047 encode; +/-2048 do not. The fence is exactly where the sign bit starts.""" + assert encode_offset(2047) == 2047 + assert encode_offset(-2047) == 4095 + for over in (2048, -2048): + with pytest.raises(CliError): + encode_offset(over) + + +# =========================================================================== +# 2. FeetechBus.read_offset — decode what the servo reports +# =========================================================================== + + +def test_feetech_read_offset_reads_address_31_as_two_bytes(): + """read_offset issues exactly one 2-byte read of addr 31.""" + packet = _RecordingPacket(reads={ADDR_HOMING_OFFSET: ELBOW_FLEX_OFFSET}) + bus = _open_feetech(packet) + + assert bus.read_offset(motor=3) == ELBOW_FLEX_OFFSET + assert packet.reads == [(3, ADDR_HOMING_OFFSET)] + + +def test_feetech_read_offset_decodes_a_negative_offset(): + """A servo holding -1073 reports 3121 on the wire; read_offset must return -1073. + + Returning the raw 3121 would make the joint look like it had a large + POSITIVE offset — the sign error that silently mis-frames every subsequent + position comparison. + """ + packet = _RecordingPacket(reads={ADDR_HOMING_OFFSET: ELBOW_FLEX_OFFSET_NEGATIVE_WIRE}) + bus = _open_feetech(packet) + + assert bus.read_offset(motor=3) == -1073 + + +def test_feetech_read_offset_raises_cli_error_on_comms_failure(): + """A failed read is a CliError (EXIT_ENV_ERROR), never a traceback or a bogus 0.""" + packet = _RecordingPacket(fail_addrs={ADDR_HOMING_OFFSET}) + bus = _open_feetech(packet) + + with pytest.raises(CliError) as exc: + bus.read_offset(motor=3) + assert exc.value.code == EXIT_ENV_ERROR + + +def test_feetech_read_offset_requires_an_open_bus(): + bus = FeetechBus(port="/dev/ttyUSB_fake") + with pytest.raises(CliError) as exc: + bus.read_offset(motor=3) + assert exc.value.code == EXIT_ENV_ERROR + + +# =========================================================================== +# 3. FeetechBus.write_offset — the register-write SEQUENCE +# =========================================================================== + + +def test_write_offset_emits_torque_off_unlock_write_relock_in_that_order(): + """The whole contract, asserted as one exact wire sequence. + + * torque-off (addr 40) FIRST — with torque on, the servo instantly + re-interprets its own position against the new offset and could LURCH + toward its standing goal. Inferred, not documented; treated as a hard + safety rule. + * unlock (addr 55 = 0) BEFORE the EEPROM write — without it the value + updates live and then REVERTS on the next power-cycle (PR #21). + * the offset (addr 31) — the only data register touched. + * relock (addr 55 = 1) LAST — write-protection restored. + """ + packet = _RecordingPacket() + bus = _open_feetech(packet) + + bus.write_offset(motor=3, offset=ELBOW_FLEX_OFFSET) + + assert packet.writes == [ + (3, ADDR_TORQUE_ENABLE, 0), # torque off — before anything else + (3, ADDR_LOCK, 0), # unlock EEPROM + (3, ADDR_HOMING_OFFSET, ELBOW_FLEX_OFFSET), # the offset itself + (3, ADDR_LOCK, 1), # relock EEPROM + ] + + +def test_write_offset_puts_the_sign_magnitude_encoding_on_the_wire(): + """A negative offset reaches addr 31 as 2048 + |offset|, not as two's complement.""" + packet = _RecordingPacket() + bus = _open_feetech(packet) + + bus.write_offset(motor=3, offset=-ELBOW_FLEX_OFFSET) + + offset_writes = [w for w in packet.writes if w[1] == ADDR_HOMING_OFFSET] + assert offset_writes == [(3, ADDR_HOMING_OFFSET, ELBOW_FLEX_OFFSET_NEGATIVE_WIRE)] + + +def test_write_offset_never_touches_the_position_limit_registers(): + """The LeRobot trap, pinned. + + LeRobot's ``write_calibration`` writes Min_Position_Limit (9) and + Max_Position_Limit (11) alongside the homing offset. In servo mode the + firmware CLAMPS goals to that window — narrowing the reachable set the + re-zero exists to recover. Our factory limits are the wide-open 0/4095 and + must stay that way. + """ + packet = _RecordingPacket() + bus = _open_feetech(packet) + + bus.write_offset(motor=3, offset=ELBOW_FLEX_OFFSET) + + touched = {addr for _motor, addr, _val in packet.writes} + assert ADDR_MIN_POSITION_LIMIT not in touched + assert ADDR_MAX_POSITION_LIMIT not in touched + # Stronger: an exact allow-list. Any NEW register this method learns to + # write must be justified here first. + assert touched == {ADDR_TORQUE_ENABLE, ADDR_LOCK, ADDR_HOMING_OFFSET} + + +def test_write_offset_out_of_range_writes_nothing_at_all(): + """A rejected offset must not disable torque, and must not open the EEPROM. + + Validation happens BEFORE any wire traffic, so a bad ``--offset`` leaves the + arm exactly as it found it — torque still on, Lock still closed. + """ + packet = _RecordingPacket() + bus = _open_feetech(packet) + + with pytest.raises(CliError) as exc: + bus.write_offset(motor=3, offset=2048) + + assert exc.value.code == EXIT_USER_ERROR + assert packet.writes == [] + + +def test_write_offset_failed_eeprom_write_still_relocks(): + """A failing addr-31 write must NEVER strand the motor at Lock=0. + + An EEPROM left unlocked is a motor whose id, baud and offset can be + clobbered by any subsequent stray write — the state PR #21 was written to + avoid. The original error still propagates. + """ + packet = _RecordingPacket(fail_addrs={ADDR_HOMING_OFFSET}) + bus = _open_feetech(packet) + + with pytest.raises(CliError) as exc: + bus.write_offset(motor=3, offset=ELBOW_FLEX_OFFSET) + + assert exc.value.code == EXIT_ENV_ERROR + assert packet.writes == [ + (3, ADDR_TORQUE_ENABLE, 0), + (3, ADDR_LOCK, 0), + (3, ADDR_HOMING_OFFSET, ELBOW_FLEX_OFFSET), # failed + (3, ADDR_LOCK, 1), # best-effort relock — the motor is never left open + ] + + +def test_write_offset_relock_failure_preserves_the_original_error(): + """If the relock ALSO fails, the caller still learns why the OFFSET write failed. + + The re-lock is best-effort: letting its error replace the offset write's + would hide the real fault behind a secondary one, and the operator would go + hunting for a Lock problem that is not the problem. + """ + + class _OffsetAndRelockFail(_RecordingPacket): + """Unlock (55 <- 0) succeeds; the offset write and the RELOCK (55 <- 1) fail.""" + + def _result(self, addr: int) -> "tuple[int, int]": + return (0, 0) + + def write1ByteTxRx(self, port, motor, addr, val): # noqa: N802 - SDK spelling + self.writes.append((motor, addr, val)) + failed = addr == ADDR_LOCK and val == 1 # the relock, not the unlock + return (1 if failed else 0), 0 + + def write2ByteTxRx(self, port, motor, addr, val): # noqa: N802 - SDK spelling + self.writes.append((motor, addr, val)) + return (1 if addr == ADDR_HOMING_OFFSET else 0), 0 + + packet = _OffsetAndRelockFail() + bus = _open_feetech(packet) + + with pytest.raises(CliError) as exc: + bus.write_offset(motor=3, offset=ELBOW_FLEX_OFFSET) + + # The surfaced error is the OFFSET write's, not the relock's. + assert "offset" in exc.value.message.lower() + assert "re-lock" not in exc.value.message.lower() + # The relock was still attempted — best-effort means tried, not skipped. + assert packet.writes[-1] == (3, ADDR_LOCK, 1) + + +def test_write_offset_unlock_failure_never_writes_the_offset(): + """If the EEPROM cannot be unlocked, the offset write is not attempted. + + Writing it anyway would produce the PR #21 failure mode by construction: a + value that reads back correctly and then vanishes on the next power-cycle. + """ + packet = _RecordingPacket(fail_addrs={ADDR_LOCK}) + bus = _open_feetech(packet) + + with pytest.raises(CliError): + bus.write_offset(motor=3, offset=ELBOW_FLEX_OFFSET) + + assert packet.writes == [ + (3, ADDR_TORQUE_ENABLE, 0), # torque came off + (3, ADDR_LOCK, 0), # unlock attempted, and failed + ] + + +def test_write_offset_torque_off_failure_never_opens_the_eeprom(): + """If torque cannot be disabled, nothing is unlocked and nothing is written. + + The safety rule is not advisory: an offset written to a torqued servo may + make it lurch. If we cannot prove the motor is limp, we do not proceed. + """ + packet = _RecordingPacket(fail_addrs={ADDR_TORQUE_ENABLE}) + bus = _open_feetech(packet) + + with pytest.raises(CliError): + bus.write_offset(motor=3, offset=ELBOW_FLEX_OFFSET) + + assert packet.writes == [(3, ADDR_TORQUE_ENABLE, 0)] + + +def test_write_offset_on_an_overloaded_motor_raises_overload_error_and_writes_no_eeprom(): + """A motor latched in overload tags every response with bit 5 (0x20). + + The torque-off write surfaces that as :class:`OverloadError` — and because + it happens before the unlock, the EEPROM is never opened. The caller (t8) + must ``clear_overload`` first. + """ + packet = _RecordingPacket(error_addrs={ADDR_TORQUE_ENABLE: 32}) + bus = _open_feetech(packet) + + with pytest.raises(OverloadError): + bus.write_offset(motor=3, offset=ELBOW_FLEX_OFFSET) + + touched = {addr for _m, addr, _v in packet.writes} + assert ADDR_LOCK not in touched + assert ADDR_HOMING_OFFSET not in touched + + +def test_feetech_write_offset_requires_an_open_bus(): + bus = FeetechBus(port="/dev/ttyUSB_fake") + with pytest.raises(CliError) as exc: + bus.write_offset(motor=3, offset=0) + assert exc.value.code == EXIT_ENV_ERROR + + +# =========================================================================== +# 4. FeetechBus.read_info — the offset is visible, and it is SIGNED +# =========================================================================== + + +def test_feetech_info_registers_includes_homing_offset(): + """addr 31 (2 bytes) is in the read-only snapshot map. + + If it is not, ``arm read`` silently reports 0 on real hardware while the + servo actually holds a nonzero offset — the register that most needs to be + inspectable would be the one register you cannot see. + """ + assert FeetechBus._INFO_REGISTERS.get("homing_offset") == (31, 2) + + +def test_feetech_read_info_reports_the_offset_decoded_not_raw(): + """``read_info["homing_offset"]`` is the SIGNED offset, never the wire value. + + The wire encoding must not escape ``bus.py``. A -1073 offset surfacing as + "3121" in ``arm read`` would read as a plausible positive offset and mislead + the human doing the re-zero. + """ + packet = _RecordingPacket(reads={ADDR_HOMING_OFFSET: ELBOW_FLEX_OFFSET_NEGATIVE_WIRE}) + bus = _open_feetech(packet) + + info = bus.read_info(motor=3) + + assert info["homing_offset"] == -1073 + assert info["homing_offset"] == bus.read_offset(motor=3) # one number, one meaning + + +# =========================================================================== +# 5. FakeBus — the offset register and its effect on present_position +# =========================================================================== + + +def test_fakebus_offset_defaults_to_zero(): + bus = FakeBus() + bus.open() + assert bus.read_offset(1) == 0 + assert bus.read_info(1)["homing_offset"] == 0 + + +def test_fakebus_offset_round_trips_through_write_then_read(): + bus = FakeBus() + bus.open() + + bus.write_offset(3, -ELBOW_FLEX_OFFSET) + + assert bus.read_offset(3) == -ELBOW_FLEX_OFFSET + assert bus.read_info(3)["homing_offset"] == -ELBOW_FLEX_OFFSET + + +def test_fakebus_offset_can_be_seeded_from_the_constructor(): + bus = FakeBus(offsets={3: 1073}) + bus.open() + assert bus.read_offset(3) == 1073 + assert bus.read_offset(1) == 0 # unseeded motors read the factory 0 + + +def test_fakebus_write_offset_records_the_same_sequence_as_the_real_bus(): + """The fake's register ledger mirrors FeetechBus's wire sequence exactly. + + If the fake skipped the Lock dance, every test built on it would "prove" + a write that on hardware would silently revert on the next power-cycle. + """ + bus = FakeBus() + bus.open() + + bus.write_offset(3, -ELBOW_FLEX_OFFSET) + + assert [(w["motor"], w["addr"], w["value"]) for w in bus.register_writes] == [ + (3, ADDR_TORQUE_ENABLE, 0), + (3, ADDR_LOCK, 0), + (3, ADDR_HOMING_OFFSET, ELBOW_FLEX_OFFSET_NEGATIVE_WIRE), # sign-magnitude on the wire + (3, ADDR_LOCK, 1), + ] + assert bus.offset_writes == [{"motor": 3, "offset": -ELBOW_FLEX_OFFSET}] + # The torque-off funnels through enable_torque, so it shows up there too — + # one operation, one wire byte, one ledger entry per view. + assert bus.torque_writes == [{"motor": 3, "on": False}] + + +def test_fakebus_write_offset_never_touches_the_position_limits(): + """The LeRobot trap, pinned on the fake as well as on the real bus.""" + bus = FakeBus() + bus.open() + + bus.write_offset(3, ELBOW_FLEX_OFFSET) + + touched = {w["addr"] for w in bus.register_writes} + assert ADDR_MIN_POSITION_LIMIT not in touched + assert ADDR_MAX_POSITION_LIMIT not in touched + assert touched == {ADDR_TORQUE_ENABLE, ADDR_LOCK, ADDR_HOMING_OFFSET} + + +def test_fakebus_write_offset_relocks_the_eeprom(): + """Lock ends closed (1) — and read_lock agrees with the ledger.""" + bus = FakeBus() + bus.open() + assert bus.read_lock(3) == 0 # factory default in the fake + + bus.write_offset(3, ELBOW_FLEX_OFFSET) + + assert bus.read_lock(3) == 1 + lock_values = [w["value"] for w in bus.register_writes if w["addr"] == ADDR_LOCK] + assert lock_values == [0, 1] # opened, then closed + + +def test_fakebus_write_offset_rejects_out_of_range_without_touching_the_bus(): + bus = FakeBus() + bus.open() + + with pytest.raises(CliError) as exc: + bus.write_offset(3, -2048) + + assert exc.value.code == EXIT_USER_ERROR + assert bus.register_writes == [] + assert bus.offset_writes == [] + assert bus.read_offset(3) == 0 # unchanged + + +def test_fakebus_write_offset_requires_an_open_bus(): + bus = FakeBus() + with pytest.raises(CliError) as exc: + bus.write_offset(3, 0) + assert exc.value.code == EXIT_ENV_ERROR + + +def test_fakebus_failed_offset_write_still_relocks(): + """The overload seam fires on the EEPROM write; the fake still relocks. + + Mirrors ``test_write_offset_failed_eeprom_write_still_relocks`` on the real + bus, so a caller's recovery path can be tested against the fake and mean + something. + """ + bus = FakeBus() + bus.open() + bus.fail_with_overload_on_op(2) # op 1 = the torque-off; op 2 = the offset write + + with pytest.raises(OverloadError): + bus.write_offset(3, ELBOW_FLEX_OFFSET) + + assert [(w["addr"], w["value"]) for w in bus.register_writes] == [ + (ADDR_TORQUE_ENABLE, 0), + (ADDR_LOCK, 0), + (ADDR_LOCK, 1), # relocked even though the offset never landed + ] + + # The servo is still latched, so recover the way a real caller must before + # reading anything back (clear_overload also disarms the fake's seam). + bus.clear_overload(3) + + assert bus.read_lock(3) == 1 # EEPROM closed again — never stranded open + assert bus.read_offset(3) == 0 # the write did not take effect + + +# --------------------------------------------------------------------------- +# 5b. The offset's EFFECT: Present_Position = Actual_Position - Homing_Offset +# --------------------------------------------------------------------------- + + +def test_fakebus_present_position_shifts_by_the_offset(): + """The exact issue-#35 arithmetic, simulated. + + ``elbow_flex`` rests at raw 126 (past its wrap). With H = 1073 the servo + reports ``(126 - 1073) mod 4096 == 3149`` — and the seam it used to cross + mid-travel now sits at raw 1073, dead centre of the arc it cannot reach. + """ + bus = FakeBus(positions={3: 126}) + bus.open() + + assert bus.read_position(3) == 126 # no offset yet: raw == reported + + bus.write_offset(3, ELBOW_FLEX_OFFSET) + + assert bus.read_position(3) == 3149 + assert bus.read_info(3)["present_position"] == 3149 + + +def test_fakebus_present_position_and_read_info_agree_under_an_offset(): + """read_position and read_info's present_position are the SAME register (addr 56). + + They must therefore never disagree. A fake in which one applies the offset + and the other does not would let a test "prove" behaviour no servo can + exhibit — the lesson ``tests/_fakes.py`` was written to enforce. + """ + bus = FakeBus(positions={1: 2000, 2: 500}) + bus.open() + bus.write_offset(1, 800) + bus.write_offset(2, -800) + + for motor in (1, 2): + assert bus.read_position(motor) == bus.read_info(motor)["present_position"] + + assert bus.read_position(1) == 1200 # 2000 - 800 + assert bus.read_position(2) == 1300 # 500 - (-800) + + +def test_fakebus_present_position_wraps_modulo_4096(): + """The reported position stays inside [0, 4095] — the seam MOVES, it does not vanish.""" + bus = FakeBus(positions={1: 100}) + bus.open() + + bus.write_offset(1, 500) + + assert bus.read_position(1) == 3696 # (100 - 500) mod 4096 + + +def test_fakebus_zero_offset_is_the_identity(): + """With no offset the fake reports the encoder byte-for-byte. + + Load-bearing: existing fixtures seed positions outside [0, 4095] (e.g. 5000) + and must keep reading back exactly what they were given. + """ + bus = FakeBus(positions={1: 5000}) + bus.open() + + assert bus.read_position(1) == 5000 + assert bus.read_info(1)["present_position"] == 5000 + + +def test_fakebus_unwrapped_seam_models_the_pessimistic_firmware_reading(): + """``offset_wraps=False`` models the reading that would make the re-zero USELESS. + + The spike (docs/spikes/sts3215-offset-register.md, section 4) could not find + a primary source for whether the corrected position is reduced modulo 4096 + (seam MOVES to raw == Ofs — the fix works) or reported as a plain signed + subtraction (seam stays pinned at the physical 4095->0 rollover — the fix + achieves nothing). Every source points at the modular reading and LeRobot + ships it, so it is the fake's DEFAULT — but the assumption is unproven, so + the other branch is modelled too rather than being silently assumed away. + Hardware test step 10 settles it; if it settles the wrong way, this is the + behaviour t8 must build against. + """ + bus = FakeBus(positions={1: 100}, offset_wraps=False) + bus.open() + + bus.write_offset(1, 500) + + assert bus.read_position(1) == -400 # 100 - 500, unwrapped — no modular reduction + + +def test_fakebus_read_arm_style_snapshot_is_read_only(): + """Reading the offset writes nothing — ``arm read`` must never mutate the servo.""" + bus = FakeBus(offsets={3: 1073}) + bus.open() + + bus.read_offset(3) + bus.read_info(3) + bus.read_position(3) + + assert bus.register_writes == [] + assert bus.offset_writes == [] + + +# =========================================================================== +# 6. ServoModelBus — the motion sim stays honest under an offset +# =========================================================================== + + +def test_servo_model_bus_reports_the_offset_shifted_position_from_both_reads(): + """``ServoModelBus`` overrides ``read_info``; it must not lose the offset doing so. + + ``read_position`` (inherited) and ``read_info["present_position"]`` + (overridden, and driven by the motion sim) are the SAME register on the + wire. If only one of them applied the offset, a test could pass against a + bus that cannot exist. + """ + from tests._fakes import ServoModelBus + + bus = ServoModelBus(positions={1: 2000}) + bus.open() + bus.write_offset(1, 800) + + assert bus.true_position(1) == 2000 # the shaft has not moved + assert bus.read_position(1) == 1200 # 2000 - 800, as the servo would report + assert bus.read_info(1)["present_position"] == 1200 + + +def test_servo_model_bus_goals_are_commanded_in_the_corrected_frame(): + """A goal written after a re-zero is in the SAME frame the servo now reports in. + + ``Present = Actual - Ofs`` implies ``Actual = Goal + Ofs``: commanding 1500 + on a joint offset by 800 must park the physical shaft at raw 2300, and the + joint must then REPORT 1500. Goal and feedback living in different frames is + the failure mode that would make a re-zero worse than useless, so the fake + refuses to model it. + """ + from tests._fakes import ServoModelBus + + bus = ServoModelBus(positions={1: 2000}, ticks_per_poll=1000) + bus.open() + bus.write_offset(1, 800) + + bus.write_goal_position(1, 1500) # corrected frame + for _ in range(5): # polls = simulated time; let the shaft arrive + reported = bus.read_info(1)["present_position"] + + assert bus.true_position(1) == 2300 # raw shaft: 1500 + 800 + assert reported == 1500 # what the servo reports: back in the corrected frame diff --git a/tests/test_rezero.py b/tests/test_rezero.py new file mode 100644 index 0000000..85ba755 --- /dev/null +++ b/tests/test_rezero.py @@ -0,0 +1,897 @@ +"""Tests for the encoder re-zero (issue #35) — ``arm_spec``'s arc table and +:mod:`arm101.hardware.rezero`'s plan / write / sweep. + +Everything here runs against :class:`~arm101.hardware.bus.FakeBus`, and the most +important thing about that fake is that it models **both** readings of the one +undocumented firmware behaviour the whole re-zero rests on +(``docs/spikes/sts3215-offset-register.md`` §4):: + + offset_wraps=True Present = (raw - Ofs) mod 4096 seam RELOCATES -> fix works + offset_wraps=False Present = raw - Ofs (signed) seam STAYS -> fix does NOTHING + +So every claim this module makes about the sweep is asserted **twice**, once in +each world. Under ``offset_wraps=False`` the verification must FAIL, loudly and +by design: that failure is the feature — it is what stands between the operator +and a re-zero that silently did nothing. + +The other invariant pinned here is the one the hardware cannot forgive: this verb +**commands no motion, on any path.** ``elbow_flex`` currently rests at raw ~126, +PAST its wrap, so a linear goal would rotate it the long way round through its +whole travel and into a wall. Several tests assert the write surface directly — +no goal-position register write, ever, and torque only ever going OFF. +""" + +from __future__ import annotations + +import pytest + +from arm101.cli._errors import EXIT_ENV_ERROR, EXIT_USER_ERROR, CliError +from arm101.hardware import arm_spec, rezero +from arm101.hardware.bus import ( + ADDR_HOMING_OFFSET, + ADDR_LOCK, + ADDR_TORQUE_ENABLE, + ENCODER_RESOLUTION, + OFFSET_MAX_MAGNITUDE, + FakeBus, + OverloadError, +) + +#: ``elbow_flex`` on the follower — the only re-zeroable joint on this arm. +ELBOW = "elbow_flex" +ELBOW_MOTOR = 3 + +#: The offset the spike derives (arc (126, 2020), midpoint 1073). +EXPECTED_OFFSET = 1073 + +#: STS3215 Goal_Position. Must NEVER appear in this verb's write surface. +ADDR_GOAL_POSITION = 42 + + +# --------------------------------------------------------------------------- +# Test doubles +# --------------------------------------------------------------------------- + + +class HandMovedBus(FakeBus): + """A :class:`FakeBus` whose shaft advances a little on every position read. + + Models the one actuator this procedure actually uses: **a human hand.** Each + :meth:`read_position` call advances the *raw* (physical) encoder count by + ``ticks_per_read``, wrapping modulo 4096 exactly as the magnet does, and then + reports it through :meth:`FakeBus._reported_position` — which is where the + written offset, and the ``offset_wraps`` question, actually bite. + + That layering is the whole point: the simulated shaft turns in raw ticks and + knows nothing about offsets, so what the sweep SEES is produced by the same + correction the servo would apply. A fake that moved the *reported* position + directly would be assuming the answer to the question under test. + + Parameters + ---------- + start_raw: + Raw tick the shaft begins at. Defaults to ``elbow_flex``'s hard wall + (2020), the end a human is told to start from. + ticks_per_read: + How far the hand advances the joint between two polls. Positive winds + toward the seam (2020 -> 4095 -> 0 -> 126), which is the direction that + crosses it. + """ + + def __init__( + self, + *args, + start_raw: int = 2020, + ticks_per_read: int = 25, + motor: int = ELBOW_MOTOR, + **kwargs, + ) -> None: + kwargs.setdefault("positions", {motor: start_raw}) + super().__init__(*args, **kwargs) + self._hand_motor = motor + self._ticks_per_read = ticks_per_read + + def read_position(self, motor: int) -> int: + reported = super().read_position(motor) + if motor == self._hand_motor: + raw = self._positions.get(motor, 0) + self._positions[motor] = (raw + self._ticks_per_read) % ENCODER_RESOLUTION + return reported + + +def _rezeroed_bus(**kwargs) -> HandMovedBus: + """A hand-moved bus whose elbow already carries the seam-evicting offset.""" + bus = HandMovedBus(offsets={ELBOW_MOTOR: EXPECTED_OFFSET}, **kwargs) + bus.open() + return bus + + +# --------------------------------------------------------------------------- +# arm_spec — the arc table and the offset derived from it +# --------------------------------------------------------------------------- + + +def test_elbow_flex_offset_matches_the_spike_arithmetic(): + """The offset is 1073 — the midpoint of the measured unreachable arc (126, 2020).""" + assert arm_spec.rezero_offset(ELBOW) == EXPECTED_OFFSET + + +def test_offset_is_derived_from_the_arc_not_typed(): + """Correct the arc and the offset follows — the two cannot drift apart. + + The far wall has never been measured (nothing could see across the seam), so + the arc WILL be corrected once ``--verify`` measures it. If the offset were a + typed constant, that correction would silently leave it stale. + """ + arc = arm_spec.rezero_arc(ELBOW) + assert arc is not None + assert arm_spec.rezero_offset(ELBOW) == arc.midpoint + + +def test_the_seam_lands_strictly_inside_the_unreachable_arc(): + """The whole point: the seam must go where the joint physically cannot follow.""" + arc = arm_spec.rezero_arc(ELBOW) + offset = arm_spec.rezero_offset(ELBOW) + assert arc.contains(offset) + # ...with real clearance on both sides, not by one tick. + assert offset - arc.low == 947 + assert arc.high - offset == 947 + + +def test_offset_fits_the_registers_sign_magnitude_range(): + """±2047 is the register's whole world; 1073 sits comfortably inside it.""" + assert abs(arm_spec.rezero_offset(ELBOW)) <= arm_spec.MAX_ENCODER_OFFSET + + +def test_arc_arithmetic_reconstructs_the_measured_travel(): + """Travel = 4096 − arc width = 2202 ticks, exactly as the spike derives it.""" + arc = arm_spec.rezero_arc(ELBOW) + assert arc.width == 1894 + assert arc.travel_ticks == 2202 + + +def test_arc_endpoints_are_reachable_and_the_interior_is_not(): + """``contains`` is the OPEN interval — the endpoints are the joint's own walls.""" + arc = arm_spec.rezero_arc(ELBOW) + assert not arc.contains(126) # its rest position, past the wrap + assert not arc.contains(2020) # its measured hard wall + assert arc.contains(127) + assert arc.contains(2019) + assert not arc.contains(3000) # in the travel, on the far side of the seam + + +@pytest.mark.parametrize("joint", ["shoulder_pan", "shoulder_lift", "wrist_flex", "gripper"]) +def test_non_wrapping_joints_are_refused_as_UNNECESSARY(joint): + """Four joints do not wrap at all: there is no seam to evict, and they say so.""" + assert arm_spec.rezero_offset(joint) is None + refusal = arm_spec.rezero_refusal(joint) + assert "does not need a re-zero" in refusal + assert joint in refusal + + +def test_wrist_roll_is_refused_as_IMPOSSIBLE_and_says_why(): + """The distinction that matters: wrist_roll CAN'T be re-zeroed, not "needn't be". + + A re-zero relocates a seam; it can never evict one from a joint whose travel + covers the whole circle, because eviction needs an arc the joint cannot + reach and such a joint has none. Collapsing this into the "you don't need + one" message would teach the operator something false about their arm — and + would make a permanent, provable impossibility read like an unimplemented + feature. + """ + assert arm_spec.rezero_offset("wrist_roll") is None + refusal = arm_spec.rezero_refusal("wrist_roll") + assert "RELOCATES" in refusal + assert "EVICT" in refusal + assert "SOFT LIMIT" in refusal + assert "does not need a re-zero" not in refusal + # And the soft limit it defers to is real and in force. + assert arm_spec.soft_limit("wrist_roll") is not None + + +def test_elbow_flex_is_the_only_re_zeroable_joint(): + rezeroable = [j for j in arm_spec.JOINTS if arm_spec.rezero_offset(j) is not None] + assert rezeroable == [ELBOW] + + +def test_every_joint_gets_either_an_offset_or_a_reason_never_neither(): + """No joint may answer "no" without saying why. Silence is the failure mode.""" + for joint in arm_spec.JOINTS: + offset = arm_spec.rezero_offset(joint) + refusal = arm_spec.rezero_refusal(joint) + assert (offset is None) != (refusal is None) + + +def test_unknown_joint_raises_valueerror_not_a_silent_none(): + for fn in (arm_spec.rezero_arc, arm_spec.rezero_offset, arm_spec.rezero_refusal): + with pytest.raises(ValueError, match="Unknown joint"): + fn("elbow") # nearly right, and therefore worth failing loudly + + +# --- the import-time table guards ----------------------------------------- + + +def test_an_arc_with_no_interior_is_rejected_at_import_time(): + """A one-tick arc has nowhere to PUT the seam — that joint needs a soft limit.""" + with pytest.raises(ValueError, match="nowhere to evict the seam"): + arm_spec._require_evictable_seam({"x": arm_spec.UnreachableArc(low=100, high=101)}) + + +def test_an_arc_whose_midpoint_is_raw_2048_is_rejected_at_import_time(): + """Raw 2048 is the ONE seam placement sign-magnitude-on-bit-11 cannot express.""" + with pytest.raises(ValueError, match=r"outside the register's"): + arm_spec._require_evictable_seam({"x": arm_spec.UnreachableArc(low=1, high=4095)}) + + +def test_a_high_arc_uses_the_NEGATIVE_congruent_offset(): + """Seam at raw 3000 is unrepresentable as +3000, but is fine as −1096. + + Modulo 4096 the register reaches every residue but 2048, and this is how: a + tick above 2047 is expressed as its negative congruent. Getting this wrong + would reject a perfectly good future arc. + """ + assert arm_spec._offset_for_seam_at(3000) == 3000 - 4096 == -1096 + assert abs(-1096) <= arm_spec.MAX_ENCODER_OFFSET + arm_spec._require_evictable_seam({"x": arm_spec.UnreachableArc(low=2900, high=3100)}) + + +def test_malformed_arcs_are_rejected_by_the_dataclass(): + for low, high in ((2020, 126), (100, 100), (-1, 500), (500, 4096)): + with pytest.raises(ValueError, match="Invalid unreachable arc"): + arm_spec.UnreachableArc(low=low, high=high) + + +# --- cross-module single-source pins --------------------------------------- + + +def test_arm_spec_encoder_constants_agree_with_the_bus(): + """``arm_spec`` restates two bus constants because it must not IMPORT the bus. + + A table of physical facts has no business depending on a serial port + (``test_arm_spec_module_never_imports_the_bus`` enforces that). The price is + two restated numbers — and this test is what keeps that price honest, so + they cannot drift. + """ + assert arm_spec.ENCODER_TICKS == ENCODER_RESOLUTION + assert arm_spec.MAX_ENCODER_OFFSET == OFFSET_MAX_MAGNITUDE + + +# --------------------------------------------------------------------------- +# require_rezeroable — answers with no hardware attached +# --------------------------------------------------------------------------- + + +def test_require_rezeroable_returns_the_offset_and_the_arc(): + offset, arc = rezero.require_rezeroable(ELBOW) + assert offset == EXPECTED_OFFSET + assert (arc.low, arc.high) == (126, 2020) + + +def test_require_rezeroable_refuses_wrist_roll_with_the_full_reason(): + """And it does so as a CliError with a remediation — never a bare ValueError.""" + with pytest.raises(CliError) as exc: + rezero.require_rezeroable("wrist_roll") + assert exc.value.code == EXIT_USER_ERROR + assert "RELOCATES" in exc.value.message + assert "SOFT LIMIT" in exc.value.message + assert "elbow_flex" in exc.value.remediation + + +def test_require_rezeroable_rejects_an_unknown_joint_as_a_user_error(): + with pytest.raises(CliError) as exc: + rezero.require_rezeroable("nonsense") + assert exc.value.code == EXIT_USER_ERROR + + +# --------------------------------------------------------------------------- +# plan_rezero — reads only, and refuses rather than guesses +# --------------------------------------------------------------------------- + + +def test_plan_reads_the_live_state_and_writes_nothing(): + bus = FakeBus(positions={ELBOW_MOTOR: 126}) + bus.open() + + plan = rezero.plan_rezero(bus, ELBOW_MOTOR, ELBOW) + + assert plan.current_offset == 0 + assert plan.target_offset == EXPECTED_OFFSET + assert plan.reported_position == 126 + assert plan.raw_position == 126 # factory offset: reported IS raw, no assumption + assert plan.already_applied is False + # Planning is a read. Not one register was touched. + assert bus.register_writes == [] + assert bus.offset_writes == [] + assert bus.torque_writes == [] + + +def test_plan_predicts_the_position_the_spike_predicts(): + """From rest at raw 126, an offset of 1073 must make the servo report 3149. + + Spike §3 step 7: reported = (126 − 1073) mod 4096 = 3149. If this number is + wrong, everything downstream of it is decoration. + """ + bus = FakeBus(positions={ELBOW_MOTOR: 126}) + bus.open() + plan = rezero.plan_rezero(bus, ELBOW_MOTOR, ELBOW) + assert plan.predicted_position == 3149 + + +@pytest.mark.parametrize( + ("raw", "predicted"), + [(2020, 947), (4060, 2987), (4095, 3022), (0, 3023), (126, 3149)], +) +def test_the_whole_travel_becomes_one_contiguous_increasing_interval(raw, predicted): + """Spike §3's table, row by row: 947 -> 2987 -> 3022 -> 3023 -> 3149. + + Strictly increasing, no discontinuity — the reachable set collapses to the + single interval [947, 3149], which a (min, max) pair can honestly describe. + That is the entire deliverable of issue #35. + """ + bus = FakeBus(positions={ELBOW_MOTOR: raw}) + bus.open() + assert rezero.plan_rezero(bus, ELBOW_MOTOR, ELBOW).predicted_position == predicted + + +def test_plan_is_idempotent_on_an_already_re_zeroed_joint(): + """The procedure sends the operator away to power-cycle and come back. + + A second run against an already-re-zeroed joint is therefore the EXPECTED + path, not a mistake, and it must be recognised rather than re-written. + """ + bus = FakeBus(positions={ELBOW_MOTOR: 126}, offsets={ELBOW_MOTOR: EXPECTED_OFFSET}) + bus.open() + + plan = rezero.plan_rezero(bus, ELBOW_MOTOR, ELBOW) + + assert plan.already_applied is True + assert plan.current_offset == EXPECTED_OFFSET + assert plan.reported_position == 3149 # the servo is already reporting corrected + assert plan.raw_position == 126 # ...and we can still find the shaft + assert bus.register_writes == [] + + +def test_plan_refuses_a_servo_holding_an_UNRECOGNISED_offset(): + """We cannot honestly convert its reports to raw ticks, so we do not pretend to. + + Writing a new offset on top of an unknown one would bury the problem in + EEPROM instead of surfacing it. + """ + bus = FakeBus(positions={ELBOW_MOTOR: 126}, offsets={ELBOW_MOTOR: 500}) + bus.open() + + with pytest.raises(CliError) as exc: + rezero.plan_rezero(bus, ELBOW_MOTOR, ELBOW) + + assert exc.value.code == EXIT_ENV_ERROR + assert "already holds an encoder offset of 500" in exc.value.message + assert bus.register_writes == [] # refused BEFORE touching anything + + +def test_plan_refuses_a_joint_that_reports_a_position_it_cannot_physically_hold(): + """A raw position inside the unreachable arc means the arc does not fit this arm. + + Either the servo is not the joint we think it is, or the travel has changed. + Either way the offset about to be written comes from a table that does not + describe the hardware — and writing it would put the seam somewhere the joint + CAN go, making issue #35 worse, persistently, in EEPROM. + """ + bus = FakeBus(positions={ELBOW_MOTOR: 1500}) # dead centre of (126, 2020) + bus.open() + + with pytest.raises(CliError) as exc: + rezero.plan_rezero(bus, ELBOW_MOTOR, ELBOW) + + assert exc.value.code == EXIT_ENV_ERROR + assert "INSIDE the arc" in exc.value.message + assert "Refusing to write" in exc.value.message + assert bus.register_writes == [] + + +def test_raw_from_reported_is_the_identity_at_the_factory_offset(): + """The case the whole procedure is designed around carries NO assumption.""" + assert rezero.raw_from_reported(126, 0) == 126 + assert rezero.raw_from_reported(3149, EXPECTED_OFFSET) == 126 # and it inverts + + +# --------------------------------------------------------------------------- +# apply_rezero — an EEPROM write, and NOT a move +# --------------------------------------------------------------------------- + + +def test_apply_writes_the_offset_and_reads_it_back(): + bus = FakeBus(positions={ELBOW_MOTOR: 126}) + bus.open() + + read_back = rezero.apply_rezero(bus, ELBOW_MOTOR, EXPECTED_OFFSET) + + assert read_back == EXPECTED_OFFSET + assert bus.offset_writes == [{"motor": ELBOW_MOTOR, "offset": EXPECTED_OFFSET}] + + +def test_apply_COMMANDS_NO_MOTION(): + """The load-bearing safety property of this entire task. + + ``elbow_flex`` rests at raw ~126, PAST its wrap. A linear goal — any linear + goal — would rotate it the long way round, through its whole travel, into a + wall. So the write surface must contain no goal position at all, and this + test pins that at the register level rather than trusting the code to + continue not doing it. + """ + bus = FakeBus(positions={ELBOW_MOTOR: 126}) + bus.open() + + rezero.apply_rezero(bus, ELBOW_MOTOR, EXPECTED_OFFSET) + + assert bus.position_writes == [] + assert all(w["addr"] != ADDR_GOAL_POSITION for w in bus.register_writes) + + +def test_apply_never_ENERGISES_the_joint(): + """Torque only ever goes OFF. A servo must not be holding while its frame moves.""" + bus = FakeBus(positions={ELBOW_MOTOR: 126}) + bus.open() + + rezero.apply_rezero(bus, ELBOW_MOTOR, EXPECTED_OFFSET) + + assert bus.torque_writes # it definitely touched torque... + assert all(w["on"] is False for w in bus.torque_writes) # ...only ever downward + torque_values = [w["value"] for w in bus.register_writes if w["addr"] == ADDR_TORQUE_ENABLE] + assert torque_values == [0, 0] # clear_overload, then write_offset's own torque-off + + +def test_apply_performs_the_full_EEPROM_lock_dance(): + """Unlock -> write addr 31 -> re-lock. Skip it and the write REVERTS (PR #21).""" + bus = FakeBus(positions={ELBOW_MOTOR: 126}) + bus.open() + + rezero.apply_rezero(bus, ELBOW_MOTOR, EXPECTED_OFFSET) + + eeprom = [ + (w["addr"], w["value"]) + for w in bus.register_writes + if w["addr"] in (ADDR_LOCK, ADDR_HOMING_OFFSET) + ] + assert eeprom == [ + (ADDR_LOCK, 0), # unlock + (ADDR_HOMING_OFFSET, EXPECTED_OFFSET), # the offset, sign-magnitude on the wire + (ADDR_LOCK, 1), # re-lock + ] + + +def test_apply_writes_addr_31_and_NOTHING_else_in_eeprom(): + """LeRobot's write_calibration also writes min/max angle limits (addr 9/11). + + Copying it wholesale would CLAMP the servo's goals to a narrower window — + shrinking the very reachable set this re-zero exists to recover. The write + surface is pinned to an allow-list so widening it is a deliberate act. + """ + bus = FakeBus(positions={ELBOW_MOTOR: 126}) + bus.open() + + rezero.apply_rezero(bus, ELBOW_MOTOR, EXPECTED_OFFSET) + + touched = {w["addr"] for w in bus.register_writes} + assert touched == {ADDR_TORQUE_ENABLE, ADDR_LOCK, ADDR_HOMING_OFFSET} + + +def test_apply_clears_a_latched_overload_before_the_write(): + """A joint that just fought a wall is EXACTLY the joint you are asked to re-zero. + + ``write_offset``'s first act is a plain ``enable_torque(False)``, which a + latched servo answers with the overload bit still set — so without the + ``clear_overload`` first, the write would raise before it ever opened the + EEPROM. That is not hypothetical: driving into a wall is how ``elbow_flex``'s + arc was measured. + """ + bus = FakeBus(positions={ELBOW_MOTOR: 126}).fail_with_overload_on_op(1) + bus.open() + + read_back = rezero.apply_rezero(bus, ELBOW_MOTOR, EXPECTED_OFFSET) + + assert read_back == EXPECTED_OFFSET # it recovered and wrote + + +def test_a_latched_motor_defeats_a_write_that_SKIPS_clear_overload(): + """The negative control for the test above — proving the guard is load-bearing.""" + bus = FakeBus(positions={ELBOW_MOTOR: 126}).fail_with_overload_on_op(1) + bus.open() + + with pytest.raises(OverloadError): + bus.write_offset(ELBOW_MOTOR, EXPECTED_OFFSET) # the primitive, unguarded + + +def test_an_unrepresentable_offset_is_rejected_before_ANY_wire_traffic(): + """A rejected offset must not leave the joint limp as a side effect of failing.""" + bus = FakeBus(positions={ELBOW_MOTOR: 126}) + bus.open() + + with pytest.raises(CliError) as exc: + rezero.apply_rezero(bus, ELBOW_MOTOR, 2048) + + assert exc.value.code == EXIT_USER_ERROR + # clear_overload ran (that is a torque-OFF, which is safe); the EEPROM never opened. + assert bus.offset_writes == [] + assert all(w["addr"] != ADDR_HOMING_OFFSET for w in bus.register_writes) + assert all(w["addr"] != ADDR_LOCK for w in bus.register_writes) + + +# --- describe_shift: the free, early probe of the open question ------------ + + +def test_shift_matches_the_prediction_when_the_offset_wraps(): + bus = FakeBus(positions={ELBOW_MOTOR: 126}) + bus.open() + plan = rezero.plan_rezero(bus, ELBOW_MOTOR, ELBOW) + rezero.apply_rezero(bus, ELBOW_MOTOR, plan.target_offset) + + shift = rezero.describe_shift(plan, bus.read_position(ELBOW_MOTOR)) + + assert shift["observed_position"] == 3149 + assert shift["as_predicted"] is True + assert shift["in_range"] is True + assert shift["unchanged"] is False + + +def test_shift_catches_the_signed_reading_IMMEDIATELY(): + """Under ``offset_wraps=False`` the servo reports −947 from rest — impossible. + + A position register cannot hold a negative number, so this alone already + proves the corrected position is an unwrapped signed subtraction and the + seam never moved. The sweep is still the proof; this is the free warning + that arrives 30 seconds earlier. + """ + bus = FakeBus(positions={ELBOW_MOTOR: 126}, offset_wraps=False) + bus.open() + plan = rezero.plan_rezero(bus, ELBOW_MOTOR, ELBOW) + rezero.apply_rezero(bus, ELBOW_MOTOR, plan.target_offset) + + shift = rezero.describe_shift(plan, bus.read_position(ELBOW_MOTOR)) + + assert shift["observed_position"] == 126 - EXPECTED_OFFSET == -947 + assert shift["in_range"] is False + assert shift["as_predicted"] is False + + +# --------------------------------------------------------------------------- +# analyse_sweep — pure judgement, no bus, no clock, no servo +# --------------------------------------------------------------------------- + + +def _analyse(positions, offset_in_force=EXPECTED_OFFSET): + return rezero.analyse_sweep( + positions, + joint=ELBOW, + motor=ELBOW_MOTOR, + offset_in_force=offset_in_force, + expected_offset=EXPECTED_OFFSET, + expected_travel=2202, + ) + + +def test_a_clean_full_sweep_is_a_PASS(): + """947 -> 3149, monotonic, no jump: the seam is gone. Issue #35 is fixed.""" + report = _analyse(list(range(947, 3150, 25))) + + assert report.continuous is True + assert report.conclusive is True + assert report.monotonic is True + assert report.seam_evicted is True + assert report.verdict == rezero.VERDICT_SEAM_EVICTED + assert report.failed is False + assert report.largest_jump == 25 + assert (report.minimum, report.maximum) == (947, 3147) + + +def test_a_4095_to_0_wrap_under_a_written_offset_is_the_STOP_condition(): + """The seam is still in the travel while the seam-evicting offset is in force. + + That can only mean the servo does NOT reduce the corrected position modulo + 4096 — so the offset merely relabels positions, and the re-zero achieves + nothing at all. This is the verdict the entire wave exists to be able to + reach. + """ + report = _analyse([4000, 4050, 4090, 5, 50, 100]) + + assert report.continuous is False + assert report.failed is True + assert report.verdict == rezero.VERDICT_SEAM_NOT_EVICTED + assert report.seam_evicted is False + assert report.largest_jump == 4085 + assert report.discontinuities == ((2, 4090, 5),) + assert "STOP" in report.describe() + assert "does NOT reduce" in report.describe() + + +def test_the_MASKED_signed_jump_is_caught_too_and_it_is_why_the_threshold_is_not_2048(): + """The subtlest way the caveat can bite, and the one a lazy threshold misses. + + If the firmware does a plain signed subtraction AND ``read_position``'s + ``& 0x0FFF`` folds the negative result back into range, the discontinuity at + the seam is only ~1949 ticks — comfortably UNDER the tempting 2048 threshold. + A 2048 threshold would call this a pass and tell the operator the fix works. + """ + jump = 3022 - 1073 + assert jump == 1949 < 2048 # the trap, stated + report = _analyse([2900, 2960, 3022, 1073, 1140, 1200]) + + assert report.largest_jump == 1949 + assert rezero.DISCONTINUITY_TICKS <= 1949 + assert report.continuous is False + assert report.failed is True + + +def test_an_impossible_negative_reading_fails_INDEPENDENTLY_of_the_jump(): + """A position register cannot hold a negative value. Seeing one is proof enough. + + Belt and braces on purpose: the jump test and the range test would each catch + the unwrapped-signed firmware alone, and neither is asked to carry it by + itself. + """ + report = _analyse([-947, -900, -850, -800]) + + assert report.out_of_range == (-947, -900, -850, -800) + assert report.continuous is False # despite every delta being a tame 47 ticks + assert report.largest_jump < rezero.DISCONTINUITY_TICKS + assert report.failed is True + assert "IMPOSSIBLE READS" in report.describe() + + +def test_an_unre_zeroed_discontinuous_sweep_is_a_BASELINE_not_a_failure(): + """The bug, photographed. Exactly what a factory elbow_flex should look like.""" + report = _analyse([4000, 4090, 5, 100], offset_in_force=0) + + assert report.rezeroed is False + assert report.continuous is False + assert report.verdict == rezero.VERDICT_SEAM_PRESENT_BASELINE + assert report.failed is False # informative, expected, exit 0 + assert "BASELINE" in report.describe() + assert "That jump IS issue #35" in report.describe() + + +def test_a_short_clean_sweep_is_INCONCLUSIVE_never_a_pass(): + """The most dangerous outcome available, and the one it must never claim. + + A sweep that moved the joint 200 of its 2202 ticks and saw no seam has proved + NOTHING — of course it saw no seam; it never went near where the seam would + be. Reporting that as a pass would close the open question with a lie. + """ + report = _analyse(list(range(1000, 1200, 10))) + + assert report.continuous is True + assert report.rezeroed is True + assert report.conclusive is False + assert report.seam_evicted is False # <- the whole point + assert report.verdict == rezero.VERDICT_INCONCLUSIVE + assert report.failed is False + assert "proves nothing" in report.describe() + assert "one hard stop ALL THE WAY to the other" in report.describe() + + +def test_a_clean_sweep_of_a_joint_that_was_never_re_zeroed_is_INCONCLUSIVE(): + """No offset was in force, so nothing about the offset was tested.""" + report = _analyse(list(range(947, 3150, 25)), offset_in_force=0) + + assert report.continuous is True + assert report.conclusive is True # it covered the travel... + assert report.seam_evicted is False # ...but there was no re-zero to prove + assert report.verdict == rezero.VERDICT_INCONCLUSIVE + assert "was NOT re-zeroed" in report.describe() + + +def test_a_DISCONTINUOUS_sweep_is_conclusive_however_short_it_was(): + """Seeing the seam is proof it is there. No extra travel can un-see it.""" + report = _analyse([4090, 5]) + assert report.span > 0 + assert report.conclusive is True + assert report.failed is True + + +def test_a_hand_that_backs_up_is_reported_but_never_FAILS_the_sweep(): + """A human wobbles. That makes the sweep non-monotonic and changes nothing else. + + Monotonicity is DESCRIPTIVE. The verdict turns on continuity, which a hand + cannot fake in either direction — and a verb that failed an honest sweep + because the operator's grip slipped would teach them to distrust it. + """ + positions = ( + list(range(947, 2000, 25)) + list(range(2000, 1800, -25)) + list(range(1800, 3150, 25)) + ) + report = _analyse(positions) + + assert report.monotonic is False # it went both ways... + assert report.continuous is True # ...but never jumped + assert report.seam_evicted is True # ...so the proof still stands + assert report.verdict == rezero.VERDICT_SEAM_EVICTED + + +def test_encoder_jitter_does_not_count_as_a_reversal(): + """A few ticks of noise is not the hand changing direction.""" + report = _analyse([1000, 1005, 1002, 1030, 1028, 1060]) + assert report.monotonic is True + + +def test_a_single_sample_cannot_be_judged_and_says_so(): + """Continuity is a claim about the change BETWEEN samples. One sample has none. + + Returning a cheerful "no discontinuities found" for a one-sample sweep would + be the exact false pass this module exists to prevent. + """ + with pytest.raises(CliError) as exc: + _analyse([2048]) + assert exc.value.code == EXIT_ENV_ERROR + assert "too few to judge" in exc.value.message + + +def test_the_report_measures_the_far_wall_for_the_first_time(): + """Free, and genuinely new: nothing could see across the seam before. + + The arc table was built on a LOWER BOUND for travel (2202 ticks) because + ``arm explore`` could not drive past the wrap. A passing sweep measures the + real number, and says so — which is what lets the arc, and the offset derived + from it, be corrected. + """ + report = _analyse(list(range(947, 3300, 25))) # a longer travel than we assumed + text = report.describe() + assert "Far wall measured for the first time" in text + assert f"{report.span} ticks" in text + + +def test_report_json_payload_is_complete_and_serialisable(): + import json + + report = _analyse([4000, 4090, 5, 100]) + payload = report.as_dict() + + json.dumps(payload) # must not raise + assert payload["verdict"] == rezero.VERDICT_SEAM_NOT_EVICTED + assert payload["failed"] is True + assert payload["discontinuities"] == [{"index": 1, "before": 4090, "after": 5}] + assert payload["discontinuity_threshold"] == rezero.DISCONTINUITY_TICKS + + +# --------------------------------------------------------------------------- +# sweep — driven against BOTH readings of the undocumented firmware behaviour +# --------------------------------------------------------------------------- + + +def test_sweep_PROVES_the_seam_moved_when_the_offset_wraps(): + """The world we hope we live in: the hand walks the travel, the report is smooth. + + The raw shaft crosses 4095->0 during this sweep — it MUST, that is the whole + travel — and the reported position does not so much as blink. + """ + bus = _rezeroed_bus(offset_wraps=True) + + report = rezero.sweep(bus, ELBOW_MOTOR, ELBOW, samples=90) + + assert report.rezeroed is True + assert report.continuous is True + assert report.conclusive is True + assert report.seam_evicted is True + assert report.verdict == rezero.VERDICT_SEAM_EVICTED + assert report.largest_jump == 25 # the hand's own step, and nothing bigger + assert report.minimum == 947 # the wall, in the corrected frame + assert max(report.samples) >= 3100 # ...all the way past rest + + +def test_sweep_FAILS_LOUDLY_when_the_offset_does_not_wrap(): + """The world the caveat warns about — and the reason this verb exists. + + ``offset_wraps=False`` is the pessimistic reading of the undocumented + firmware behaviour: the offset only RELABELS positions and the discontinuity + stays pinned to the physical angle where the magnet rolls over. The re-zero + achieves nothing, and the operator MUST be told so rather than shipping on + top of it. Under this reading the verification must fail — that failure is + the feature. + """ + bus = _rezeroed_bus(offset_wraps=False) + + report = rezero.sweep(bus, ELBOW_MOTOR, ELBOW, samples=90) + + assert report.rezeroed is True + assert report.continuous is False # <- the seam is STILL in the travel + assert report.seam_evicted is False + assert report.failed is True + assert report.verdict == rezero.VERDICT_SEAM_NOT_EVICTED + assert report.out_of_range # it reported positions no register can hold + assert "STOP" in report.describe() + + +def test_sweep_of_a_FACTORY_joint_shows_the_bug_itself(): + """Run it BEFORE the write and you photograph issue #35: a ~4000-tick jump.""" + bus = HandMovedBus() # offset 0 — a factory servo + bus.open() + + report = rezero.sweep(bus, ELBOW_MOTOR, ELBOW, samples=90) + + assert report.rezeroed is False + assert report.continuous is False + assert report.largest_jump > 4000 # the raw 4095 -> 0 seam, in the open + assert report.verdict == rezero.VERDICT_SEAM_PRESENT_BASELINE + assert report.failed is False # a baseline is not a failure + + +def test_sweep_DE_ENERGISES_the_joint_and_never_re_energises_it(): + """The human's hand is on this joint. It must be limp, and it must STAY limp.""" + bus = _rezeroed_bus() + + rezero.sweep(bus, ELBOW_MOTOR, ELBOW, samples=10) + + assert bus.torque_writes # torque was definitely addressed + assert all(w["on"] is False for w in bus.torque_writes) # only ever downward + assert bus.position_writes == [] # and nothing was ever commanded + + +def test_sweep_clears_a_latched_overload_before_polling(): + """A joint that just stalled against a wall would otherwise refuse to go limp.""" + bus = _rezeroed_bus() + bus.fail_with_overload_on_op(1) + + report = rezero.sweep(bus, ELBOW_MOTOR, ELBOW, samples=10) + + assert len(report.samples) == 10 # it recovered and swept + + +def test_sweep_reports_the_offset_ACTUALLY_in_force_not_the_one_we_hoped_for(): + """The report must say which frame its samples are in, not assume the good one.""" + bus = HandMovedBus(offsets={ELBOW_MOTOR: 0}) + bus.open() + + report = rezero.sweep(bus, ELBOW_MOTOR, ELBOW, samples=10) + + assert report.offset_in_force == 0 + assert report.expected_offset == EXPECTED_OFFSET + assert report.rezeroed is False + + +def test_sweep_invokes_the_on_sample_hook_for_every_poll(): + """The operator needs to SEE the position moving, or they cannot tell they are + driving the joint from merely holding a dead arm.""" + bus = _rezeroed_bus() + seen: list[tuple[int, int]] = [] + + report = rezero.sweep( + bus, ELBOW_MOTOR, ELBOW, samples=12, on_sample=lambda i, p: seen.append((i, p)) + ) + + assert [i for i, _ in seen] == list(range(12)) + assert [p for _, p in seen] == list(report.samples) + + +def test_sweep_rejects_a_sample_count_that_cannot_have_a_delta(): + bus = _rezeroed_bus() + with pytest.raises(CliError) as exc: + rezero.sweep(bus, ELBOW_MOTOR, ELBOW, samples=1) + assert exc.value.code == EXIT_USER_ERROR + + +def test_sweep_refuses_a_joint_that_cannot_be_re_zeroed(): + """Sweeping wrist_roll would measure a seam no offset could ever have moved.""" + bus = _rezeroed_bus() + with pytest.raises(CliError) as exc: + rezero.sweep(bus, 5, "wrist_roll", samples=10) + assert exc.value.code == EXIT_USER_ERROR + assert "RELOCATES" in exc.value.message + + +def test_sweep_does_not_sleep_against_a_fake_bus(): + """A simulated shaft advances per READ, not per second — pacing it would only + make the suite sleep. Same seam as ``gentle_move``'s ``_needs_pacing``.""" + bus = _rezeroed_bus() + assert rezero._needs_pacing(bus) is False + + +# --- samples_for ----------------------------------------------------------- + + +def test_samples_for_converts_a_duration_into_polls(): + assert rezero.samples_for(30.0, 0.05) == 600 + assert rezero.samples_for(1.0, 0.05) == 20 + + +def test_samples_for_rejects_a_duration_too_short_to_have_a_delta(): + with pytest.raises(CliError) as exc: + rezero.samples_for(0.05, 0.05) + assert exc.value.code == EXIT_USER_ERROR + assert "too few to detect a discontinuity" in exc.value.message diff --git a/tests/test_soft_limit_enforcement.py b/tests/test_soft_limit_enforcement.py new file mode 100644 index 0000000..82886a6 --- /dev/null +++ b/tests/test_soft_limit_enforcement.py @@ -0,0 +1,368 @@ +"""The soft limit BINDS: every motion path resolves its bounds through arm_spec. + +Task t9 gave ``wrist_roll`` a :class:`~arm101.hardware.arm_spec.SoftLimit` of +``(100, 3995)`` — a permitted range whose dead arc, ``[0, 100) ∪ (3995, 4095]``, +contains the 4095->0 encoder seam. That made the *table* correct. It did not +make the *arm* safe: nothing read the table. Every move in the codebase sourced +its bounds from the servo's EEPROM ``min_angle``/``max_angle`` registers, which +are the factory ``0-4095`` on every joint of this arm +(``docs/hardware-validation-arm-explore.md``), so ``arm flex wrist_roll --to +4090 --apply`` drove the joint straight into the dead arc and across the seam — +exactly the failure the soft limit exists to prevent. A soft limit nobody reads +is inert data. + +These tests pin the fix from BOTH ends: + +* the pure resolver (:func:`arm101.hardware.arm_spec.resolve_bounds`) computes + the INTERSECTION of the EEPROM range with the soft limit — never a + replacement, so a servo whose EEPROM range is genuinely tighter still wins; +* every call site that turns ``read_info`` into move bounds — ``arm flex`` + (both the compliant and the gentle path), ``arm explore``'s grid, and the + ``demo_sweep`` choreography — actually goes through it, asserted on the goal + positions REALLY written to the bus, not on an intermediate return value. + +And the spec boundary: the soft limit is SOFTWARE-only. No path here may ever +write it into the servo's EEPROM angle-limit registers (addresses 9 and 11) — +measured/derived ranges are pose- and environment-dependent and are never burnt +into hardware. +""" + +from __future__ import annotations + +import argparse +import json +import sys + +import pytest + +from arm101.cli._commands import arm as arm_cmd +from arm101.cli._errors import EXIT_ENV_ERROR, CliError +from arm101.hardware import arm_spec +from arm101.hardware.arm_spec import SOFT_LIMITS, TICK_MAX, TICK_MIN, resolve_bounds +from arm101.hardware.bus import FakeBus +from arm101.hardware.demo import demo_sweep + +#: The one joint with a soft limit, and its permitted range — read from the +#: table rather than hard-coded, so a deliberate retune of the width retunes +#: these tests with it, while the *properties* they assert stay pinned. +_WRIST_ROLL = "wrist_roll" +_SOFT_MIN = SOFT_LIMITS[_WRIST_ROLL].min_tick +_SOFT_MAX = SOFT_LIMITS[_WRIST_ROLL].max_tick + +#: A joint with NO soft limit — the regression guard that the fix did not +#: quietly narrow every joint's travel. +_FREE_JOINT = "shoulder_pan" + +#: STS3215 EEPROM addresses of the angle-limit registers (bus.py ``_INFO``: +#: ``"min_angle": (9, 2)``, ``"max_angle": (11, 2)``). Nothing may write these. +_EEPROM_ANGLE_LIMIT_ADDRS = (9, 10, 11, 12) + + +class _FakeStdin: + """Scripted stdin: non-TTY, so consent resolves to the agent ``--apply`` mode.""" + + def __init__(self, tty: bool = False) -> None: + self._tty = tty + + def isatty(self) -> bool: + return self._tty + + def readline(self) -> str: + return "" + + +def _patch_bus(monkeypatch, fake: FakeBus, port: str = "/dev/ttyACM_fake") -> None: + fake.open() + monkeypatch.setattr(arm_cmd, "_candidate_ports", lambda: [port]) + monkeypatch.setattr(arm_cmd, "_open_bus", lambda _p: fake) + monkeypatch.setattr(sys, "stdin", _FakeStdin()) + + +def _flex_args( + joint=None, + to=None, + demo=False, + gentle=False, + threshold=None, + role="follower", + port=None, + apply=True, + json_mode=False, +): + return argparse.Namespace( + joint=joint, + to=to, + demo=demo, + gentle=gentle, + threshold=threshold, + role=role, + port=port, + apply=apply, + json=json_mode, + ) + + +def _goals(fake: FakeBus, motor: int) -> "list[int]": + """Every goal position ACTUALLY written to *motor* — the ground truth.""" + return [w["position"] for w in fake.position_writes if w["motor"] == motor] + + +# =========================================================================== +# The pure resolver: intersection, not replacement +# =========================================================================== + + +def test_resolve_bounds_applies_the_soft_limit_to_a_wrapping_joint() -> None: + """Factory EEPROM 0-4095 + wrist_roll's soft limit -> the soft limit. + + This is the case that exists on the real arm: read_info returns the + untouched factory range for every joint, so before this resolver the soft + limit had no way to reach the motion path at all. + """ + assert resolve_bounds(_WRIST_ROLL, TICK_MIN, TICK_MAX) == (_SOFT_MIN, _SOFT_MAX) + + +def test_resolve_bounds_leaves_a_joint_without_a_soft_limit_verbatim() -> None: + """A joint absent from SOFT_LIMITS gets its EEPROM bounds back unchanged. + + The regression guard for the whole change: it would be very easy to + "helpfully" clamp every joint into wrist_roll's range and silently steal + 200 ticks of travel from five joints that never had a wrap problem. + """ + assert resolve_bounds(_FREE_JOINT, TICK_MIN, TICK_MAX) == (TICK_MIN, TICK_MAX) + assert resolve_bounds(_FREE_JOINT, 700, 3300) == (700, 3300) + + +def test_resolve_bounds_takes_the_tighter_end_of_each_bound() -> None: + """INTERSECTION, not replacement — a narrower EEPROM bound still wins. + + The soft limit says "never outside (100, 3995)". It does NOT say "always + permit (100, 3995)": if an operator has genuinely narrowed a servo's EEPROM + angle limits (a calibration, a fixture, a cable-routing constraint), those + are a real physical constraint and replacing them with the wider soft limit + would drive the joint somewhere the servo was explicitly configured not to + go. So each end independently takes the tighter of the two. + """ + # Both ends tighter in EEPROM -> EEPROM survives untouched. + assert resolve_bounds(_WRIST_ROLL, 500, 3000) == (500, 3000) + # Low end tighter in the soft limit, high end tighter in EEPROM -> one of each. + assert resolve_bounds(_WRIST_ROLL, 50, 3000) == (_SOFT_MIN, 3000) + # High end tighter in the soft limit, low end tighter in EEPROM -> the mirror. + assert resolve_bounds(_WRIST_ROLL, 500, 4090) == (500, _SOFT_MAX) + + +def test_resolve_bounds_is_idempotent() -> None: + """Resolving an already-resolved range changes nothing. + + Cheap property, but it is what lets a call site route through the resolver + without having to prove it is the only one on the path. + """ + once = resolve_bounds(_WRIST_ROLL, TICK_MIN, TICK_MAX) + assert resolve_bounds(_WRIST_ROLL, *once) == once + + +def test_resolve_bounds_rejects_an_unknown_joint() -> None: + with pytest.raises(ValueError, match="Unknown joint"): + resolve_bounds("elbow_twist", TICK_MIN, TICK_MAX) + + +def test_resolve_bounds_rejects_an_empty_intersection() -> None: + """An EEPROM range lying entirely inside the dead arc is a contradiction. + + ``(0, 50)`` for wrist_roll says "only ever go where the soft limit says + never go". There is no bound this function could return that honours both, + and silently returning the inverted pair would surface downstream as + clamp_goal's misleading "min/max were swapped" error. Fail loudly, here. + """ + with pytest.raises(ValueError, match="no permitted travel"): + resolve_bounds(_WRIST_ROLL, 0, 50) + + +# =========================================================================== +# arm flex — the goal actually written to the bus is soft-limited +# =========================================================================== + + +def test_flex_clamps_wrist_roll_to_the_soft_max_not_into_the_dead_arc(monkeypatch, capsys) -> None: + """``arm flex wrist_roll --to 4090 --apply`` writes 3995, not 4090. + + 4090 sits inside the dead arc, 5 ticks shy of the seam. The EEPROM says + 4095 is fine; the soft limit says otherwise, and the soft limit is what + keeps this joint's travel linear. + """ + fake = FakeBus(positions={5: 2048}) + _patch_bus(monkeypatch, fake) + + arm_cmd.cmd_arm_flex(_flex_args(joint=_WRIST_ROLL, to=4090, json_mode=True)) + + assert _goals(fake, 5) == [_SOFT_MAX] + payload = json.loads(capsys.readouterr().out) + assert payload["move"]["clamped_target"] == _SOFT_MAX + assert payload["move"]["was_clamped"] is True + + +def test_flex_clamps_wrist_roll_to_the_soft_min_at_the_low_end(monkeypatch) -> None: + """The mirror case: ``--to 5`` is in the dead arc on the other side of the seam.""" + fake = FakeBus(positions={5: 2048}) + _patch_bus(monkeypatch, fake) + + arm_cmd.cmd_arm_flex(_flex_args(joint=_WRIST_ROLL, to=5)) + + assert _goals(fake, 5) == [_SOFT_MIN] + + +def test_flex_gentle_path_is_soft_limited_too(monkeypatch) -> None: + """The gentle path resolves bounds at the same call site — every write stays inside. + + ``gentle_move`` steps toward the target and may back off on contact; both + the stepped goals and any retreat write are clamped against the bounds it + was handed, so soft-limiting that ONE pair covers the whole move. + """ + fake = FakeBus(positions={5: 3900}) + _patch_bus(monkeypatch, fake) + + arm_cmd.cmd_arm_flex(_flex_args(joint=_WRIST_ROLL, to=4090, gentle=True)) + + written = _goals(fake, 5) + assert written, "the gentle move wrote no goal at all" + assert max(written) <= _SOFT_MAX + assert min(written) >= _SOFT_MIN + + +def test_flex_does_not_narrow_a_joint_without_a_soft_limit(monkeypatch) -> None: + """shoulder_pan still reaches 4090 — the EEPROM bounds are used verbatim. + + The regression guard, end-to-end: proves the fix did not quietly apply + wrist_roll's dead arc to joints that have no wrap problem. + """ + fake = FakeBus(positions={1: 2048}) + _patch_bus(monkeypatch, fake) + + arm_cmd.cmd_arm_flex(_flex_args(joint=_FREE_JOINT, to=4090)) + + assert _goals(fake, 1) == [4090] + + +def test_flex_honours_a_tighter_eeprom_bound_over_the_soft_limit(monkeypatch) -> None: + """Intersection at the CLI layer: a narrower EEPROM max wins over the soft max.""" + fake = FakeBus(positions={5: 2048}, info={5: {"min_angle": 0, "max_angle": 3000}}) + _patch_bus(monkeypatch, fake) + + arm_cmd.cmd_arm_flex(_flex_args(joint=_WRIST_ROLL, to=4090)) + + assert _goals(fake, 5) == [3000] + + +def test_flex_reports_an_impossible_bound_as_a_cli_error(monkeypatch) -> None: + """A servo configured entirely inside the dead arc fails with a CliError, not a traceback.""" + fake = FakeBus(positions={5: 20}, info={5: {"min_angle": 0, "max_angle": 50}}) + _patch_bus(monkeypatch, fake) + + with pytest.raises(CliError) as exc: + arm_cmd.cmd_arm_flex(_flex_args(joint=_WRIST_ROLL, to=30)) + + assert exc.value.code == EXIT_ENV_ERROR + assert _goals(fake, 5) == [], "no goal may be written when the bounds are impossible" + + +# =========================================================================== +# arm explore — the grid's bounds are the soft-limited range +# =========================================================================== + + +def test_grid_spec_bounds_are_soft_limited_for_wrist_roll(monkeypatch) -> None: + """``_build_grid_spec`` seeds wrist_roll's bounds from the resolver, not raw EEPROM. + + The explore engine takes EVERY move bound it ever uses from + ``GridSpec.bounds`` (flood-fill neighbours and multi-joint escape probes + both read ``spec.bounds[joint]``), so soft-limiting the grid is what stops + exploration from walking a cell into the dead arc. + """ + fake = FakeBus(positions={i: 2048 for i in range(1, 7)}) + fake.open() + + spec = arm_cmd._build_grid_spec(fake, "follower", 512) + + wrist_roll_index = arm_spec.JOINTS.index(_WRIST_ROLL) + assert spec.bounds[wrist_roll_index] == (_SOFT_MIN, _SOFT_MAX) + for index, joint in enumerate(arm_spec.JOINTS): + if joint != _WRIST_ROLL: + assert spec.bounds[index] == (TICK_MIN, TICK_MAX), f"{joint} was narrowed" + + +def test_grid_spec_origin_is_pulled_out_of_the_dead_arc(monkeypatch) -> None: + """A wrist_roll resting AT the seam yields a grid origin inside the permitted range. + + The t9 hardware run found wrist_roll parked at raw tick 4 — sitting on the + seam. The origin is clamped into the joint's bounds, and now those bounds + are the soft-limited ones, so the flood-fill starts from a cell it is + allowed to be in rather than from inside its own dead arc. + """ + fake = FakeBus(positions={i: 2048 for i in range(1, 7)} | {5: 4}) + fake.open() + + spec = arm_cmd._build_grid_spec(fake, "follower", 512) + + assert spec.origin[arm_spec.JOINTS.index(_WRIST_ROLL)] == _SOFT_MIN + + +# =========================================================================== +# demo_sweep — the scripted choreography is soft-limited too +# =========================================================================== + + +def test_demo_sweep_never_targets_the_dead_arc(monkeypatch) -> None: + """A demo sweep of wrist_roll from near the seam stays inside the permitted range. + + ``demo_sweep`` centres a sub-range on the joint's CURRENT position and + clamps it against the joint's bounds. Starting at tick 4 with the factory + EEPROM range, that low target clamped to 0 — straight into the dead arc, + on the far side of the seam. With the resolver it clamps to 100 instead. + """ + fake = FakeBus(positions={5: 4}) + fake.open() + + report = demo_sweep(fake, {_WRIST_ROLL: 5}, allow_motion=True) + + joint_report = report["joints"][_WRIST_ROLL] + assert (joint_report["min_angle"], joint_report["max_angle"]) == (_SOFT_MIN, _SOFT_MAX) + for goal in _goals(fake, 5): + assert _SOFT_MIN <= goal <= _SOFT_MAX, f"demo sweep wrote {goal} into the dead arc" + + +def test_demo_sweep_leaves_an_unlimited_joint_on_its_full_range(monkeypatch) -> None: + """shoulder_pan's demo sub-range is still computed from the full EEPROM span.""" + fake = FakeBus(positions={1: 2048}) + fake.open() + + report = demo_sweep(fake, {_FREE_JOINT: 1}, allow_motion=True) + + joint_report = report["joints"][_FREE_JOINT] + assert (joint_report["min_angle"], joint_report["max_angle"]) == (TICK_MIN, TICK_MAX) + + +# =========================================================================== +# Spec boundary: the soft limit is SOFTWARE-only, never burnt into EEPROM +# =========================================================================== + + +def test_no_motion_path_writes_the_soft_limit_into_the_servo(monkeypatch) -> None: + """Not one register write lands on the EEPROM angle-limit addresses (9/11). + + The explicit spec boundary of the whole soft-limit line of work: measured + and derived ranges are pose- and environment-dependent, so they live in + ``arm_spec`` and in the reachability map — NEVER burnt into a servo's + EEPROM, where they would silently outlive the pose that produced them and + could not be undone by reinstalling the software. + """ + fake = FakeBus(positions={5: 2048}) + _patch_bus(monkeypatch, fake) + + arm_cmd.cmd_arm_flex(_flex_args(joint=_WRIST_ROLL, to=4090, gentle=True)) + + touched = {w["addr"] for w in fake.register_writes} + assert not touched & set(_EEPROM_ANGLE_LIMIT_ADDRS), ( + "a motion path wrote the servo's EEPROM angle-limit registers: " + f"{sorted(touched & set(_EEPROM_ANGLE_LIMIT_ADDRS))}" + ) + assert fake.eeprom_writes == [] diff --git a/uv.lock b/uv.lock index 1eee746..626df8c 100644 --- a/uv.lock +++ b/uv.lock @@ -4,7 +4,7 @@ requires-python = ">=3.12" [[package]] name = "arm101-cli" -version = "0.20.0" +version = "0.21.0" source = { editable = "." } [package.optional-dependencies]