From 8503e8e1660d1a3cb6a932aab9d5b5e38ee2442b Mon Sep 17 00:00:00 2001
From: Simon Alibert <simon.alibert@huggingface.co>
Date: Mon, 31 Mar 2025 00:35:31 +0200
Subject: [PATCH] Move encoding functions to encoding_utils

---
 lerobot/common/motors/dynamixel/dynamixel.py | 27 +---------
 lerobot/common/motors/feetech/feetech.py     | 25 +---------
 lerobot/common/utils/encoding_utils.py       | 52 ++++++++++++++++++++
 3 files changed, 56 insertions(+), 48 deletions(-)
 create mode 100644 lerobot/common/utils/encoding_utils.py

diff --git a/lerobot/common/motors/dynamixel/dynamixel.py b/lerobot/common/motors/dynamixel/dynamixel.py
index f7e4569c..e6d6808c 100644
--- a/lerobot/common/motors/dynamixel/dynamixel.py
+++ b/lerobot/common/motors/dynamixel/dynamixel.py
@@ -22,6 +22,8 @@ import logging
 from copy import deepcopy
 from enum import Enum
 
+from lerobot.common.utils.encoding_utils import decode_twos_complement, encode_twos_complement
+
 from ..motors_bus import Motor, MotorsBus, NameOrID, Value
 from .tables import (
     AVAILABLE_BAUDRATES,
@@ -41,31 +43,6 @@ CONVERT_UINT32_TO_INT32_REQUIRED = ["Goal_Position", "Present_Position"]
 logger = logging.getLogger(__name__)
 
 
-def encode_twos_complement(value: int, n_bytes: int):
-    if value >= 0:
-        return value
-
-    bit_width = n_bytes * 8
-    min_val = -(1 << (bit_width - 1))
-    max_val = (1 << (bit_width - 1)) - 1
-
-    if not (min_val <= value <= max_val):
-        raise ValueError(
-            f"Value {value} out of range for {n_bytes}-byte two's complement: [{min_val}, {max_val}]"
-        )
-
-    return (1 << bit_width) + value
-
-
-def decode_twos_complement(value: int, n_bytes: int) -> int:
-    # https://en.wikipedia.org/wiki/Two%27s_complement
-    bits = n_bytes * 8
-    sign_bit = 1 << (bits - 1)
-    if value & sign_bit:
-        value -= 1 << bits
-    return value
-
-
 class OperatingMode(Enum):
     # DYNAMIXEL only controls current(torque) regardless of speed and position. This mode is ideal for a
     # gripper or a system that only uses current(torque) control or a system that has additional
diff --git a/lerobot/common/motors/feetech/feetech.py b/lerobot/common/motors/feetech/feetech.py
index 6eb37314..c0bd41fd 100644
--- a/lerobot/common/motors/feetech/feetech.py
+++ b/lerobot/common/motors/feetech/feetech.py
@@ -17,6 +17,8 @@ from copy import deepcopy
 from enum import Enum
 from pprint import pformat
 
+from lerobot.common.utils.encoding_utils import decode_sign_magnitude, encode_sign_magnitude
+
 from ..motors_bus import Motor, MotorsBus, NameOrID, Value
 from .tables import (
     AVAILABLE_BAUDRATES,
@@ -35,29 +37,6 @@ DEFAULT_TIMEOUT_MS = 1000
 logger = logging.getLogger(__name__)
 
 
-def encode_sign_magnitude(value: int, sign_bit_index: int):
-    """
-    https://en.wikipedia.org/wiki/Signed_number_representations#Sign%E2%80%93magnitude
-    """
-    max_magnitude = (1 << sign_bit_index) - 1
-    magnitude = abs(value)
-    if magnitude > max_magnitude:
-        raise ValueError(f"Magnitude {magnitude} exceeds {max_magnitude} (max for {sign_bit_index=})")
-
-    direction_bit = 1 if value < 0 else 0
-    return (direction_bit << sign_bit_index) | magnitude
-
-
-def decode_sign_magnitude(encoded_value: int, sign_bit_index: int):
-    """
-    https://en.wikipedia.org/wiki/Signed_number_representations#Sign%E2%80%93magnitude
-    """
-    direction_bit = (encoded_value >> sign_bit_index) & 1
-    magnitude_mask = (1 << sign_bit_index) - 1
-    magnitude = encoded_value & magnitude_mask
-    return -magnitude if direction_bit else magnitude
-
-
 class OperatingMode(Enum):
     # position servo mode
     POSITION = 0
diff --git a/lerobot/common/utils/encoding_utils.py b/lerobot/common/utils/encoding_utils.py
new file mode 100644
index 00000000..5f09297b
--- /dev/null
+++ b/lerobot/common/utils/encoding_utils.py
@@ -0,0 +1,52 @@
+def encode_sign_magnitude(value: int, sign_bit_index: int):
+    """
+    https://en.wikipedia.org/wiki/Signed_number_representations#Sign%E2%80%93magnitude
+    """
+    max_magnitude = (1 << sign_bit_index) - 1
+    magnitude = abs(value)
+    if magnitude > max_magnitude:
+        raise ValueError(f"Magnitude {magnitude} exceeds {max_magnitude} (max for {sign_bit_index=})")
+
+    direction_bit = 1 if value < 0 else 0
+    return (direction_bit << sign_bit_index) | magnitude
+
+
+def decode_sign_magnitude(encoded_value: int, sign_bit_index: int):
+    """
+    https://en.wikipedia.org/wiki/Signed_number_representations#Sign%E2%80%93magnitude
+    """
+    direction_bit = (encoded_value >> sign_bit_index) & 1
+    magnitude_mask = (1 << sign_bit_index) - 1
+    magnitude = encoded_value & magnitude_mask
+    return -magnitude if direction_bit else magnitude
+
+
+def encode_twos_complement(value: int, n_bytes: int):
+    """
+    https://en.wikipedia.org/wiki/Signed_number_representations#Two%27s_complement
+    """
+
+    bit_width = n_bytes * 8
+    min_val = -(1 << (bit_width - 1))
+    max_val = (1 << (bit_width - 1)) - 1
+
+    if not (min_val <= value <= max_val):
+        raise ValueError(
+            f"Value {value} out of range for {n_bytes}-byte two's complement: [{min_val}, {max_val}]"
+        )
+
+    if value >= 0:
+        return value
+
+    return (1 << bit_width) + value
+
+
+def decode_twos_complement(value: int, n_bytes: int) -> int:
+    """
+    https://en.wikipedia.org/wiki/Signed_number_representations#Two%27s_complement
+    """
+    bits = n_bytes * 8
+    sign_bit = 1 << (bits - 1)
+    if value & sign_bit:
+        value -= 1 << bits
+    return value