From 5c444302c1130a0375af60f6631caecad684ccf5 Mon Sep 17 00:00:00 2001 From: Khalil Meftah Date: Tue, 28 Apr 2026 18:40:48 +0200 Subject: [PATCH] feat(so_follower): synchronize goal position with present position to prevent positional error during torque re-enablement --- src/lerobot/robots/so_follower/so_follower.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/lerobot/robots/so_follower/so_follower.py b/src/lerobot/robots/so_follower/so_follower.py index 0651f566c..9dc5bd97f 100644 --- a/src/lerobot/robots/so_follower/so_follower.py +++ b/src/lerobot/robots/so_follower/so_follower.py @@ -168,6 +168,12 @@ class SOFollower(Robot): self.bus.write("Protection_Current", motor, 250) # 50% of max current to avoid burnout self.bus.write("Overload_Torque", motor, 25) # 25% torque when overloaded + # Set Goal_Position = Present_Position while torque is still disabled so + # that when torque is re-enabled at the end of this block the motors have + # zero positional error and do not snap to a stale register value. + present = self.bus.sync_read("Present_Position") + self.bus.sync_write("Goal_Position", present) + def setup_motors(self) -> None: for motor in reversed(self.bus.motors): input(f"Connect the controller board to the '{motor}' motor only and press enter.")