From 63e2a2e129d92353ba6145c697aa61726a6ae4ea Mon Sep 17 00:00:00 2001 From: Pepijn Date: Tue, 1 Jul 2025 16:32:43 +0200 Subject: [PATCH] fix: change to actual degrees --- lerobot/common/motors/motors_bus.py | 14 ++++---- .../so101_follower_torque/so101_follower_t.py | 35 +++++++------------ lerobot/grav_comp.py | 2 +- 3 files changed, 21 insertions(+), 30 deletions(-) diff --git a/lerobot/common/motors/motors_bus.py b/lerobot/common/motors/motors_bus.py index 7ac9e6813..21efb6c45 100644 --- a/lerobot/common/motors/motors_bus.py +++ b/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 diff --git a/lerobot/common/robots/so101_follower_torque/so101_follower_t.py b/lerobot/common/robots/so101_follower_torque/so101_follower_t.py index 1a413a8b1..ef964a0b5 100644 --- a/lerobot/common/robots/so101_follower_torque/so101_follower_t.py +++ b/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) diff --git a/lerobot/grav_comp.py b/lerobot/grav_comp.py index 6c915ad35..6ab5b8104 100644 --- a/lerobot/grav_comp.py +++ b/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()