Fix incorrect sign-magnitude handling for STS/SMS series encodings

[nahakiole]

What this does

We noticed that in our sm8512bl servos that after moving below zero with torque disabled we get a jump to over ~327xx. After some debugging we noticed that the integer that is sent is signed, which is documented in the driver code of the feetech python library: https://gitee.com/ftservo/FTServo_Python/blob/main/scservo_sdk/sms_sts.py

This pull request fixes the incorrect sign-magnitude handling for STS/SMS series encodings so values don’t “wrap” to ~32k when crossing zero. Specifically, we update the encoding bit mapping used for sign-magnitude fields:

STS_SMS_SERIES_ENCODINGS_TABLE = {
    "Homing_Offset": 11,
    "Goal_Position": 15,
    "Goal_Velocity": 15,
    "Goal_Speed": 15,
    "Present_Position": 15,
    "Present_Velocity": 15,
    "Present_Speed": 15,
}

This was also noticed by @MoffKalast for the STS3215s servos on the lerobot discord (video is from them): https://discord.com/channels/1216765309076115607/1237741463832363039/1427617779267403897

wat.mp4

Why this matters: on SM8512BL and STS3215 we observed the present position flipping to >32768 after going below 0.

Using the proper sign-magnitude bit fixes the wrap.

References:

• Feetech’s Python SDK writes position = scs_toscs(position, 15) and reads scs_tohost(..., 15) for position and speed, i.e., sign bit at 15. Same for wheel speed. ([gitee.com][1])
• Utility for Feetech SCS/STS servo (https://github.com/Kotakku/FT_SCServo_Debug_Qt/blob/master/servo/sms_sts.h)

How it was tested

• Reproduced the wrap on STS3215 and SM8512BL by moving manually with zero torque through zero and reading back Present_Position.
• Patched the encoding table to use bit 15 and re-read values while moving through negative to positive range. Confirmed continuous monotonic readings with no 32k jump.

How to checkout & try? (for the reviewer)

You should see no jump to ~327xx when you cross negative to positive.

from time import sleep

from lerobot.motors import Motor, MotorNormMode
from lerobot.motors.feetech import FeetechMotorsBus
from lerobot.motors import MotorCalibration

bus = FeetechMotorsBus(
    port="/dev/tty.usbserial-110",
    motors={
        "shoulder_pitch": Motor(1, "sm8512bl", MotorNormMode.RANGE_0_100),
    })

bus.connect()
bus.write_calibration(calibration_dict={
    "shoulder_pitch": MotorCalibration(  id=1, drive_mode=1, homing_offset=0,
                                         range_min=0, range_max=4096),
})

bus.disable_torque(motors=["shoulder_pitch"])
while True:
    print("Status")
    for motor in bus.motors:
        position = bus.read("Present_Position", motor, normalize=False, num_retry=4)
        print(f"{motor}: {position}")
    sleep(1)

I've used the above script to test this on our sm8512bl and the issue is gone.

---

Notes and open questions

• Normalization expectations. Some users expect angles in [0, MODEL_RESOLUTION) rather than signed host units. After this fix, values are decoded correctly into signed host units. If your public API promises [0..4095] (or model-specific resolution), _normalize should map signed values with:

  normalized = (raw_signed % MODEL_RESOLUTION)
  

  and document whether positions are returned as signed ticks vs modulo resolution

Feel free to rewrite this, if you have a better solution that fits the library better, but I've wanted to document this somewhere and I think this addition doesn't make things worse. :)

[Copilot]

Pull Request Overview

This PR fixes incorrect sign-magnitude bit handling in Feetech STS/SMS series servo encodings to prevent position values from wrapping to ~32k when crossing zero.

• Updates the sign-magnitude encoding table to use bit 15 for position and speed fields
• Fixes position reading jumps observed on SM8512BL and STS3215 servos when moving through negative values
• Aligns encoding with Feetech's official Python SDK implementation

---

_{Tip: Customize your code reviews with copilot-instructions.md. Create the file or learn how to get started.}

[imstevenpmwork]

This looks good. While I haven't tested it personally, the community who reported the issue has confirmed it works, and it's a minor change.