Update calibration gui

lerobot/common/motors/calibration_gui.py

@@ -1,205 +1,379 @@
-#!/usr/bin/env python
+import math
-"""
+from dataclasses import dataclass
-Hand + Arm slider GUI for HopeJr with optional --no-hand / --no-arm flags.
 
-Run examples:
-    python sliders.py                 # hand + arm
-    python sliders.py --no-hand       # arm only
-    python sliders.py --no-arm        # hand only
-"""
-
-# ruff: noqa: N806
-
-import json
-import sys
-from pathlib import Path
-
-import numpy as np
 import pygame
 
-from lerobot.common.robots.hope_jr import HopeJrHand, HopeJrHandConfig
+from lerobot.common.motors import MotorsBus
 
-from .motors_bus import MotorsBus
+BAR_LEN, BAR_THICKNESS = 450, 8
+HANDLE_R = 10
+BRACKET_W, BRACKET_H = 6, 14
+TRI_W, TRI_H = 12, 14
+
+BTN_W, BTN_H = 60, 22
+SAVE_W, SAVE_H = 80, 28
+LOAD_W = 80
+DD_W, DD_H = 160, 28
+
+TOP_GAP = 50
+PADDING_Y, TOP_OFFSET = 70, 60
+FONT_SIZE, FPS = 20, 60
+
+BG_COLOR = (30, 30, 30)
+BAR_RED, BAR_GREEN = (200, 60, 60), (60, 200, 60)
+HANDLE_COLOR, TEXT_COLOR = (240, 240, 240), (250, 250, 250)
+TICK_COLOR = (250, 220, 40)
+BTN_COLOR, BTN_COLOR_HL = (80, 80, 80), (110, 110, 110)
+DD_COLOR, DD_COLOR_HL = (70, 70, 70), (100, 100, 100)
 
 
-# ────────────────────────────────────────────────────────────────────────────────
+def dist(a, b):
-#  Helpers for JSON range files
+    return math.hypot(a[0] - b[0], a[1] - b[1])
-# ────────────────────────────────────────────────────────────────────────────────
-def load_ranges(path: str):
-    d = json.loads(Path(path).read_text())
-    return d["start_pos"], d["end_pos"]
 
 
-def save_ranges(path: str, lo, hi):
+@dataclass
-    d = {
+class RangeValues:
-        "start_pos": lo,
+    min_v: int
-        "end_pos": hi,
+    pos_v: int
-        "homing_offset": [],
+    max_v: int
-        "drive_mode": [],
-        "calib_mode": "LINEAR",
-    }
-    Path(path).write_text(json.dumps(d, indent=2))
 
 
-def main(bus: MotorsBus, motors: list[str] | None = None):
+class RangeSlider:
-    motors = motors if motors else list(bus.motors)
+    """One motor = one slider row"""
 
-    if not bus.is_connected:
+    def __init__(self, motor, idx, res, calibration, present, label_pad, base_y):
-        bus.connect()
+        self.motor = motor
+        self.res = res
+        self.x0 = 40 + label_pad
+        self.x1 = self.x0 + BAR_LEN
+        self.y = base_y + idx * PADDING_Y
 
-    H_RANGE = "examples/hopejr/settings/hand_ranges.json"
+        self.min_v = calibration.range_min
-    hand_lo, hand_hi = load_ranges(H_RANGE) if motors else ([], [])
+        self.max_v = calibration.range_max
-    # arm_lo, arm_hi = load_ranges(A_RANGE) if arm_names else ([], [])
+        self.pos_v = max(self.min_v, min(present, self.max_v))
 
-    pygame.init()
+        self.min_x = self._pos_from_val(self.min_v)
-    screen = pygame.display.set_mode((1080, 720))
+        self.max_x = self._pos_from_val(self.max_v)
-    pygame.display.set_caption("Hand + Arm Sliders")
+        self.pos_x = self._pos_from_val(self.pos_v)
-    font = pygame.font.SysFont(None, 20)
-    big = pygame.font.SysFont(None, 24)
 
