fix: change to actual degrees

lerobot/common/motors/motors_bus.py

@@ -83,6 +83,9 @@ class MotorNormMode(str, Enum):
     DEGREES = "degrees"
 
 
+COUNT_TO_DEG = 0.087  # 1 encoder count = 0.087 °
+
+
 @dataclass
 class MotorCalibration:
     id: int
@@ -793,9 +796,8 @@ class MotorsBus(abc.ABC):
                 norm = ((bounded_val - min_) / (max_ - min_)) * 100
                 normalized_values[id_] = 100 - norm if drive_mode else norm
             elif self.motors[motor].norm_mode is MotorNormMode.DEGREES:
-                mid = (min_ + max_) / 2
-                max_res = self.model_resolution_table[self._id_to_model(id_)] - 1
-                normalized_values[id_] = (val - mid) * 360 / max_res
+                deg = bounded_val * COUNT_TO_DEG
+                normalized_values[id_] = -deg if drive_mode else deg
             else:
                 raise NotImplementedError
 
@@ -823,9 +825,9 @@ class MotorsBus(abc.ABC):
                 bounded_val = min(100.0, max(0.0, val))
                 unnormalized_values[id_] = int((bounded_val / 100) * (max_ - min_) + min_)
             elif self.motors[motor].norm_mode is MotorNormMode.DEGREES:
-                mid = (min_ + max_) / 2
-                max_res = self.model_resolution_table[self._id_to_model(id_)] - 1
-                unnormalized_values[id_] = int((val * max_res / 360) + mid)
+                val = -val if drive_mode else val
+                raw_counts = int(round(val / COUNT_TO_DEG))
+                unnormalized_values[id_] = raw_counts
             else:
                 raise NotImplementedError
 

lerobot/common/robots/so101_follower_torque/so101_follower_t.py

@@ -66,20 +66,9 @@ class SO101FollowerT(Robot):
             for k, t in torque.items()
         }
 
-    def _counts_to_rad(self, raw: dict[str, int]) -> dict[str, float]:
-        """Convert "Present_Position" counts to rad."""
-        out: dict[str, float] = {}
-        for motor, cnt in raw.items():
-            out[motor] = cnt * self._COUNT_TO_RAD
-        return out
-
-    def _rad_to_counts(self, pos: dict[str, float]) -> dict[str, int]:
-        """Convert radians to position counts."""
-        out: dict[str, int] = {}
-        for motor, rad in pos.items():
-            cnt = int(round(rad / self._COUNT_TO_RAD))
-            out[motor] = cnt
-        return out
+    def _deg_to_rad(self, deg: dict[str, float | int]) -> dict[str, float]:
+        """GDegrees to radians."""
+        return {m: math.radians(float(v)) for m, v in deg.items()}
 
     def __init__(self, config: SO101FollowerTConfig):
         super().__init__(config)
@@ -87,12 +76,12 @@ class SO101FollowerT(Robot):
         self.bus = FeetechMotorsBus(
             port=self.config.port,
             motors={
-                "shoulder_pan": Motor(1, "hls3625", MotorNormMode.RANGE_M100_100),
-                "shoulder_lift": Motor(2, "hls3625", MotorNormMode.RANGE_M100_100),
-                "elbow_flex": Motor(3, "hls3625", MotorNormMode.RANGE_M100_100),
-                "wrist_flex": Motor(4, "hls3625", MotorNormMode.RANGE_M100_100),
-                "wrist_roll": Motor(5, "hls3625", MotorNormMode.RANGE_M100_100),
-                "gripper": Motor(6, "hls3625", MotorNormMode.RANGE_0_100),
+                "shoulder_pan": Motor(1, "hls3625", MotorNormMode.DEGREES),
+                "shoulder_lift": Motor(2, "hls3625", MotorNormMode.DEGREES),
+                "elbow_flex": Motor(3, "hls3625", MotorNormMode.DEGREES),
+                "wrist_flex": Motor(4, "hls3625", MotorNormMode.DEGREES),
+                "wrist_roll": Motor(5, "hls3625", MotorNormMode.DEGREES),
+                "gripper": Motor(6, "hls3625", MotorNormMode.DEGREES),
             },
             calibration=self.calibration,
         )
@@ -201,11 +190,11 @@ class SO101FollowerT(Robot):
         start = time.perf_counter()
 
         # Positions
-        pos_counts = self.bus.sync_read("Present_Position", normalize=False, num_retry=10)
-        pos_rad = self._counts_to_rad(pos_counts)
+        pos_deg = self.bus.sync_read("Present_Position", num_retry=10)
+        pos_rad = self._deg_to_rad(pos_deg)
         obs_dict = {f"{m}.pos": r for m, r in pos_rad.items()}
 
-        # Currents (counts) → torque (N·m)
+        # Currents to torque (Nm)
         curr_raw = self.bus.sync_read("Present_Current", normalize=False, num_retry=10)
         torque_nm = self._current_to_torque_nm({f"{m}.effort": v for m, v in curr_raw.items()})
         obs_dict.update(torque_nm)

lerobot/grav_comp.py

@@ -40,7 +40,7 @@ pin_robot.initViewer(open=True)
 pin_robot.loadViewerModel()
 pin_robot.display(pin_robot.q0)
 
-cfg = SO101FollowerTConfig(port="/dev/tty.usbmodem58760431551", id="my_awesome_follower_arm_t")
+cfg = SO101FollowerTConfig(port="/dev/tty.usbmodem58760431551", id="follower_t")
 real = SO101FollowerT(cfg)
 real.connect()