fix(tools): a servo's Present_Position carries a sign on the pose tool's read - #3863
Conversation
…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.
yinsong1986
left a comment
There was a problem hiding this comment.
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_packetoutput vs. the codec's builders — the refactor is behaviour-preserving on the TX side. serial_tool'sallow_broadcast=Trueon the reply-less writes preserves the old builder's acceptance of 0xFE, and_option_error/_motor_id_errorstill validatemotor_idbefore the port opens; any residualValueErrorfrom 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 FFheaders. No host paths, no non-ASCII, changelog entry and docs included.
What
drivers/feetech/protocolowns this wire format, yetpose_toolandserial_tooleach carried abuild_feetech_packetof 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, asSIGN_BITand lerobot'sSTS_SMS_SERIES_ENCODINGS_TABLEdeclare and the driver's bus reads it. The pose tool decoded the whole field as magnitude.0x08000x80010x80640x8320shoulder_panspans -180..180 degrees, so every bold number is a position the joint cannot hold, quoted as a measurement by a path that bounds nothing.Why
One authority for the format (#3818, 0.8-0.9): both tools now frame through
read_packet/write_packet/ping_packetand name registers fromRegister.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 onmain, 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,mypyclean on 2205 files; 3428 passed across the servo scope;scripts/check_whole_tree_graders.py6190 passed / 78 skipped.