Skip to content

fix(tools): a servo's Present_Position carries a sign on the pose tool's read - #3863

Merged
cagataycali merged 2 commits into
strands-labs:mainfrom
cagataycali:feetech-one-frame-builder
Sep 18, 2026
Merged

cagataycali merged 2 commits into
strands-labs:mainfrom
cagataycali:feetech-one-frame-builder

Conversation

@cagataycali

Copy link
Copy Markdown
Member

What

drivers/feetech/protocol owns this wire format, yet pose_tool and serial_tool each carried a build_feetech_packet of their own, with the register address as a hex literal at every call site. Present_Position (0x38) is sign-magnitude on the STS/SMS series - bit 15 is the direction, as SIGN_BIT and lerobot's STS_SMS_SERIES_ENCODINGS_TABLE declare and the driver's bus reads it. The pose tool decoded the whole field as magnitude.

wire word counts reported before reported after
0x0800 2048 0.04 deg 0.04 deg
0x8001 -1 2700.79 deg -180.09 deg
0x8064 -100 2709.49 deg -188.79 deg
0x8320 -800 2771.03 deg -250.33 deg

shoulder_pan spans -180..180 degrees, so every bold number is a position the joint cannot hold, quoted as a measurement by a path that bounds nothing.

pose_tool Present_Position decode, measured before and after

Why

One authority for the format (#3818, 0.8-0.9): both tools now frame through read_packet / write_packet / ping_packet and name registers from Register.

added deleted
package 44 40
tests 228 15

The addition is the new pin; a duplicate framing test it replaces is deleted.

Tests

tests/tools/test_feetech_tool_frames_come_from_the_codec.py: 6 failed / 6 passed on main, 12 pass here. Every frame is byte-identical to the codec's (the control - the sign is the only behaviour that moved), and one cell refuses a hand-rolled header in either module. ruff, ruff format, mypy clean on 2205 files; 3428 passed across the servo scope; scripts/check_whole_tree_graders.py 6190 passed / 78 skipped.

…l's read

strands_robots.drivers.feetech.protocol owns this wire format, and the two
tools that drive a Feetech bus each carried a build_feetech_packet of their
own - six lines assembling FF FF ID LEN INST <params> CHK, with the register
address spelled as a hex literal at every call site.

They disagreed in the half that is not the frame. Present_Position (0x38) is
sign-magnitude on the STS/SMS series: bit 15 is the direction, which is what
SIGN_BIT declares and what the driver's bus reads. The pose tool decoded the
whole field as a magnitude, so a servo reporting a joint just past its homing
zero - a routine reading on a calibrated arm - was quoted as 2709.49 degrees
on a joint that spans 360.

Both tools now frame through read_packet / write_packet / ping_packet and name
registers from Register, and the read takes its sign from SIGN_BIT. Every frame
is byte-identical to the one sent before, which is what leaves the sign as the
only behaviour that moved.
@cagataycali
cagataycali enabled auto-merge (squash) September 18, 2026 17:16

@yinsong1986 yinsong1986 left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Summary

This PR fixes a real correctness bug in pose_tool.read_motor_position: Present_Position (0x38) is sign-magnitude on the STS/SMS series (bit 15 = direction), and the tool decoded the full 16-bit field as unsigned magnitude, so a joint just past its homing zero (e.g. 0x8064 = -100 counts) was reported as ~2709 deg on a joint that spans -180..180. The fix routes the decode through the codec's decode_sign_magnitude + SIGN_BIT[Register.PRESENT_POSITION], matching lerobot's STS_SMS_SERIES_ENCODINGS_TABLE and the native driver bus. Alongside the fix, both pose_tool and serial_tool drop their private build_feetech_packet copies and frame through the codec's read_packet / write_packet / ping_packet with registers named from Register. I independently verified byte-identity of every migrated frame (goal-position write, torque-release write, position read, ping) against the deleted builder, and the sign decode against the sign-magnitude convention — the frames on the wire are unchanged and the sign is the only behaviour that moved. Read path only, so no wire-format or persisted-schema one-way door.

What's good

  • Byte-identity of all migrated frames confirmed by independent recomputation of the old build_feetech_packet output vs. the codec's builders — the refactor is behaviour-preserving on the TX side.
  • serial_tool's allow_broadcast=True on the reply-less writes preserves the old builder's acceptance of 0xFE, and _option_error / _motor_id_error still validate motor_id before the port opens; any residual ValueError from the codec is absorbed by the dispatch-level handler into the {"status": "error"} contract (per AGENTS.md, action handlers return error dicts).
  • New codec builders raise on out-of-range IDs/values where the old inline builder silently emitted malformed bytes — a strictness improvement, not a regression, and both tools' existing error contracts (False/None/error dict) absorb it.
  • Regression pins are thorough: frame byte-identity per verb, the sign bit graded against lerobot's vendor table, the consequence asserted in the caller's unit without magic floats, and an AST-based anti-recurrence pin against hand-rolled FF FF headers. No host paths, no non-ASCII, changelog entry and docs included.

@cagataycali
cagataycali merged commit c9a2537 into strands-labs:main Sep 18, 2026
3 checks passed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

2 participants