-    ROWS = len(motors)
+        self.min_btn = pygame.Rect(self.x0 - BTN_W - 6, self.y - BTN_H // 2, BTN_W, BTN_H)
-    TOP, BOT = 100, 20
+        self.max_btn = pygame.Rect(self.x1 + 6, self.y - BTN_H // 2, BTN_W, BTN_H)
-    ROW_H = (720 - TOP - BOT) // max(1, ROWS)
-    BAR_H = ROW_H - 25
 
-    HX, H_W = None, 0
+        self.drag_min = self.drag_max = self.drag_pos = False
+        self.tick_val = present
+        self.font = pygame.font.Font(None, FONT_SIZE)
 
-    H_MAX = 1024
+    def _val_from_pos(self, x):
-    h_vals = np.zeros(len(motors), int)
+        return round((x - self.x0) / BAR_LEN * self.res)
 
-    BTN_W, BTN_H = 120, 28
+    def _pos_from_val(self, v):
-    btns = [
+        return self.x0 + (v / self.res) * BAR_LEN
-        {
-            "rect": pygame.Rect(HX, 20, BTN_W, BTN_H),
-            "txt": "Hand  LOWER",
-            "col": "hand",
-            "type": "low",
-        },
-        {
-            "rect": pygame.Rect(HX + BTN_W + 10, 20, BTN_W, BTN_H),
-            "txt": "Hand  UPPER",
-            "col": "hand",
-            "type": "high",
-        },
-    ]
-    edit_mode = None  # None or ("hand"/"arm","low"/"high")
-    toast = None
-    toast_timer = 0
 
-    def bar(x, y, w, val, lo, hi, vmax, label):
+    def set_tick(self, v):
-        lo, hi = sorted((lo, hi))
+        self.tick_val = max(0, min(v, self.res))
-        px_lo = int(w * lo / vmax)
-        px_hi = int(w * hi / vmax)
-        px_val = int(w * val / vmax)
-        bar_y = y + 5
-        pygame.draw.rect(screen, (200, 200, 200), (x, bar_y, w, BAR_H))
-        pygame.draw.rect(screen, (80, 200, 80), (x, bar_y, px_val, BAR_H))
-        pygame.draw.rect(screen, (255, 60, 60), (x, bar_y, px_lo, BAR_H))
-        pygame.draw.rect(screen, (255, 60, 60), (x + px_hi, bar_y, w - px_hi, BAR_H))
-        name = font.render(label, True, (255, 255, 255))
-        screen.blit(name, name.get_rect(midbottom=(x + w // 2, y - 3)))
-        screen.blit(font.render(str(lo), True, (80, 120, 255)), (x - 45, bar_y + BAR_H // 4))
-        screen.blit(font.render(str(hi), True, (80, 120, 255)), (x + w - 55, bar_y + BAR_H // 4))
-        screen.blit(font.render(str(val), True, (255, 255, 255)), (x + w + 10, bar_y + BAR_H // 4))
 
-    def slider_at(pos):
+    def _triangle_hit(self, pos):
-        x, y = pos
+        tri_top = self.y - BAR_THICKNESS // 2 - 2
-        row = (y - TOP) // ROW_H
+        return pygame.Rect(self.pos_x - TRI_W // 2, tri_top - TRI_H, TRI_W, TRI_H).collidepoint(pos)
-        if not 0 <= row < ROWS:
-            return None
-        y0 = TOP + row * ROW_H
-        if not y0 <= y <= y0 + BAR_H + 5:
-            return None
-        if HX <= x <= HX + H_W and row < len(motors):
-            return (str(bus), row, HX, H_W, H_MAX)
-        return None
 
-    def toast_msg(txt):
+    def handle_event(self, e):
-        nonlocal toast, toast_timer
+        if e.type == pygame.MOUSEBUTTONDOWN and e.button == 1:
-        toast = big.render(txt, True, (255, 255, 0))
+            if self.min_btn.collidepoint(e.pos):
-        toast_timer = 120
+                self.min_x, self.min_v = self.pos_x, self.pos_v
+                return
+            if self.max_btn.collidepoint(e.pos):
+                self.max_x, self.max_v = self.pos_x, self.pos_v
+                return
+            if dist(e.pos, (self.min_x, self.y)) <= HANDLE_R:
+                self.drag_min = True
+            elif dist(e.pos, (self.max_x, self.y)) <= HANDLE_R:
+                self.drag_max = True
+            elif self._triangle_hit(e.pos):
+                self.drag_pos = True
 
-    clock = pygame.time.Clock()
+        elif e.type == pygame.MOUSEBUTTONUP and e.button == 1:
-    dragging = None
+            self.drag_min = self.drag_max = self.drag_pos = False
-    while True:
-        screen.fill((20, 20, 20))
 
-        for i, n in enumerate(motors):
+        elif e.type == pygame.MOUSEMOTION:
-            bar(HX, TOP + i * ROW_H, H_W, h_vals[i], hand_lo[i], hand_hi[i], H_MAX, n)
+            x = e.pos[0]
+            if self.drag_min:
+                self.min_x = max(self.x0, min(x, self.pos_x))
+            elif self.drag_max:
+                self.max_x = min(self.x1, max(x, self.pos_x))
+            elif self.drag_pos:
+                self.pos_x = max(self.min_x, min(x, self.max_x))
 
-        # for i, n in enumerate(arm_names):
+            self.min_v = self._val_from_pos(self.min_x)
-        #     bar(AX, TOP + i * ROW_H, A_W, a_vals[i], arm_lo[i], arm_hi[i], A_MAX, n)
+            self.max_v = self._val_from_pos(self.max_x)
+            self.pos_v = self._val_from_pos(self.pos_x)
+
+    def _draw_button(self, surf, rect, text):
+        clr = BTN_COLOR_HL if rect.collidepoint(pygame.mouse.get_pos()) else BTN_COLOR
+        pygame.draw.rect(surf, clr, rect, border_radius=4)
+        t = self.font.render(text, True, TEXT_COLOR)
+        surf.blit(t, (rect.centerx - t.get_width() // 2, rect.centery - t.get_height() // 2))
+
+    def draw(self, surf):
+        # motor name above set-min button (right-aligned)
+        name_surf = self.font.render(self.motor, True, TEXT_COLOR)
+        surf.blit(
+            name_surf,
+            (self.min_btn.right - name_surf.get_width(), self.min_btn.y - name_surf.get_height() - 4),
+        )
+
+        # bar + active section
+        pygame.draw.rect(surf, BAR_RED, (self.x0, self.y - BAR_THICKNESS // 2, BAR_LEN, BAR_THICKNESS))
+        pygame.draw.rect(
+            surf, BAR_GREEN, (self.min_x, self.y - BAR_THICKNESS // 2, self.max_x - self.min_x, BAR_THICKNESS)
+        )
+
+        # tick
+        tick_x = self._pos_from_val(self.tick_val)
+        pygame.draw.line(
+            surf,
+            TICK_COLOR,
+            (tick_x, self.y - BAR_THICKNESS // 2 - 4),
+            (tick_x, self.y + BAR_THICKNESS // 2 + 4),
+            2,
+        )
+
+        # brackets
+        for x, sign in ((self.min_x, +1), (self.max_x, -1)):
+            pygame.draw.line(
+                surf, HANDLE_COLOR, (x, self.y - BRACKET_H // 2), (x, self.y + BRACKET_H // 2), 2
+            )
+            pygame.draw.line(
+                surf,
+                HANDLE_COLOR,
+                (x, self.y - BRACKET_H // 2),
+                (x + sign * BRACKET_W, self.y - BRACKET_H // 2),
+                2,
+            )
+            pygame.draw.line(
+                surf,
+                HANDLE_COLOR,
+                (x, self.y + BRACKET_H // 2),
+                (x + sign * BRACKET_W, self.y + BRACKET_H // 2),
+                2,
+            )
+
+        # triangle ▼
+        tri_top = self.y - BAR_THICKNESS // 2 - 2
+        pygame.draw.polygon(
+            surf,
+            HANDLE_COLOR,
+            [
+                (self.pos_x, tri_top),
+                (self.pos_x - TRI_W // 2, tri_top - TRI_H),
+                (self.pos_x + TRI_W // 2, tri_top - TRI_H),
+            ],
+        )
+
+        # numeric labels
+        fh = self.font.get_height()
+        pos_y = tri_top - TRI_H - 4 - fh
+        txts = [
+            (self.min_v, self.min_x, self.y - BRACKET_H // 2 - 4 - fh),
+            (self.max_v, self.max_x, self.y - BRACKET_H // 2 - 4 - fh),
+            (self.pos_v, self.pos_x, pos_y),
+        ]
+        for v, x, y in txts:
+            s = self.font.render(str(v), True, TEXT_COLOR)
+            surf.blit(s, (x - s.get_width() // 2, y))
 
         # buttons
-        for b in btns:
+        self._draw_button(surf, self.min_btn, "set min")
-            col = (120, 120, 120) if edit_mode == (b["col"], b["type"]) else (60, 60, 60)
+        self._draw_button(surf, self.max_btn, "set max")
-            pygame.draw.rect(screen, col, b["rect"])
-            txt = font.render(b["txt"], True, (255, 255, 255))
-            screen.blit(txt, txt.get_rect(center=b["rect"].center))
 
-        if toast:
+    # external
-            screen.blit(toast, toast.get_rect(center=(540, 55)))
+    def values(self) -> RangeValues:
-
+        return RangeValues(self.min_v, self.pos_v, self.max_v)
-        pygame.display.flip()
-
-        # events
-        for ev in pygame.event.get():
-            if ev.type == pygame.QUIT:
-                bus.disconnect()
-                pygame.quit()
-                sys.exit()
-
-            if ev.type == pygame.MOUSEBUTTONDOWN:
-                # button toggle
-                for b in btns:
-                    if b["rect"].collidepoint(ev.pos):
-                        edit_mode = None if edit_mode == (b["col"], b["type"]) else (b["col"], b["type"])
-                        toast_msg(f"Editing: {edit_mode[0]} {edit_mode[1]}" if edit_mode else "Edit off")
-                        break
-                else:
-                    hit = slider_at(ev.pos)
-                    if hit:
-                        dragging = hit
-                    # bound edit
-                    if edit_mode and hit and hit[0] == edit_mode[0]:
-                        col, row, left, w, vmax = hit
-                        mx, _ = ev.pos
-                        new = int(np.clip((mx - left) / w * vmax, 0, vmax))
-                        if col == "hand":
-                            if edit_mode[1] == "low":
-                                hand_lo[row] = new
-                            else:
-                                hand_hi[row] = new
-                            save_ranges(H_RANGE, hand_lo, hand_hi)
-                        else:
-                            raise ValueError(col)
-                            # if edit_mode[1] == "low":
-                            #     arm_lo[row] = new
-                            # else:
-                            #     arm_hi[row] = new
-                            # save_ranges(A_RANGE, arm_lo, arm_hi)
-                        toast_msg(f"{col} {row} {edit_mode[1]}→{new}")
-
-            if ev.type == pygame.MOUSEBUTTONUP:
-                dragging = None
-            if ev.type == pygame.MOUSEMOTION and dragging:
-                col, row, left, w, vmax = dragging
-                mx, _ = ev.pos
-                new = int(np.clip((mx - left) / w * vmax, 0, vmax))
-                if h_vals[row] != new:
-                    h_vals[row] = new
-                    bus.write("Goal_Position", [new], [motors[row]])
-
-        if toast_timer > 0:
-            toast_timer -= 1
-        else:
-            toast = None
-        clock.tick(60)
 
 
-if __name__ == "__main__":
+class RangeFinderGUI:
-    from lerobot.common.robots.hope_jr import HopeJrHand, HopeJrHandConfig
+    def __init__(self, bus: MotorsBus, groups: dict[str, list[str]]):
+        self.bus = bus
+        self.groups = groups
+        self.group_names = list(groups.keys())
+        self.current_group = self.group_names[0]
 
-    cfg = HopeJrHandConfig("/dev/tty.usbmodem58760431541")
+        if not bus.is_connected:
-    hand = HopeJrHand()
+            bus.connect()
-    main(hand.bus)
+
+        self.calibration = bus.read_calibration()
+        self.res_table = bus.model_resolution_table
+        self.present_cache = {
+            m: bus.read("Present_Position", m, normalize=False) for motors in groups.values() for m in motors
+        }
+
+        # pygame init
+        pygame.init()
+        self.font = pygame.font.Font(None, FONT_SIZE)
+
+        label_pad = max(self.font.size(m)[0] for ms in groups.values() for m in ms)
+        self.label_pad = label_pad
+        width = 40 + label_pad + BAR_LEN + 6 + BTN_W + 10 + SAVE_W + 10
+        self.controls_bottom = 10 + SAVE_H
+        self.base_y = self.controls_bottom + TOP_GAP
+        height = self.base_y + PADDING_Y * len(groups[self.current_group]) + 40
+
+        self.screen = pygame.display.set_mode((width, height))
+        pygame.display.set_caption("Motors range finder")
+
+        # ui rects
+        self.save_btn = pygame.Rect(width - SAVE_W - 10, 10, SAVE_W, SAVE_H)
+        self.load_btn = pygame.Rect(self.save_btn.left - LOAD_W - 10, 10, LOAD_W, SAVE_H)
+        self.dd_btn = pygame.Rect(width // 2 - DD_W // 2, 10, DD_W, DD_H)
+        self.dd_open = False  # dropdown expanded?
+
+        self.clock = pygame.time.Clock()
+        self._build_sliders()
+        self._adjust_height()
+
+    def _adjust_height(self):
+        motors = self.groups[self.current_group]
+        new_h = self.base_y + PADDING_Y * len(motors) + 40
+        if new_h != self.screen.get_height():
+            w = self.screen.get_width()
+            self.screen = pygame.display.set_mode((w, new_h))
+
+    def _build_sliders(self):
+        self.sliders: list[RangeSlider] = []
+        motors = self.groups[self.current_group]
+        for i, m in enumerate(motors):
+            self.sliders.append(
+                RangeSlider(
+                    motor=m,
+                    idx=i,
+                    res=self.res_table[self.bus.motors[m].model],
+                    calibration=self.calibration[m],
+                    present=self.present_cache[m],
+                    label_pad=self.label_pad,
+                    base_y=self.base_y,
+                )
+            )
+
+    def _draw_dropdown(self):
+        # collapsed box
+        hover = self.dd_btn.collidepoint(pygame.mouse.get_pos())
+        pygame.draw.rect(self.screen, DD_COLOR_HL if hover else DD_COLOR, self.dd_btn, border_radius=6)
+
+        txt = self.font.render(self.current_group, True, TEXT_COLOR)
+        self.screen.blit(
+            txt, (self.dd_btn.centerx - txt.get_width() // 2, self.dd_btn.centery - txt.get_height() // 2)
+        )
+
+        tri_w, tri_h = 12, 6
+        cx = self.dd_btn.right - 14
+        cy = self.dd_btn.centery + 1
+        pygame.draw.polygon(
+            self.screen,
+            TEXT_COLOR,
+            [(cx - tri_w // 2, cy - tri_h // 2), (cx + tri_w // 2, cy - tri_h // 2), (cx, cy + tri_h // 2)],
+        )
+
+        if not self.dd_open:
+            return
+
+        # expanded list
+        for i, name in enumerate(self.group_names):
+            item_rect = pygame.Rect(self.dd_btn.left, self.dd_btn.bottom + i * DD_H, DD_W, DD_H)
+            clr = DD_COLOR_HL if item_rect.collidepoint(pygame.mouse.get_pos()) else DD_COLOR
+            pygame.draw.rect(self.screen, clr, item_rect)
+            t = self.font.render(name, True, TEXT_COLOR)
+            self.screen.blit(
+                t, (item_rect.centerx - t.get_width() // 2, item_rect.centery - t.get_height() // 2)
+            )
+
+    def _handle_dropdown_event(self, e):
+        if e.type == pygame.MOUSEBUTTONDOWN and e.button == 1:
+            if self.dd_btn.collidepoint(e.pos):
+                self.dd_open = not self.dd_open
+                return True
+            if self.dd_open:
+                for i, name in enumerate(self.group_names):
+                    item_rect = pygame.Rect(self.dd_btn.left, self.dd_btn.bottom + i * DD_H, DD_W, DD_H)
+                    if item_rect.collidepoint(e.pos):
+                        if name != self.current_group:
+                            self.current_group = name
+                            self._build_sliders()
+                            self._adjust_height()
+                        self.dd_open = False
+                        return True
+                self.dd_open = False
+        return False
+
+    def _save_current(self):
+        for s in self.sliders:
+            self.calibration[s.motor].range_min = s.min_v
+            self.calibration[s.motor].range_max = s.max_v
+
+        with self.bus.torque_disabled():
+            self.bus.write_calibration(self.calibration)
+
+    def _load_current(self):
+        self.calibration = self.bus.read_calibration()
+        for s in self.sliders:
+            s.min_v = self.calibration[s.motor].range_min
+            s.max_v = self.calibration[s.motor].range_max
+            s.min_x = s._pos_from_val(s.min_v)
+            s.max_x = s._pos_from_val(s.max_v)
+
+    def run(self):
+        # disp_len = max(len(m) for ms in self.groups.values() for m in ms)
+
+        while True:
+            for e in pygame.event.get():
+                if e.type == pygame.QUIT:
+                    self.bus.disconnect()
+                    pygame.quit()
+                    return self.calibration
+
+                if self._handle_dropdown_event(e):
+                    continue
+
+                if e.type == pygame.MOUSEBUTTONDOWN and e.button == 1:
+                    if self.save_btn.collidepoint(e.pos):
+                        self._save_current()
+                    elif self.load_btn.collidepoint(e.pos):
+                        self._load_current()
+
+                for s in self.sliders:
+                    s.handle_event(e)
+
+            # live goal write while dragging
+            for s in self.sliders:
+                if s.drag_pos:
+                    self.bus.write("Goal_Position", s.motor, s.pos_v, normalize=False)
+
+            # tick update
+            for s in self.sliders:
+                pos = self.bus.read("Present_Position", s.motor, normalize=False)
+                s.set_tick(pos)
+                self.present_cache[s.motor] = pos
+
+            # ─ drawing
+            self.screen.fill(BG_COLOR)
+            for s in self.sliders:
+                s.draw(self.screen)
+
+            self._draw_dropdown()
+
+            # load / save buttons
+            for rect, text in ((self.load_btn, "LOAD"), (self.save_btn, "SAVE")):
+                clr = BTN_COLOR_HL if rect.collidepoint(pygame.mouse.get_pos()) else BTN_COLOR
+                pygame.draw.rect(self.screen, clr, rect, border_radius=6)
+                t = self.font.render(text, True, TEXT_COLOR)
+                self.screen.blit(t, (rect.centerx - t.get_width() // 2, rect.centery - t.get_height() // 2))
+
+            pygame.display.flip()
+
+            # terminal table (optional)
+            # print("\n-----------------------------")
+            # print(f"{'NAME':<{disp_len}} | {'MIN':>6} | {'POS':>6} | {'MAX':>6}")
+            # for s in self.sliders:
+            #     vals = s.values()
+            #     print(f"{s.motor:<{disp_len}} | {vals.min_v:>6} | {vals.pos_v:>6} | {vals.max_v:>6}")
+            # move_cursor_up(len(self.sliders) + 3)
+
+            self.clock.tick(FPS)

lerobot/common/motors/dynamixel/dynamixel.py

@@ -186,13 +186,14 @@ class DynamixelMotorsBus(MotorsBus):
 
         return calibration
 
-    def write_calibration(self, calibration_dict: dict[str, MotorCalibration]) -> None:
+    def write_calibration(self, calibration_dict: dict[str, MotorCalibration], cache: bool = True) -> None:
         for motor, calibration in calibration_dict.items():
             self.write("Homing_Offset", motor, calibration.homing_offset)
             self.write("Min_Position_Limit", motor, calibration.range_min)
             self.write("Max_Position_Limit", motor, calibration.range_max)
 
-        self.calibration = calibration_dict
+        if cache:
+            self.calibration = calibration_dict
 
     def disable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None:
         for motor in self._get_motors_list(motors):

lerobot/common/motors/feetech/feetech.py

@@ -254,14 +254,15 @@ class FeetechMotorsBus(MotorsBus):
 
         return calibration
 
-    def write_calibration(self, calibration_dict: dict[str, MotorCalibration]) -> None:
+    def write_calibration(self, calibration_dict: dict[str, MotorCalibration], cache: bool = True) -> None:
         for motor, calibration in calibration_dict.items():
             if self.protocol_version == 0:
                 self.write("Homing_Offset", motor, calibration.homing_offset)
             self.write("Min_Position_Limit", motor, calibration.range_min)
             self.write("Max_Position_Limit", motor, calibration.range_max)
 
-        self.calibration = calibration_dict
+        if cache:
+            self.calibration = calibration_dict
 
     def _get_half_turn_homings(self, positions: dict[NameOrID, Value]) -> dict[NameOrID, Value]:
         """

lerobot/common/motors/motors_bus.py

@@ -572,7 +572,7 @@ class MotorsBus(abc.ABC):
         pass
 
     @contextmanager
-    def torque_disabled(self):
+    def torque_disabled(self, motors: int | str | list[str] | None = None):
         """Context-manager that guarantees torque is re-enabled.
 
         This helper is useful to temporarily disable torque when configuring motors.
@@ -582,11 +582,11 @@ class MotorsBus(abc.ABC):
             ...     # Safe operations here
             ...     pass
         """
-        self.disable_torque()
+        self.disable_torque(motors)
         try:
             yield
         finally:
-            self.enable_torque()
+            self.enable_torque(motors)
 
     def set_timeout(self, timeout_ms: int | None = None):
         """Change the packet timeout used by the SDK.
@@ -638,12 +638,13 @@ class MotorsBus(abc.ABC):
         pass
 
     @abc.abstractmethod
-    def write_calibration(self, calibration_dict: dict[str, MotorCalibration]) -> None:
+    def write_calibration(self, calibration_dict: dict[str, MotorCalibration], cache: bool = True) -> None:
-        """Write calibration parameters to the motors and cache them.
+        """Write calibration parameters to the motors and optionally cache them.
 
         Args:
             calibration_dict (dict[str, MotorCalibration]): Calibration obtained from
                 :pymeth:`read_calibration` or crafted by the user.
+            cache (bool, optional): Save the calibration to :pyattr:`calibration`. Defaults to True.
         """
         pass
 

lerobot/common/robots/hope_jr/hope_jr_hand.py

@@ -21,10 +21,10 @@ from typing import Any
 
 from lerobot.common.cameras.utils import make_cameras_from_configs
 from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
-from lerobot.common.motors import Motor, MotorCalibration, MotorNormMode
+from lerobot.common.motors import Motor, MotorNormMode
+from lerobot.common.motors.calibration_gui import RangeFinderGUI
 from lerobot.common.motors.feetech import (
     FeetechMotorsBus,
-    OperatingMode,
 )
 
 from ..robot import Robot
@@ -113,36 +113,11 @@ class HopeJrHand(Robot):
         return self.bus.is_calibrated
 
     def calibrate(self) -> None:
-        raise NotImplementedError  # TODO(aliberts): adapt code below (copied from koch)
+        fingers = {}
-        logger.info(f"\nRunning calibration of {self}")
+        for finger in ["thumb", "index", "middle", "ring", "pinky"]:
-        self.bus.disable_torque()
+            fingers[finger] = [motor for motor in self.bus.motors if motor.startswith(finger)]
-        for name in self.bus.names:
-            self.bus.write("Operating_Mode", name, OperatingMode.POSITION.value)
 
-        input("Move robot to the middle of its range of motion and press ENTER....")
+        self.calibration = RangeFinderGUI(self.bus, fingers).run()
-        homing_offsets = self.bus.set_half_turn_homings()
-
-        full_turn_motor = "wrist_roll"
-        unknown_range_motors = [name for name in self.bus.names if name != full_turn_motor]
-        logger.info(
-            f"Move all joints except '{full_turn_motor}' sequentially through their "
-            "entire ranges of motion.\nRecording positions. Press ENTER to stop..."
-        )
-        range_mins, range_maxes = self.bus.record_ranges_of_motion(unknown_range_motors)
-        range_mins[full_turn_motor] = 0
-        range_maxes[full_turn_motor] = 4095
-
-        self.calibration = {}
-        for name, motor in self.bus.motors.items():
-            self.calibration[name] = MotorCalibration(
-                id=motor.id,
-                drive_mode=0,
-                homing_offset=homing_offsets[name],
-                range_min=range_mins[name],
-                range_max=range_maxes[name],
-            )
-
-        self.bus.write_calibration(self.calibration)
         self._save_calibration()
         print("Calibration saved to", self.calibration_fpath)