Move encoding functions to encoding_utils

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