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.")