Install ffmpeg for integration tests

for more information, see https://pre-commit.ci

.dockerignore

@@ -73,7 +73,7 @@ pip-log.txt
 pip-delete-this-directory.txt
 
 # Unit test / coverage reports
-!tests/artifacts
+!tests/data
 htmlcov/
 .tox/
 .nox/

.github/workflows/test.yml

@@ -126,7 +126,7 @@ jobs:
       # portaudio19-dev is needed to install pyaudio
         run: |
           sudo apt-get update && \
-          sudo apt-get install -y libegl1-mesa-dev portaudio19-dev
+          sudo apt-get install -y libegl1-mesa-dev ffmpeg portaudio19-dev
 
       - name: Install uv and python
         uses: astral-sh/setup-uv@v5

.gitignore

@@ -78,7 +78,7 @@ pip-log.txt
 pip-delete-this-directory.txt
 
 # Unit test / coverage reports
-!tests/artifacts
+!tests/data
 htmlcov/
 .tox/
 .nox/

.pre-commit-config.yaml

@@ -12,7 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-exclude: "tests/artifacts/.*\\.safetensors$"
+exclude: ^(tests/data)
 default_language_version:
     python: python3.10
 repos:

CONTRIBUTING.md

@@ -291,7 +291,7 @@ sudo apt-get install git-lfs
 git lfs install
 ```
 
-Pull artifacts if they're not in [tests/artifacts](tests/artifacts)
+Pull artifacts if they're not in [tests/data](tests/data)
 ```bash
 git lfs pull
 ```

benchmarks/video/run_video_benchmark.py

@@ -67,7 +67,7 @@ def parse_int_or_none(value) -> int | None:
 def check_datasets_formats(repo_ids: list) -> None:
     for repo_id in repo_ids:
         dataset = LeRobotDataset(repo_id)
-        if dataset.video:
+        if len(dataset.meta.video_keys) > 0:
             raise ValueError(
                 f"Use only image dataset for running this benchmark. Video dataset provided: {repo_id}"
             )

lerobot/common/datasets/lerobot_dataset.py

@@ -67,7 +67,7 @@ from lerobot.common.datasets.utils import (
 )
 from lerobot.common.datasets.video_utils import (
     VideoFrame,
-    decode_video_frames_torchvision,
+    decode_video_frames,
     encode_video_frames,
     get_video_info,
 )
@@ -462,8 +462,8 @@ class LeRobotDataset(torch.utils.data.Dataset):
             download_videos (bool, optional): Flag to download the videos. Note that when set to True but the
                 video files are already present on local disk, they won't be downloaded again. Defaults to
                 True.
-            video_backend (str | None, optional): Video backend to use for decoding videos. There is currently
-                a single option which is the pyav decoder used by Torchvision. Defaults to pyav.
+            video_backend (str | None, optional): Video backend to use for decoding videos. Defaults to torchcodec.
+                You can also use the 'pyav' decoder used by Torchvision, which used to be the default option, or 'video_reader' which is another decoder of Torchvision.
         """
         super().__init__()
         self.repo_id = repo_id
@@ -473,7 +473,7 @@ class LeRobotDataset(torch.utils.data.Dataset):
         self.episodes = episodes
         self.tolerance_s = tolerance_s
         self.revision = revision if revision else CODEBASE_VERSION
-        self.video_backend = video_backend if video_backend else "pyav"
+        self.video_backend = video_backend if video_backend else "torchcodec"
         self.delta_indices = None
 
         # Unused attributes
@@ -707,9 +707,7 @@ class LeRobotDataset(torch.utils.data.Dataset):
         item = {}
         for vid_key, query_ts in query_timestamps.items():
             video_path = self.root / self.meta.get_video_file_path(ep_idx, vid_key)
-            frames = decode_video_frames_torchvision(
-                video_path, query_ts, self.tolerance_s, self.video_backend
-            )
+            frames = decode_video_frames(video_path, query_ts, self.tolerance_s, self.video_backend)
             item[vid_key] = frames.squeeze(0)
 
         return item
@@ -1029,7 +1027,7 @@ class LeRobotDataset(torch.utils.data.Dataset):
         obj.delta_timestamps = None
         obj.delta_indices = None
         obj.episode_data_index = None
-        obj.video_backend = video_backend if video_backend is not None else "pyav"
+        obj.video_backend = video_backend if video_backend is not None else "torchcodec"
         return obj
 
 

lerobot/common/datasets/video_utils.py

@@ -27,6 +27,35 @@ import torch
 import torchvision
 from datasets.features.features import register_feature
 from PIL import Image
+from torchcodec.decoders import VideoDecoder
+
+
+def decode_video_frames(
+    video_path: Path | str,
+    timestamps: list[float],
+    tolerance_s: float,
+    backend: str = "torchcodec",
+) -> torch.Tensor:
+    """
+    Decodes video frames using the specified backend.
+
+    Args:
+        video_path (Path): Path to the video file.
+        timestamps (list[float]): List of timestamps to extract frames.
+        tolerance_s (float): Allowed deviation in seconds for frame retrieval.
+        backend (str, optional): Backend to use for decoding. Defaults to "torchcodec".
+
+    Returns:
+        torch.Tensor: Decoded frames.
+
+    Currently supports torchcodec on cpu and pyav.
+    """
+    if backend == "torchcodec":
+        return decode_video_frames_torchcodec(video_path, timestamps, tolerance_s)
+    elif backend in ["pyav", "video_reader"]:
+        return decode_video_frames_torchvision(video_path, timestamps, tolerance_s, backend)
+    else:
+        raise ValueError(f"Unsupported video backend: {backend}")
 
 
 def decode_video_frames_torchvision(
@@ -127,6 +156,76 @@ def decode_video_frames_torchvision(
     return closest_frames
 
 
+def decode_video_frames_torchcodec(
+    video_path: Path | str,
+    timestamps: list[float],
+    tolerance_s: float,
+    device: str = "cpu",
+    log_loaded_timestamps: bool = False,
+) -> torch.Tensor:
+    """Loads frames associated with the requested timestamps of a video using torchcodec.
+
+    Note: Setting device="cuda" outside the main process, e.g. in data loader workers, will lead to CUDA initialization errors.
+
+    Note: Video benefits from inter-frame compression. Instead of storing every frame individually,
+    the encoder stores a reference frame (or a key frame) and subsequent frames as differences relative to
+    that key frame. As a consequence, to access a requested frame, we need to load the preceding key frame,
+    and all subsequent frames until reaching the requested frame. The number of key frames in a video
+    can be adjusted during encoding to take into account decoding time and video size in bytes.
+    """
+    video_path = str(video_path)
+    # initialize video decoder
+    decoder = VideoDecoder(video_path, device=device)
+    loaded_frames = []
+    loaded_ts = []
+    # get metadata for frame information
+    metadata = decoder.metadata
+    average_fps = metadata.average_fps
+
+    # convert timestamps to frame indices
+    frame_indices = [round(ts * average_fps) for ts in timestamps]
+
+    # retrieve frames based on indices
+    frames_batch = decoder.get_frames_at(indices=frame_indices)
+
+    for frame, pts in zip(frames_batch.data, frames_batch.pts_seconds, strict=False):
+        loaded_frames.append(frame)
+        loaded_ts.append(pts.item())
+        if log_loaded_timestamps:
+            logging.info(f"Frame loaded at timestamp={pts:.4f}")
+
+    query_ts = torch.tensor(timestamps)
+    loaded_ts = torch.tensor(loaded_ts)
+
+    # compute distances between each query timestamp and loaded timestamps
+    dist = torch.cdist(query_ts[:, None], loaded_ts[:, None], p=1)
+    min_, argmin_ = dist.min(1)
+
+    is_within_tol = min_ < tolerance_s
+    assert is_within_tol.all(), (
+        f"One or several query timestamps unexpectedly violate the tolerance ({min_[~is_within_tol]} > {tolerance_s=})."
+        "It means that the closest frame that can be loaded from the video is too far away in time."
+        "This might be due to synchronization issues with timestamps during data collection."
+        "To be safe, we advise to ignore this item during training."
+        f"\nqueried timestamps: {query_ts}"
+        f"\nloaded timestamps: {loaded_ts}"
+        f"\nvideo: {video_path}"
+    )
+
+    # get closest frames to the query timestamps
+    closest_frames = torch.stack([loaded_frames[idx] for idx in argmin_])
+    closest_ts = loaded_ts[argmin_]
+
+    if log_loaded_timestamps:
+        logging.info(f"{closest_ts=}")
+
+    # convert to float32 in [0,1] range (channel first)
+    closest_frames = closest_frames.type(torch.float32) / 255
+
+    assert len(timestamps) == len(closest_frames)
+    return closest_frames
+
+
 def encode_video_frames(
     imgs_dir: Path | str,
     video_path: Path | str,

lerobot/common/robot_devices/cameras/intelrealsense.py

@@ -48,7 +48,7 @@ def find_cameras(raise_when_empty=True, mock=False) -> list[dict]:
     connected to the computer.
     """
     if mock:
-        import tests.cameras.mock_pyrealsense2 as rs
+        import tests.mock_pyrealsense2 as rs
     else:
         import pyrealsense2 as rs
 
@@ -100,7 +100,7 @@ def save_images_from_cameras(
         serial_numbers = [cam["serial_number"] for cam in camera_infos]
 
     if mock:
-        import tests.cameras.mock_cv2 as cv2
+        import tests.mock_cv2 as cv2
     else:
         import cv2
 
@@ -253,7 +253,7 @@ class IntelRealSenseCamera:
         self.logs = {}
 
         if self.mock:
-            import tests.cameras.mock_cv2 as cv2
+            import tests.mock_cv2 as cv2
         else:
             import cv2
 
@@ -287,7 +287,7 @@ class IntelRealSenseCamera:
             )
 
         if self.mock:
-            import tests.cameras.mock_pyrealsense2 as rs
+            import tests.mock_pyrealsense2 as rs
         else:
             import pyrealsense2 as rs
 
@@ -375,7 +375,7 @@ class IntelRealSenseCamera:
             )
 
         if self.mock:
-            import tests.cameras.mock_cv2 as cv2
+            import tests.mock_cv2 as cv2
         else:
             import cv2
 

lerobot/common/robot_devices/cameras/opencv.py

@@ -80,7 +80,7 @@ def _find_cameras(
     possible_camera_ids: list[int | str], raise_when_empty=False, mock=False
 ) -> list[int | str]:
     if mock:
-        import tests.cameras.mock_cv2 as cv2
+        import tests.mock_cv2 as cv2
     else:
         import cv2
 
@@ -269,7 +269,7 @@ class OpenCVCamera:
         self.logs = {}
 
         if self.mock:
-            import tests.cameras.mock_cv2 as cv2
+            import tests.mock_cv2 as cv2
         else:
             import cv2
 
@@ -286,7 +286,7 @@ class OpenCVCamera:
             raise RobotDeviceAlreadyConnectedError(f"OpenCVCamera({self.camera_index}) is already connected.")
 
         if self.mock:
-            import tests.cameras.mock_cv2 as cv2
+            import tests.mock_cv2 as cv2
         else:
             import cv2
 
@@ -398,7 +398,7 @@ class OpenCVCamera:
         # so we convert the image color from BGR to RGB.
         if requested_color_mode == "rgb":
             if self.mock:
-                import tests.cameras.mock_cv2 as cv2
+                import tests.mock_cv2 as cv2
             else:
                 import cv2
 

lerobot/common/robot_devices/motors/dynamixel.py

@@ -332,7 +332,7 @@ class DynamixelMotorsBus:
             )
 
         if self.mock:
-            import tests.motors.mock_dynamixel_sdk as dxl
+            import tests.mock_dynamixel_sdk as dxl
         else:
             import dynamixel_sdk as dxl
 
@@ -356,7 +356,7 @@ class DynamixelMotorsBus:
 
     def reconnect(self):
         if self.mock:
-            import tests.motors.mock_dynamixel_sdk as dxl
+            import tests.mock_dynamixel_sdk as dxl
         else:
             import dynamixel_sdk as dxl
 
@@ -646,7 +646,7 @@ class DynamixelMotorsBus:
 
     def read_with_motor_ids(self, motor_models, motor_ids, data_name, num_retry=NUM_READ_RETRY):
         if self.mock:
-            import tests.motors.mock_dynamixel_sdk as dxl
+            import tests.mock_dynamixel_sdk as dxl
         else:
             import dynamixel_sdk as dxl
 
@@ -691,7 +691,7 @@ class DynamixelMotorsBus:
         start_time = time.perf_counter()
 
         if self.mock:
-            import tests.motors.mock_dynamixel_sdk as dxl
+            import tests.mock_dynamixel_sdk as dxl
         else:
             import dynamixel_sdk as dxl
 
@@ -757,7 +757,7 @@ class DynamixelMotorsBus:
 
     def write_with_motor_ids(self, motor_models, motor_ids, data_name, values, num_retry=NUM_WRITE_RETRY):
         if self.mock:
-            import tests.motors.mock_dynamixel_sdk as dxl
+            import tests.mock_dynamixel_sdk as dxl
         else:
             import dynamixel_sdk as dxl
 
@@ -793,7 +793,7 @@ class DynamixelMotorsBus:
         start_time = time.perf_counter()
 
         if self.mock:
-            import tests.motors.mock_dynamixel_sdk as dxl
+            import tests.mock_dynamixel_sdk as dxl
         else:
             import dynamixel_sdk as dxl
 

lerobot/common/robot_devices/motors/feetech.py

@@ -13,6 +13,8 @@
 # limitations under the License.
 
 import enum
+import logging
+import math
 import time
 import traceback
 from copy import deepcopy
@@ -30,6 +32,13 @@ TIMEOUT_MS = 1000
 
 MAX_ID_RANGE = 252
 
+# The following bounds define the lower and upper joints range (after calibration).
+# For joints in degree (i.e. revolute joints), their nominal range is [-180, 180] degrees
+# which corresponds to a half rotation on the left and half rotation on the right.
+# Some joints might require higher range, so we allow up to [-270, 270] degrees until
+# an error is raised.
+LOWER_BOUND_DEGREE = -270
+UPPER_BOUND_DEGREE = 270
 # For joints in percentage (i.e. joints that move linearly like the prismatic joint of a gripper),
 # their nominal range is [0, 100] %. For instance, for Aloha gripper, 0% is fully
 # closed, and 100% is fully open. To account for slight calibration issue, we allow up to
@@ -39,6 +48,7 @@ UPPER_BOUND_LINEAR = 110
 
 HALF_TURN_DEGREE = 180
 
+
 # See this link for STS3215 Memory Table:
 # https://docs.google.com/spreadsheets/d/1GVs7W1VS1PqdhA1nW-abeyAHhTUxKUdR/edit?usp=sharing&ouid=116566590112741600240&rtpof=true&sd=true
 # data_name: (address, size_byte)
@@ -104,6 +114,8 @@ SCS_SERIES_BAUDRATE_TABLE = {
 }
 
 CALIBRATION_REQUIRED = ["Goal_Position", "Present_Position"]
+CONVERT_UINT32_TO_INT32_REQUIRED = ["Goal_Position", "Present_Position"]
+
 
 MODEL_CONTROL_TABLE = {
     "scs_series": SCS_SERIES_CONTROL_TABLE,
@@ -125,63 +137,15 @@ NUM_READ_RETRY = 20
 NUM_WRITE_RETRY = 20
 
 
-def convert_ticks_to_degrees(ticks, model):
-    resolutions = MODEL_RESOLUTION[model]
-    # Convert the ticks to degrees
-    return ticks * (360.0 / resolutions)
-
-
-def convert_degrees_to_ticks(degrees, model):
-    resolutions = MODEL_RESOLUTION[model]
-    # Convert degrees to motor ticks
-    return int(degrees * (resolutions / 360.0))
-
-
-def adjusted_to_homing_ticks(raw_motor_ticks: int, model: str, motorbus, motor_id: int) -> int:
+def convert_degrees_to_steps(degrees: float | np.ndarray, models: str | list[str]) -> np.ndarray:
+    """This function converts the degree range to the step range for indicating motors rotation.
+    It assumes a motor achieves a full rotation by going from -180 degree position to +180.
+    The motor resolution (e.g. 4096) corresponds to the number of steps needed to achieve a full rotation.
     """
-    Takes a raw reading [0..(res-1)] (e.g. 0..4095) and shifts it so that '2048'
-    becomes 0 in the homed coordinate system ([-2048..+2047] for 4096 resolution).
-    """
-    resolutions = MODEL_RESOLUTION[model]
-
-    # Shift raw ticks by half-resolution so 2048 -> 0, then wrap [0..res-1].
-    ticks = (raw_motor_ticks - (resolutions // 2)) % resolutions
-
-    # If above halfway, fold it into negative territory => [-2048..+2047].
-    if ticks > (resolutions // 2):
-        ticks -= resolutions
-
-    # Flip sign if drive_mode is set.
-    drive_mode = 0
-    if motorbus.calibration is not None:
-        drive_mode = motorbus.calibration["drive_mode"][motor_id - 1]
-
-    if drive_mode:
-        ticks *= -1
-
-    return ticks
-
-
-def adjusted_to_motor_ticks(adjusted_pos: int, model: str, motorbus, motor_id: int) -> int:
-    """
-    Inverse of adjusted_to_homing_ticks(). Takes a 'homed' position in [-2048..+2047]
-    and recovers the raw [0..(res-1)] ticks with 2048 as midpoint.
-    """
-    # Flip sign if drive_mode was set.
-    drive_mode = 0
-    if motorbus.calibration is not None:
-        drive_mode = motorbus.calibration["drive_mode"][motor_id - 1]
-
-    if drive_mode:
-        adjusted_pos *= -1
-
-    resolutions = MODEL_RESOLUTION[model]
-
-    # Shift by +half-resolution and wrap into [0..res-1].
-    # This undoes the earlier shift by -half-resolution.
-    ticks = (adjusted_pos + (resolutions // 2)) % resolutions
-
-    return ticks
+    resolutions = [MODEL_RESOLUTION[model] for model in models]
+    steps = degrees / 180 * np.array(resolutions) / 2
+    steps = steps.astype(int)
+    return steps
 
 
 def convert_to_bytes(value, bytes, mock=False):
@@ -340,6 +304,8 @@ class FeetechMotorsBus:
         self.group_writers = {}
         self.logs = {}
 
+        self.track_positions = {}
+
     def connect(self):
         if self.is_connected:
             raise RobotDeviceAlreadyConnectedError(
@@ -347,7 +313,7 @@ class FeetechMotorsBus:
             )
 
         if self.mock:
-            import tests.motors.mock_scservo_sdk as scs
+            import tests.mock_scservo_sdk as scs
         else:
             import scservo_sdk as scs
 
@@ -371,7 +337,7 @@ class FeetechMotorsBus:
 
     def reconnect(self):
         if self.mock:
-            import tests.motors.mock_scservo_sdk as scs
+            import tests.mock_scservo_sdk as scs
         else:
             import scservo_sdk as scs
 
@@ -436,7 +402,33 @@ class FeetechMotorsBus:
     def set_calibration(self, calibration: dict[str, list]):
         self.calibration = calibration
 
+    def apply_calibration_autocorrect(self, values: np.ndarray | list, motor_names: list[str] | None):
+        """This function apply the calibration, automatically detects out of range errors for motors values and attempt to correct.
+
+        For more info, see docstring of `apply_calibration` and `autocorrect_calibration`.
+        """
+        try:
+            values = self.apply_calibration(values, motor_names)
+        except JointOutOfRangeError as e:
+            print(e)
+            self.autocorrect_calibration(values, motor_names)
+            values = self.apply_calibration(values, motor_names)
+        return values
+
     def apply_calibration(self, values: np.ndarray | list, motor_names: list[str] | None):
+        """Convert from unsigned int32 joint position range [0, 2**32[ to the universal float32 nominal degree range ]-180.0, 180.0[ with
+        a "zero position" at 0 degree.
+
+        Note: We say "nominal degree range" since the motors can take values outside this range. For instance, 190 degrees, if the motor
+        rotate more than a half a turn from the zero position. However, most motors can't rotate more than 180 degrees and will stay in this range.
+
+        Joints values are original in [0, 2**32[ (unsigned int32). Each motor are expected to complete a full rotation
+        when given a goal position that is + or - their resolution. For instance, feetech xl330-m077 have a resolution of 4096, and
+        at any position in their original range, let's say the position 56734, they complete a full rotation clockwise by moving to 60830,
+        or anticlockwise by moving to 52638. The position in the original range is arbitrary and might change a lot between each motor.
+        To harmonize between motors of the same model, different robots, or even models of different brands, we propose to work
+        in the centered nominal degree range ]-180, 180[.
+        """
         if motor_names is None:
             motor_names = self.motor_names
 
@@ -448,11 +440,34 @@ class FeetechMotorsBus:
             calib_mode = self.calibration["calib_mode"][calib_idx]
 
             if CalibrationMode[calib_mode] == CalibrationMode.DEGREE:
-                motor_idx, model = self.motors[name]
+                drive_mode = self.calibration["drive_mode"][calib_idx]
+                homing_offset = self.calibration["homing_offset"][calib_idx]
+                _, model = self.motors[name]
+                resolution = self.model_resolution[model]
 
-                # Convert raw motor ticks to homed ticks, then convert the homed ticks to degrees
-                values[i] = adjusted_to_homing_ticks(values[i], model, self, motor_idx)
-                values[i] = convert_ticks_to_degrees(values[i], model)
+                # Update direction of rotation of the motor to match between leader and follower.
+                # In fact, the motor of the leader for a given joint can be assembled in an
+                # opposite direction in term of rotation than the motor of the follower on the same joint.
+                if drive_mode:
+                    values[i] *= -1
+
+                # Convert from range [-2**31, 2**31[ to
+                # nominal range ]-resolution, resolution[ (e.g. ]-2048, 2048[)
+                values[i] += homing_offset
+
+                # Convert from range ]-resolution, resolution[ to
+                # universal float32 centered degree range ]-180, 180[
+                values[i] = values[i] / (resolution // 2) * HALF_TURN_DEGREE
+
+                if (values[i] < LOWER_BOUND_DEGREE) or (values[i] > UPPER_BOUND_DEGREE):
+                    raise JointOutOfRangeError(
+                        f"Wrong motor position range detected for {name}. "
+                        f"Expected to be in nominal range of [-{HALF_TURN_DEGREE}, {HALF_TURN_DEGREE}] degrees (a full rotation), "
+                        f"with a maximum range of [{LOWER_BOUND_DEGREE}, {UPPER_BOUND_DEGREE}] degrees to account for joints that can rotate a bit more, "
+                        f"but present value is {values[i]} degree. "
+                        "This might be due to a cable connection issue creating an artificial 360 degrees jump in motor values. "
+                        "You need to recalibrate by running: `python lerobot/scripts/control_robot.py calibrate`"
+                    )
 
             elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
                 start_pos = self.calibration["start_pos"][calib_idx]
@@ -474,6 +489,103 @@ class FeetechMotorsBus:
 
         return values
 
+    def autocorrect_calibration(self, values: np.ndarray | list, motor_names: list[str] | None):
+        """This function automatically detects issues with values of motors after calibration, and correct for these issues.
+
+        Some motors might have values outside of expected maximum bounds after calibration.
+        For instance, for a joint in degree, its value can be outside [-270, 270] degrees, which is totally unexpected given
+        a nominal range of [-180, 180] degrees, which represents half a turn to the left or right starting from zero position.
+
+        Known issues:
+        #1: Motor value randomly shifts of a full turn, caused by hardware/connection errors.
+        #2: Motor internal homing offset is shifted of a full turn, caused by using default calibration (e.g Aloha).
+        #3: motor internal homing offset is shifted of less or more than a full turn, caused by using default calibration
+            or by human error during manual calibration.
+
+        Issues #1 and #2 can be solved by shifting the calibration homing offset by a full turn.
+        Issue #3 will be visually detected by user and potentially captured by the safety feature `max_relative_target`,
+        that will slow down the motor, raise an error asking to recalibrate. Manual recalibrating will solve the issue.
+
+        Note: A full turn corresponds to 360 degrees but also to 4096 steps for a motor resolution of 4096.
+        """
+        if motor_names is None:
+            motor_names = self.motor_names
+
+        # Convert from unsigned int32 original range [0, 2**32] to signed float32 range
+        values = values.astype(np.float32)
+
+        for i, name in enumerate(motor_names):
+            calib_idx = self.calibration["motor_names"].index(name)
+            calib_mode = self.calibration["calib_mode"][calib_idx]
+
+            if CalibrationMode[calib_mode] == CalibrationMode.DEGREE:
+                drive_mode = self.calibration["drive_mode"][calib_idx]
+                homing_offset = self.calibration["homing_offset"][calib_idx]
+                _, model = self.motors[name]
+                resolution = self.model_resolution[model]
+
+                if drive_mode:
+                    values[i] *= -1
+
+                # Convert from initial range to range [-180, 180] degrees
+                calib_val = (values[i] + homing_offset) / (resolution // 2) * HALF_TURN_DEGREE
+                in_range = (calib_val > LOWER_BOUND_DEGREE) and (calib_val < UPPER_BOUND_DEGREE)
+
+                # Solve this inequality to find the factor to shift the range into [-180, 180] degrees
+                # values[i] = (values[i] + homing_offset + resolution * factor) / (resolution // 2) * HALF_TURN_DEGREE
+                # - HALF_TURN_DEGREE <= (values[i] + homing_offset + resolution * factor) / (resolution // 2) * HALF_TURN_DEGREE <= HALF_TURN_DEGREE
+                # (- HALF_TURN_DEGREE / HALF_TURN_DEGREE * (resolution // 2) - values[i] - homing_offset) / resolution <= factor <= (HALF_TURN_DEGREE / 180 * (resolution // 2) - values[i] - homing_offset) / resolution
+                low_factor = (
+                    -HALF_TURN_DEGREE / HALF_TURN_DEGREE * (resolution // 2) - values[i] - homing_offset
+                ) / resolution
+                upp_factor = (
+                    HALF_TURN_DEGREE / HALF_TURN_DEGREE * (resolution // 2) - values[i] - homing_offset
+                ) / resolution
+
+            elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
+                start_pos = self.calibration["start_pos"][calib_idx]
+                end_pos = self.calibration["end_pos"][calib_idx]
+
+                # Convert from initial range to range [0, 100] in %
+                calib_val = (values[i] - start_pos) / (end_pos - start_pos) * 100
+                in_range = (calib_val > LOWER_BOUND_LINEAR) and (calib_val < UPPER_BOUND_LINEAR)
+
+                # Solve this inequality to find the factor to shift the range into [0, 100] %
+                # values[i] = (values[i] - start_pos + resolution * factor) / (end_pos + resolution * factor - start_pos - resolution * factor) * 100
+                # values[i] = (values[i] - start_pos + resolution * factor) / (end_pos - start_pos) * 100
+                # 0 <= (values[i] - start_pos + resolution * factor) / (end_pos - start_pos) * 100 <= 100
+                # (start_pos - values[i]) / resolution <= factor <= (end_pos - values[i]) / resolution
+                low_factor = (start_pos - values[i]) / resolution
+                upp_factor = (end_pos - values[i]) / resolution
+
+            if not in_range:
+                # Get first integer between the two bounds
+                if low_factor < upp_factor:
+                    factor = math.ceil(low_factor)
+
+                    if factor > upp_factor:
+                        raise ValueError(f"No integer found between bounds [{low_factor=}, {upp_factor=}]")
+                else:
+                    factor = math.ceil(upp_factor)
+
+                    if factor > low_factor:
+                        raise ValueError(f"No integer found between bounds [{low_factor=}, {upp_factor=}]")
+
+                if CalibrationMode[calib_mode] == CalibrationMode.DEGREE:
+                    out_of_range_str = f"{LOWER_BOUND_DEGREE} < {calib_val} < {UPPER_BOUND_DEGREE} degrees"
+                    in_range_str = f"{LOWER_BOUND_DEGREE} < {calib_val} < {UPPER_BOUND_DEGREE} degrees"
+                elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
+                    out_of_range_str = f"{LOWER_BOUND_LINEAR} < {calib_val} < {UPPER_BOUND_LINEAR} %"
+                    in_range_str = f"{LOWER_BOUND_LINEAR} < {calib_val} < {UPPER_BOUND_LINEAR} %"
+
+                logging.warning(
+                    f"Auto-correct calibration of motor '{name}' by shifting value by {abs(factor)} full turns, "
+                    f"from '{out_of_range_str}' to '{in_range_str}'."
+                )
+
+                # A full turn corresponds to 360 degrees but also to 4096 steps for a motor resolution of 4096.
+                self.calibration["homing_offset"][calib_idx] += resolution * factor
+
     def revert_calibration(self, values: np.ndarray | list, motor_names: list[str] | None):
         """Inverse of `apply_calibration`."""
         if motor_names is None:
@@ -484,11 +596,23 @@ class FeetechMotorsBus:
             calib_mode = self.calibration["calib_mode"][calib_idx]
 
             if CalibrationMode[calib_mode] == CalibrationMode.DEGREE:
-                motor_idx, model = self.motors[name]
+                drive_mode = self.calibration["drive_mode"][calib_idx]
+                homing_offset = self.calibration["homing_offset"][calib_idx]
+                _, model = self.motors[name]
+                resolution = self.model_resolution[model]
 
-                # Convert degrees to homed ticks, then convert the homed ticks to raw ticks
-                values[i] = convert_degrees_to_ticks(values[i], model)
-                values[i] = adjusted_to_motor_ticks(values[i], model, self, motor_idx)
+                # Convert from nominal 0-centered degree range [-180, 180] to
+                # 0-centered resolution range (e.g. [-2048, 2048] for resolution=4096)
+                values[i] = values[i] / HALF_TURN_DEGREE * (resolution // 2)
+
+                # Subtract the homing offsets to come back to actual motor range of values
+                # which can be arbitrary.
+                values[i] -= homing_offset
+
+                # Remove drive mode, which is the rotation direction of the motor, to come back to
+                # actual motor rotation direction which can be arbitrary.
+                if drive_mode:
+                    values[i] *= -1
 
             elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
                 start_pos = self.calibration["start_pos"][calib_idx]
@@ -501,9 +625,46 @@ class FeetechMotorsBus:
         values = np.round(values).astype(np.int32)
         return values
 
+    def avoid_rotation_reset(self, values, motor_names, data_name):
+        if data_name not in self.track_positions:
+            self.track_positions[data_name] = {
+                "prev": [None] * len(self.motor_names),
+                # Assume False at initialization
+                "below_zero": [False] * len(self.motor_names),
+                "above_max": [False] * len(self.motor_names),
+            }
+
+        track = self.track_positions[data_name]
+
+        if motor_names is None:
+            motor_names = self.motor_names
+
+        for i, name in enumerate(motor_names):
+            idx = self.motor_names.index(name)
+
+            if track["prev"][idx] is None:
+                track["prev"][idx] = values[i]
+                continue
+
+            # Detect a full rotation occurred
+            if abs(track["prev"][idx] - values[i]) > 2048:
+                # Position went below 0 and got reset to 4095
+                if track["prev"][idx] < values[i]:
+                    # So we set negative value by adding a full rotation
+                    values[i] -= 4096
+
+                # Position went above 4095 and got reset to 0
+                elif track["prev"][idx] > values[i]:
+                    # So we add a full rotation
+                    values[i] += 4096
+
+            track["prev"][idx] = values[i]
+
+        return values
+
     def read_with_motor_ids(self, motor_models, motor_ids, data_name, num_retry=NUM_READ_RETRY):
         if self.mock:
-            import tests.motors.mock_scservo_sdk as scs
+            import tests.mock_scservo_sdk as scs
         else:
             import scservo_sdk as scs
 
@@ -541,7 +702,7 @@ class FeetechMotorsBus:
 
     def read(self, data_name, motor_names: str | list[str] | None = None):
         if self.mock:
-            import tests.motors.mock_scservo_sdk as scs
+            import tests.mock_scservo_sdk as scs
         else:
             import scservo_sdk as scs
 
@@ -574,7 +735,7 @@ class FeetechMotorsBus:
             self.port_handler.ser.reset_output_buffer()
             self.port_handler.ser.reset_input_buffer()
 
-            # Create new group reader
+            # create new group reader
             self.group_readers[group_key] = scs.GroupSyncRead(
                 self.port_handler, self.packet_handler, addr, bytes
             )
@@ -599,8 +760,15 @@ class FeetechMotorsBus:
 
         values = np.array(values)
 
+        # Convert to signed int to use range [-2048, 2048] for our motor positions.
+        if data_name in CONVERT_UINT32_TO_INT32_REQUIRED:
+            values = values.astype(np.int32)
+
+        if data_name in CALIBRATION_REQUIRED:
+            values = self.avoid_rotation_reset(values, motor_names, data_name)
+
         if data_name in CALIBRATION_REQUIRED and self.calibration is not None:
-            values = self.apply_calibration(values, motor_names)
+            values = self.apply_calibration_autocorrect(values, motor_names)
 
         # log the number of seconds it took to read the data from the motors
         delta_ts_name = get_log_name("delta_timestamp_s", "read", data_name, motor_names)
@@ -614,7 +782,7 @@ class FeetechMotorsBus:
 
     def write_with_motor_ids(self, motor_models, motor_ids, data_name, values, num_retry=NUM_WRITE_RETRY):
         if self.mock:
-            import tests.motors.mock_scservo_sdk as scs
+            import tests.mock_scservo_sdk as scs
         else:
             import scservo_sdk as scs
 
@@ -650,7 +818,7 @@ class FeetechMotorsBus:
         start_time = time.perf_counter()
 
         if self.mock:
-            import tests.motors.mock_scservo_sdk as scs
+            import tests.mock_scservo_sdk as scs
         else:
             import scservo_sdk as scs
 

lerobot/common/robot_devices/robots/feetech_calibration.py

@@ -11,11 +11,18 @@
 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 # See the License for the specific language governing permissions and
 # limitations under the License.
+
+"""Logic to calibrate a robot arm built with feetech motors"""
+# TODO(rcadene, aliberts): move this logic into the robot code when refactoring
+
+import time
+
 import numpy as np
 
 from lerobot.common.robot_devices.motors.feetech import (
     CalibrationMode,
     TorqueMode,
+    convert_degrees_to_steps,
 )
 from lerobot.common.robot_devices.motors.utils import MotorsBus
 
@@ -23,164 +30,469 @@ URL_TEMPLATE = (
     "https://raw.githubusercontent.com/huggingface/lerobot/main/media/{robot}/{arm}_{position}.webp"
 )
 
+# The following positions are provided in nominal degree range ]-180, +180[
+# For more info on these constants, see comments in the code where they get used.
+ZERO_POSITION_DEGREE = 0
+ROTATED_POSITION_DEGREE = 90
 
-def disable_torque(arm: MotorsBus):
+
+def assert_drive_mode(drive_mode):
+    # `drive_mode` is in [0,1] with 0 means original rotation direction for the motor, and 1 means inverted.
+    if not np.all(np.isin(drive_mode, [0, 1])):
+        raise ValueError(f"`drive_mode` contains values other than 0 or 1: ({drive_mode})")
+
+
+def apply_drive_mode(position, drive_mode):
+    assert_drive_mode(drive_mode)
+    # Convert `drive_mode` from [0, 1] with 0 indicates original rotation direction and 1 inverted,
+    # to [-1, 1] with 1 indicates original rotation direction and -1 inverted.
+    signed_drive_mode = -(drive_mode * 2 - 1)
+    position *= signed_drive_mode
+    return position
+
+
+def move_until_block(arm, motor_name, positive_direction=True, while_move_hook=None):
+    count = 0
+    while True:
+        present_pos = arm.read("Present_Position", motor_name)
+        if positive_direction:
+            # Move +100 steps every time. Lower the steps to lower the speed at which the arm moves.
+            arm.write("Goal_Position", present_pos + 100, motor_name)
+        else:
+            arm.write("Goal_Position", present_pos - 100, motor_name)
+
+        if while_move_hook is not None:
+            while_move_hook()
+
+        present_pos = arm.read("Present_Position", motor_name).item()
+        present_speed = arm.read("Present_Speed", motor_name).item()
+        present_current = arm.read("Present_Current", motor_name).item()
+        # present_load = arm.read("Present_Load", motor_name).item()
+        # present_voltage = arm.read("Present_Voltage", motor_name).item()
+        # present_temperature = arm.read("Present_Temperature", motor_name).item()
+
+        # print(f"{present_pos=}")
+        # print(f"{present_speed=}")
+        # print(f"{present_current=}")
+        # print(f"{present_load=}")
+        # print(f"{present_voltage=}")
+        # print(f"{present_temperature=}")
+
+        if present_speed == 0 and present_current > 40:
+            count += 1
+            if count > 100 or present_current > 300:
+                return present_pos
+        else:
+            count = 0
+
+
+def move_to_calibrate(
+    arm,
+    motor_name,
+    invert_drive_mode=False,
+    positive_first=True,
+    in_between_move_hook=None,
+    while_move_hook=None,
+):
+    initial_pos = arm.read("Present_Position", motor_name)
+
+    if positive_first:
+        p_present_pos = move_until_block(
+            arm, motor_name, positive_direction=True, while_move_hook=while_move_hook
+        )
+    else:
+        n_present_pos = move_until_block(
+            arm, motor_name, positive_direction=False, while_move_hook=while_move_hook
+        )
+
+    if in_between_move_hook is not None:
+        in_between_move_hook()
+
+    if positive_first:
+        n_present_pos = move_until_block(
+            arm, motor_name, positive_direction=False, while_move_hook=while_move_hook
+        )
+    else:
+        p_present_pos = move_until_block(
+            arm, motor_name, positive_direction=True, while_move_hook=while_move_hook
+        )
+
+    zero_pos = (n_present_pos + p_present_pos) / 2
+
+    calib_data = {
+        "initial_pos": initial_pos,
+        "homing_offset": zero_pos if invert_drive_mode else -zero_pos,
+        "invert_drive_mode": invert_drive_mode,
+        "drive_mode": -1 if invert_drive_mode else 0,
+        "zero_pos": zero_pos,
+        "start_pos": n_present_pos if invert_drive_mode else p_present_pos,
+        "end_pos": p_present_pos if invert_drive_mode else n_present_pos,
+    }
+    return calib_data
+
+
+def apply_offset(calib, offset):
+    calib["zero_pos"] += offset
+    if calib["drive_mode"]:
+        calib["homing_offset"] += offset
+    else:
+        calib["homing_offset"] -= offset
+    return calib
+
+
+def run_arm_auto_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
+    if robot_type == "so100":
+        return run_arm_auto_calibration_so100(arm, robot_type, arm_name, arm_type)
+    elif robot_type == "moss":
+        return run_arm_auto_calibration_moss(arm, robot_type, arm_name, arm_type)
+    else:
+        raise ValueError(robot_type)
+
+
+def run_arm_auto_calibration_so100(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
+    """All the offsets and magic numbers are hand tuned, and are unique to SO-100 follower arms"""
     if (arm.read("Torque_Enable") != TorqueMode.DISABLED.value).any():
         raise ValueError("To run calibration, the torque must be disabled on all motors.")
 
+    if not (robot_type == "so100" and arm_type == "follower"):
+        raise NotImplementedError("Auto calibration only supports the follower of so100 arms for now.")
 
-def get_calibration_modes(arm: MotorsBus):
-    """Returns calibration modes for each motor (DEGREE for rotational, LINEAR for gripper)."""
-    return [
-        CalibrationMode.LINEAR.name if name == "gripper" else CalibrationMode.DEGREE.name
-        for name in arm.motor_names
-    ]
+    print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...")
 
+    print("\nMove arm to initial position")
+    print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="initial"))
+    input("Press Enter to continue...")
 
-def reset_offset(motor_id, motor_bus):
-    # Open the write lock, changes to EEPROM do NOT persist yet
-    motor_bus.write("Lock", 1)
+    # Lower the acceleration of the motors (in [0,254])
+    initial_acceleration = arm.read("Acceleration")
+    arm.write("Lock", 0)
+    arm.write("Acceleration", 10)
+    time.sleep(1)
 
-    # Set offset to 0
-    motor_name = motor_bus.motor_names[motor_id - 1]
-    motor_bus.write("Offset", 0, motor_names=[motor_name])
+    arm.write("Torque_Enable", TorqueMode.ENABLED.value)
 
-    # Close the write lock, changes to EEPROM do persist
-    motor_bus.write("Lock", 0)
+    print(f'{arm.read("Present_Position", "elbow_flex")=}')
 
-    # Confirm that the offset is zero by reading it back
-    confirmed_offset = motor_bus.read("Offset")[motor_id - 1]
-    print(f"Offset for motor {motor_id} reset to: {confirmed_offset}")
-    return confirmed_offset
+    calib = {}
 
+    init_wf_pos = arm.read("Present_Position", "wrist_flex")
+    init_sl_pos = arm.read("Present_Position", "shoulder_lift")
+    init_ef_pos = arm.read("Present_Position", "elbow_flex")
+    arm.write("Goal_Position", init_wf_pos - 800, "wrist_flex")
+    arm.write("Goal_Position", init_sl_pos + 150 + 1024, "shoulder_lift")
+    arm.write("Goal_Position", init_ef_pos - 2048, "elbow_flex")
+    time.sleep(2)
 
-def calibrate_homing_motor(motor_id, motor_bus):
-    reset_offset(motor_id, motor_bus)
+    print("Calibrate shoulder_pan")
+    calib["shoulder_pan"] = move_to_calibrate(arm, "shoulder_pan")
+    arm.write("Goal_Position", calib["shoulder_pan"]["zero_pos"], "shoulder_pan")
+    time.sleep(1)
 
-    home_ticks = motor_bus.read("Present_Position")[motor_id - 1]  # Read index starts at 0
-    print(f"Encoder offset (present position in homing position): {home_ticks}")
+    print("Calibrate gripper")
+    calib["gripper"] = move_to_calibrate(arm, "gripper", invert_drive_mode=True)
+    time.sleep(1)
 
-    return home_ticks
+    print("Calibrate wrist_flex")
+    calib["wrist_flex"] = move_to_calibrate(arm, "wrist_flex")
+    calib["wrist_flex"] = apply_offset(calib["wrist_flex"], offset=80)
 
+    def in_between_move_hook():
+        nonlocal arm, calib
+        time.sleep(2)
+        ef_pos = arm.read("Present_Position", "elbow_flex")
+        sl_pos = arm.read("Present_Position", "shoulder_lift")
+        arm.write("Goal_Position", ef_pos + 1024, "elbow_flex")
+        arm.write("Goal_Position", sl_pos - 1024, "shoulder_lift")
+        time.sleep(2)
 
-def calibrate_linear_motor(motor_id, motor_bus):
-    motor_names = motor_bus.motor_names
-    motor_name = motor_names[motor_id - 1]
+    print("Calibrate elbow_flex")
+    calib["elbow_flex"] = move_to_calibrate(
+        arm, "elbow_flex", positive_first=False, in_between_move_hook=in_between_move_hook
+    )
+    calib["elbow_flex"] = apply_offset(calib["elbow_flex"], offset=80 - 1024)
 
-    reset_offset(motor_id, motor_bus)
+    arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"] + 1024 + 512, "elbow_flex")
+    time.sleep(1)
 
-    input(f"Close the {motor_name}, then press Enter...")
-    start_pos = motor_bus.read("Present_Position")[motor_id - 1]  # Read index starts ar 0
-    print(f"  [Motor {motor_id}] start position recorded: {start_pos}")
+    def in_between_move_hook():
+        nonlocal arm, calib
+        arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"], "elbow_flex")
 
-    input(f"Open the {motor_name} fully, then press Enter...")
-    end_pos = motor_bus.read("Present_Position")[motor_id - 1]  # Read index starts ar 0
-    print(f"  [Motor {motor_id}] end position recorded: {end_pos}")
+    print("Calibrate shoulder_lift")
+    calib["shoulder_lift"] = move_to_calibrate(
+        arm,
+        "shoulder_lift",
+        invert_drive_mode=True,
+        positive_first=False,
+        in_between_move_hook=in_between_move_hook,
+    )
+    # add an 30 steps as offset to align with body
+    calib["shoulder_lift"] = apply_offset(calib["shoulder_lift"], offset=1024 - 50)
 
-    return start_pos, end_pos
+    def while_move_hook():
+        nonlocal arm, calib
+        positions = {
+            "shoulder_lift": round(calib["shoulder_lift"]["zero_pos"] - 1600),
+            "elbow_flex": round(calib["elbow_flex"]["zero_pos"] + 1700),
+            "wrist_flex": round(calib["wrist_flex"]["zero_pos"] + 800),
+            "gripper": round(calib["gripper"]["end_pos"]),
+        }
+        arm.write("Goal_Position", list(positions.values()), list(positions.keys()))
 
+    arm.write("Goal_Position", round(calib["shoulder_lift"]["zero_pos"] - 1600), "shoulder_lift")
+    time.sleep(2)
+    arm.write("Goal_Position", round(calib["elbow_flex"]["zero_pos"] + 1700), "elbow_flex")
+    time.sleep(2)
+    arm.write("Goal_Position", round(calib["wrist_flex"]["zero_pos"] + 800), "wrist_flex")
+    time.sleep(2)
+    arm.write("Goal_Position", round(calib["gripper"]["end_pos"]), "gripper")
+    time.sleep(2)
 
-def single_motor_calibration(arm: MotorsBus, motor_id: int):
-    """Calibrates a single motor and returns its calibration data for updating the calibration file."""
+    print("Calibrate wrist_roll")
+    calib["wrist_roll"] = move_to_calibrate(
+        arm, "wrist_roll", invert_drive_mode=True, positive_first=False, while_move_hook=while_move_hook
+    )
 
-    disable_torque(arm)
-    print(f"\n--- Calibrating Motor {motor_id} ---")
+    arm.write("Goal_Position", calib["wrist_roll"]["zero_pos"], "wrist_roll")
+    time.sleep(1)
+    arm.write("Goal_Position", calib["gripper"]["start_pos"], "gripper")
+    time.sleep(1)
+    arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"], "wrist_flex")
+    time.sleep(1)
+    arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"] + 2048, "elbow_flex")
+    arm.write("Goal_Position", calib["shoulder_lift"]["zero_pos"] - 2048, "shoulder_lift")
+    time.sleep(1)
+    arm.write("Goal_Position", calib["shoulder_pan"]["zero_pos"], "shoulder_pan")
+    time.sleep(1)
 
-    start_pos = 0
-    end_pos = 0
-    encoder_offset = 0
+    calib_modes = []
+    for name in arm.motor_names:
+        if name == "gripper":
+            calib_modes.append(CalibrationMode.LINEAR.name)
+        else:
+            calib_modes.append(CalibrationMode.DEGREE.name)
 
-    if motor_id == 6:
-        start_pos, end_pos = calibrate_linear_motor(motor_id, arm)
-    else:
-        input("Move the motor to (zero) position, then press Enter...")
-        encoder_offset = calibrate_homing_motor(motor_id, arm)
-
-    print(f"Calibration for motor ID:{motor_id} done.")
-
-    # Create a calibration dictionary for the single motor
     calib_dict = {
-        "homing_offset": int(encoder_offset),
-        "drive_mode": 0,
-        "start_pos": int(start_pos),
-        "end_pos": int(end_pos),
-        "calib_mode": get_calibration_modes(arm)[motor_id - 1],
-        "motor_name": arm.motor_names[motor_id - 1],
+        "homing_offset": [calib[name]["homing_offset"] for name in arm.motor_names],
+        "drive_mode": [calib[name]["drive_mode"] for name in arm.motor_names],
+        "start_pos": [calib[name]["start_pos"] for name in arm.motor_names],
+        "end_pos": [calib[name]["end_pos"] for name in arm.motor_names],
+        "calib_mode": calib_modes,
+        "motor_names": arm.motor_names,
     }
 
+    # Re-enable original accerlation
+    arm.write("Lock", 0)
+    arm.write("Acceleration", initial_acceleration)
+    time.sleep(1)
+
     return calib_dict
 
 
-def run_full_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
-    """
-    Runs a full calibration process for all motors in a robotic arm.
+def run_arm_auto_calibration_moss(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
+    """All the offsets and magic numbers are hand tuned, and are unique to SO-100 follower arms"""
+    if (arm.read("Torque_Enable") != TorqueMode.DISABLED.value).any():
+        raise ValueError("To run calibration, the torque must be disabled on all motors.")
 
-    This function calibrates each motor in the arm, determining encoder offsets and
-    start/end positions for linear and rotational motors. The calibration data is then
-    stored in a dictionary for later use.
-
-    **Calibration Process:**
-    - The user is prompted to move the arm to its homing position before starting.
-    - Motors with rotational motion are calibrated using a homing method.
-    - Linear actuators (e.g., grippers) are calibrated separately.
-    - Encoder offsets, start positions, and end positions are recorded.
-
-    **Example Usage:**
-    ```python
-    run_full_arm_calibration(arm, "so100", "left", "follower")
-    ```
-    """
-    disable_torque(arm)
+    if not (robot_type == "moss" and arm_type == "follower"):
+        raise NotImplementedError("Auto calibration only supports the follower of moss arms for now.")
 
     print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...")
 
-    print("\nMove arm to homing position (middle)")
-    print(
-        "See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="zero")
-    )  # TODO(pepijn): replace with new instruction homing pos (all motors in middle) in tutorial
+    print("\nMove arm to initial position")
+    print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="initial"))
     input("Press Enter to continue...")
 
-    start_positions = np.zeros(len(arm.motor_indices))
-    end_positions = np.zeros(len(arm.motor_indices))
-    encoder_offsets = np.zeros(len(arm.motor_indices))
+    # Lower the acceleration of the motors (in [0,254])
+    initial_acceleration = arm.read("Acceleration")
+    arm.write("Lock", 0)
+    arm.write("Acceleration", 10)
+    time.sleep(1)
 
-    modes = get_calibration_modes(arm)
+    arm.write("Torque_Enable", TorqueMode.ENABLED.value)
 
-    for i, motor_id in enumerate(arm.motor_indices):
-        if modes[i] == CalibrationMode.DEGREE.name:
-            encoder_offsets[i] = calibrate_homing_motor(motor_id, arm)
-            start_positions[i] = 0
-            end_positions[i] = 0
+    sl_pos = arm.read("Present_Position", "shoulder_lift")
+    arm.write("Goal_Position", sl_pos - 1024 - 450, "shoulder_lift")
+    ef_pos = arm.read("Present_Position", "elbow_flex")
+    arm.write("Goal_Position", ef_pos + 1024 + 450, "elbow_flex")
+    time.sleep(2)
 
-    for i, motor_id in enumerate(arm.motor_indices):
-        if modes[i] == CalibrationMode.LINEAR.name:
-            start_positions[i], end_positions[i] = calibrate_linear_motor(motor_id, arm)
-            encoder_offsets[i] = 0
+    calib = {}
 
-    print("\nMove arm to rest position")
-    input("Press Enter to continue...")
+    print("Calibrate shoulder_pan")
+    calib["shoulder_pan"] = move_to_calibrate(arm, "shoulder_pan")
+    arm.write("Goal_Position", calib["shoulder_pan"]["zero_pos"], "shoulder_pan")
+    time.sleep(1)
 
-    print(f"\n calibration of {robot_type} {arm_name} {arm_type} done!")
+    print("Calibrate gripper")
+    calib["gripper"] = move_to_calibrate(arm, "gripper", invert_drive_mode=True)
+    time.sleep(1)
 
-    # Force drive_mode values (can be static)
-    drive_modes = [0, 1, 0, 0, 1, 0]
+    print("Calibrate wrist_flex")
+    calib["wrist_flex"] = move_to_calibrate(arm, "wrist_flex", invert_drive_mode=True)
+    calib["wrist_flex"] = apply_offset(calib["wrist_flex"], offset=-210 + 1024)
+
+    wr_pos = arm.read("Present_Position", "wrist_roll")
+    arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"] - 1024, "wrist_flex")
+    time.sleep(1)
+    arm.write("Goal_Position", wr_pos - 1024, "wrist_roll")
+    time.sleep(1)
+    arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"] - 2048, "wrist_flex")
+    time.sleep(1)
+    arm.write("Goal_Position", calib["gripper"]["end_pos"], "gripper")
+    time.sleep(1)
+
+    print("Calibrate wrist_roll")
+    calib["wrist_roll"] = move_to_calibrate(arm, "wrist_roll", invert_drive_mode=True)
+    calib["wrist_roll"] = apply_offset(calib["wrist_roll"], offset=790)
+
+    arm.write("Goal_Position", calib["wrist_roll"]["zero_pos"] - 1024, "wrist_roll")
+    arm.write("Goal_Position", calib["gripper"]["start_pos"], "gripper")
+    arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"] - 1024, "wrist_flex")
+    time.sleep(1)
+    arm.write("Goal_Position", calib["wrist_roll"]["zero_pos"], "wrist_roll")
+    arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"] - 2048, "wrist_flex")
+
+    def in_between_move_elbow_flex_hook():
+        nonlocal arm, calib
+        arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"], "wrist_flex")
+
+    print("Calibrate elbow_flex")
+    calib["elbow_flex"] = move_to_calibrate(
+        arm,
+        "elbow_flex",
+        invert_drive_mode=True,
+        in_between_move_hook=in_between_move_elbow_flex_hook,
+    )
+    arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"] - 1024, "wrist_flex")
+
+    def in_between_move_shoulder_lift_hook():
+        nonlocal arm, calib
+        sl = arm.read("Present_Position", "shoulder_lift")
+        arm.write("Goal_Position", sl - 1500, "shoulder_lift")
+        time.sleep(1)
+        arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"] + 1536, "elbow_flex")
+        time.sleep(1)
+        arm.write("Goal_Position", calib["wrist_flex"]["start_pos"], "wrist_flex")
+        time.sleep(1)
+
+    print("Calibrate shoulder_lift")
+    calib["shoulder_lift"] = move_to_calibrate(
+        arm, "shoulder_lift", in_between_move_hook=in_between_move_shoulder_lift_hook
+    )
+    calib["shoulder_lift"] = apply_offset(calib["shoulder_lift"], offset=-1024)
+
+    arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"] - 1024, "wrist_flex")
+    time.sleep(1)
+    arm.write("Goal_Position", calib["shoulder_lift"]["zero_pos"] + 2048, "shoulder_lift")
+    arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"] - 1024 - 400, "elbow_flex")
+    time.sleep(2)
+
+    calib_modes = []
+    for name in arm.motor_names:
+        if name == "gripper":
+            calib_modes.append(CalibrationMode.LINEAR.name)
+        else:
+            calib_modes.append(CalibrationMode.DEGREE.name)
 
     calib_dict = {
-        "homing_offset": encoder_offsets.astype(int).tolist(),
-        "drive_mode": drive_modes,
-        "start_pos": start_positions.astype(int).tolist(),
-        "end_pos": end_positions.astype(int).tolist(),
-        "calib_mode": get_calibration_modes(arm),
+        "homing_offset": [calib[name]["homing_offset"] for name in arm.motor_names],
+        "drive_mode": [calib[name]["drive_mode"] for name in arm.motor_names],
+        "start_pos": [calib[name]["start_pos"] for name in arm.motor_names],
+        "end_pos": [calib[name]["end_pos"] for name in arm.motor_names],
+        "calib_mode": calib_modes,
+        "motor_names": arm.motor_names,
+    }
+
+    # Re-enable original accerlation
+    arm.write("Lock", 0)
+    arm.write("Acceleration", initial_acceleration)
+    time.sleep(1)
+
+    return calib_dict
+
+
+def run_arm_manual_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
+    """This function ensures that a neural network trained on data collected on a given robot
+    can work on another robot. For instance before calibration, setting a same goal position
+    for each motor of two different robots will get two very different positions. But after calibration,
+    the two robots will move to the same position.To this end, this function computes the homing offset
+    and the drive mode for each motor of a given robot.
+
+    Homing offset is used to shift the motor position to a ]-2048, +2048[ nominal range (when the motor uses 2048 steps
+    to complete a half a turn). This range is set around an arbitrary "zero position" corresponding to all motor positions
+    being 0. During the calibration process, you will need to manually move the robot to this "zero position".
+
+    Drive mode is used to invert the rotation direction of the motor. This is useful when some motors have been assembled
+    in the opposite orientation for some robots. During the calibration process, you will need to manually move the robot
+    to the "rotated position".
+
+    After calibration, the homing offsets and drive modes are stored in a cache.
+
+    Example of usage:
+    ```python
+    run_arm_calibration(arm, "so100", "left", "follower")
+    ```
+    """
+    if (arm.read("Torque_Enable") != TorqueMode.DISABLED.value).any():
+        raise ValueError("To run calibration, the torque must be disabled on all motors.")
+
+    print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...")
+
+    print("\nMove arm to zero position")
+    print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="zero"))
+    input("Press Enter to continue...")
+
+    # We arbitrarily chose our zero target position to be a straight horizontal position with gripper upwards and closed.
+    # It is easy to identify and all motors are in a "quarter turn" position. Once calibration is done, this position will
+    # correspond to every motor angle being 0. If you set all 0 as Goal Position, the arm will move in this position.
+    zero_target_pos = convert_degrees_to_steps(ZERO_POSITION_DEGREE, arm.motor_models)
+
+    # Compute homing offset so that `present_position + homing_offset ~= target_position`.
+    zero_pos = arm.read("Present_Position")
+    homing_offset = zero_target_pos - zero_pos
+
+    # The rotated target position corresponds to a rotation of a quarter turn from the zero position.
+    # This allows to identify the rotation direction of each motor.
+    # For instance, if the motor rotates 90 degree, and its value is -90 after applying the homing offset, then we know its rotation direction
+    # is inverted. However, for the calibration being successful, we need everyone to follow the same target position.
+    # Sometimes, there is only one possible rotation direction. For instance, if the gripper is closed, there is only one direction which
+    # corresponds to opening the gripper. When the rotation direction is ambiguous, we arbitrarily rotate clockwise from the point of view
+    # of the previous motor in the kinetic chain.
+    print("\nMove arm to rotated target position")
+    print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="rotated"))
+    input("Press Enter to continue...")
+
+    rotated_target_pos = convert_degrees_to_steps(ROTATED_POSITION_DEGREE, arm.motor_models)
+
+    # Find drive mode by rotating each motor by a quarter of a turn.
+    # Drive mode indicates if the motor rotation direction should be inverted (=1) or not (=0).
+    rotated_pos = arm.read("Present_Position")
+    drive_mode = (rotated_pos < zero_pos).astype(np.int32)
+
+    # Re-compute homing offset to take into account drive mode
+    rotated_drived_pos = apply_drive_mode(rotated_pos, drive_mode)
+    homing_offset = rotated_target_pos - rotated_drived_pos
+
+    print("\nMove arm to rest position")
+    print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="rest"))
+    input("Press Enter to continue...")
+    print()
+
+    # Joints with rotational motions are expressed in degrees in nominal range of [-180, 180]
+    calib_modes = []
+    for name in arm.motor_names:
+        if name == "gripper":
+            calib_modes.append(CalibrationMode.LINEAR.name)
+        else:
+            calib_modes.append(CalibrationMode.DEGREE.name)
+
+    calib_dict = {
+        "homing_offset": homing_offset.tolist(),
+        "drive_mode": drive_mode.tolist(),
+        "start_pos": zero_pos.tolist(),
+        "end_pos": rotated_pos.tolist(),
+        "calib_mode": calib_modes,
         "motor_names": arm.motor_names,
     }
     return calib_dict
-
-
-def run_full_auto_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
-    """TODO(pepijn): Add this method later as extra
-    Example of usage:
-    ```python
-    run_full_auto_arm_calibration(arm, "so100", "left", "follower")
-    ```
-    """
-    print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...")

lerobot/common/robot_devices/robots/manipulator.py

@@ -54,73 +54,6 @@ def ensure_safe_goal_position(
     return safe_goal_pos
 
 
-def apply_feetech_offsets_from_calibration(motorsbus, calibration_dict: dict):
-    """
-    Reads 'calibration_dict' containing 'homing_offset' and 'motor_names',
-    then writes each motor's offset to the servo's internal Offset (0x1F) in EPROM.
-
-    This version is modified so each homed position (originally 0) will now read
-    2047, i.e. 180° away from 0 in the 4096-count circle. Offsets are permanently
-    stored in EEPROM, so the servo's Present_Position is hardware-shifted even
-    after power cycling.
-
-    Steps:
-      1) Subtract 2047 from the old offset (so 0 -> 2047).
-      2) Clamp to [-2047..+2047].
-      3) Encode sign bit and magnitude into a 12-bit number.
-    """
-
-    homing_offsets = calibration_dict["homing_offset"]
-    motor_names = calibration_dict["motor_names"]
-    start_pos = calibration_dict["start_pos"]
-
-    # Open the write lock, changes to EEPROM do NOT persist yet
-    motorsbus.write("Lock", 1)
-
-    # For each motor, set the 'Offset' parameter
-    for m_name, old_offset in zip(motor_names, homing_offsets, strict=False):
-        # If bus doesn’t have a motor named m_name, skip
-        if m_name not in motorsbus.motors:
-            print(f"Warning: '{m_name}' not found in motorsbus.motors; skipping offset.")
-            continue
-
-        if m_name == "gripper":
-            old_offset = start_pos  # If gripper set the offset to the start position of the gripper
-            continue
-
-        # Shift the offset so the homed position reads 2047
-        new_offset = old_offset - 2047
-
-        # Clamp to [-2047..+2047]
-        if new_offset > 2047:
-            new_offset = 2047
-            print(
-                f"Warning: '{new_offset}' is getting clamped because its larger then 2047; This should not happen!"
-            )
-        elif new_offset < -2047:
-            new_offset = -2047
-            print(
-                f"Warning: '{new_offset}' is getting clamped because its smaller then -2047; This should not happen!"
-            )
-
-        # Determine the direction (sign) bit and magnitude
-        direction_bit = 1 if new_offset < 0 else 0
-        magnitude = abs(new_offset)
-
-        # Combine sign bit (bit 11) with the magnitude (bits 0..10)
-        servo_offset = (direction_bit << 11) | magnitude
-
-        # Write offset to servo
-        motorsbus.write("Offset", servo_offset, motor_names=m_name)
-        print(
-            f"Set offset for {m_name}: "
-            f"old_offset={old_offset}, new_offset={new_offset}, servo_encoded={magnitude} + direction={direction_bit}"
-        )
-
-    motorsbus.write("Lock", 0)
-    print("Offsets have been saved to EEPROM successfully.")
-
-
 class ManipulatorRobot:
     # TODO(rcadene): Implement force feedback
     """This class allows to control any manipulator robot of various number of motors.
@@ -382,10 +315,10 @@ class ManipulatorRobot:
 
                 elif self.robot_type in ["so100", "moss", "lekiwi"]:
                     from lerobot.common.robot_devices.robots.feetech_calibration import (
-                        run_full_arm_calibration,
+                        run_arm_manual_calibration,
                     )
 
-                    calibration = run_full_arm_calibration(arm, self.robot_type, name, arm_type)
+                    calibration = run_arm_manual_calibration(arm, self.robot_type, name, arm_type)
 
                 print(f"Calibration is done! Saving calibration file '{arm_calib_path}'")
                 arm_calib_path.parent.mkdir(parents=True, exist_ok=True)
@@ -394,24 +327,12 @@ class ManipulatorRobot:
 
             return calibration
 
-            # For each follower arm
-
-        for name, arm_bus in self.follower_arms.items():
-            calibration = load_or_run_calibration_(name, arm_bus, "follower")
-            arm_bus.set_calibration(calibration)
-
-            # If this is a Feetech robot, also set the servo offset into EEPROM
-            if self.robot_type in ["so100", "lekiwi"]:
-                apply_feetech_offsets_from_calibration(arm_bus, calibration)
-
-        # For each leader arm
-        for name, arm_bus in self.leader_arms.items():
-            calibration = load_or_run_calibration_(name, arm_bus, "leader")
-            arm_bus.set_calibration(calibration)
-
-            # Optionally also set offset for leader if you want the servo offsets as well
-            if self.robot_type in ["so100", "lekiwi"]:
-                apply_feetech_offsets_from_calibration(arm_bus, calibration)
+        for name, arm in self.follower_arms.items():
+            calibration = load_or_run_calibration_(name, arm, "follower")
+            arm.set_calibration(calibration)
+        for name, arm in self.leader_arms.items():
+            calibration = load_or_run_calibration_(name, arm, "leader")
+            arm.set_calibration(calibration)
 
     def set_koch_robot_preset(self):
         def set_operating_mode_(arm):

lerobot/scripts/calibration_visualization_so100.py

@@ -1,176 +0,0 @@
-import argparse
-import json
-import time
-from pathlib import Path
-
-from lerobot.common.robot_devices.motors.feetech import (
-    FeetechMotorsBus,
-    adjusted_to_homing_ticks,
-    adjusted_to_motor_ticks,
-    convert_degrees_to_ticks,
-    convert_ticks_to_degrees,
-)
-from lerobot.common.robot_devices.robots.configs import So100RobotConfig
-from lerobot.common.robot_devices.robots.utils import make_robot_from_config
-
-
-def apply_feetech_offsets_from_calibration(motorsbus: FeetechMotorsBus, calibration_dict: dict):
-    """
-    Reads 'calibration_dict' containing 'homing_offset' and 'motor_names',
-    then writes each motor's offset to the servo's internal Offset (0x1F) in EPROM.
-
-    This version is modified so each homed position (originally 0) will now read
-    2047, i.e. 180° away from 0 in the 4096-count circle. Offsets are permanently
-    stored in EEPROM, so the servo's Present_Position is hardware-shifted even
-    after power cycling.
-
-    Steps:
-      1) Subtract 2047 from the old offset (so 0 -> 2047).
-      2) Clamp to [-2047..+2047].
-      3) Encode sign bit and magnitude into a 12-bit number.
-    """
-
-    homing_offsets = calibration_dict["homing_offset"]
-    motor_names = calibration_dict["motor_names"]
-    start_pos = calibration_dict["start_pos"]
-
-    # Open the write lock, changes to EEPROM do NOT persist yet
-    motorsbus.write("Lock", 1)
-
-    # For each motor, set the 'Offset' parameter
-    for m_name, old_offset in zip(motor_names, homing_offsets, strict=False):
-        # If bus doesn’t have a motor named m_name, skip
-        if m_name not in motorsbus.motors:
-            print(f"Warning: '{m_name}' not found in motorsbus.motors; skipping offset.")
-            continue
-
-        if m_name == "gripper":
-            old_offset = start_pos  # If gripper set the offset to the start position of the gripper
-            continue
-
-        # Shift the offset so the homed position reads 2047
-        new_offset = old_offset - 2047
-
-        # Clamp to [-2047..+2047]
-        if new_offset > 2047:
-            new_offset = 2047
-            print(
-                f"Warning: '{new_offset}' is getting clamped because its larger then 2047; This should not happen!"
-            )
-        elif new_offset < -2047:
-            new_offset = -2047
-            print(
-                f"Warning: '{new_offset}' is getting clamped because its smaller then -2047; This should not happen!"
-            )
-
-        # Determine the direction (sign) bit and magnitude
-        direction_bit = 1 if new_offset < 0 else 0
-        magnitude = abs(new_offset)
-
-        # Combine sign bit (bit 11) with the magnitude (bits 0..10)
-        servo_offset = (direction_bit << 11) | magnitude
-
-        # Write offset to servo
-        motorsbus.write("Offset", servo_offset, motor_names=m_name)
-        print(
-            f"Set offset for {m_name}: "
-            f"old_offset={old_offset}, new_offset={new_offset}, servo_encoded={magnitude} + direction={direction_bit}"
-        )
-
-    motorsbus.write("Lock", 0)
-    print("Offsets have been saved to EEPROM successfully.")
-
-
-def debug_feetech_positions(cfg, arm_arg: str):
-    """
-    Reads each joint’s (1) raw ticks, (2) homed ticks, (3) degrees, and (4) invert-adjusted ticks.
-    :param arm_arg: One of "main_leader" or "main_follower".
-    """
-    robot = make_robot_from_config(cfg)
-
-    # Parse which arm we want: 'main_leader' or 'main_follower'
-    if arm_arg not in ("main_leader", "main_follower"):
-        raise ValueError("Please specify --arm=main_leader or --arm=main_follower")
-
-    bus_config = robot.leader_arms["main"] if arm_arg == "main_leader" else robot.follower_arms["main"]
-    bus = FeetechMotorsBus(bus_config)
-
-    # Load calibration if it exists
-    calib_file = Path(robot.calibration_dir) / f"{arm_arg}.json"
-    if calib_file.exists():
-        with open(calib_file) as f:
-            calibration_dict = json.load(f)
-        bus.set_calibration(calibration_dict)
-        print(f"Loaded calibration from {calib_file}")
-    else:
-        print(f"No calibration file found at {calib_file}, skipping calibration set.")
-
-    bus.connect()
-    print(f"Connected to Feetech bus on port: {bus.port}")
-
-    # Apply offset to servo EEPROM so the servo’s Present_Position is hardware-shifted
-    if calibration_dict is not None:
-        print("Applying offsets from calibration object to servo EEPROM. Be careful—this is permanent!")
-        apply_feetech_offsets_from_calibration(bus, calibration_dict)
-
-    # Disable torque on all motors so you can move them freely by hand
-    bus.write("Torque_Enable", 0, motor_names=bus.motor_names)
-    print("Torque disabled on all joints.")
-
-    try:
-        print("\nPress Ctrl+C to quit.\n")
-        while True:
-            # Read *raw* positions (no calibration).
-            raw_positions = bus.read_with_motor_ids(
-                bus.motor_models, bus.motor_indices, data_name="Present_Position"
-            )
-
-            # Read *already-homed* positions
-            homed_positions = bus.read("Present_Position")
-
-            for i, name in enumerate(bus.motor_names):
-                motor_idx, model = bus.motors[name]
-
-                raw_ticks = raw_positions[i]  # 0..4095
-                homed_val = homed_positions[i]  # degrees or % if linear
-
-                # Manually compute "adjusted ticks" from raw ticks
-                manual_adjusted = adjusted_to_homing_ticks(raw_ticks, model, bus, motor_idx)
-                # Convert to degrees
-                manual_degs = convert_ticks_to_degrees(manual_adjusted, model)
-
-                # Convert that deg back to ticks
-                manual_ticks = convert_degrees_to_ticks(manual_degs, model)
-                # Then invert them using offset & bus drive mode
-                inv_ticks = adjusted_to_motor_ticks(manual_ticks, model, bus, motor_idx)
-
-                print(
-                    f"{name:15s} | "
-                    f"RAW={raw_ticks:4d} | "
-                    f"HOMED_FROM_READ={homed_val:7.2f} | "
-                    f"HOMED_TICKS={manual_adjusted:6d} | "
-                    f"MANUAL_ADJ_DEG={manual_degs:7.2f} | "
-                    f"MANUAL_ADJ_TICKS={manual_ticks:6d} | "
-                    f"INV_TICKS={inv_ticks:4d} "
-                )
-            print("----------------------------------------------------")
-            time.sleep(0.25)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        print("\nExiting. Disconnecting bus...")
-        bus.disconnect()
-
-
-if __name__ == "__main__":
-    parser = argparse.ArgumentParser(description="Debug Feetech positions.")
-    parser.add_argument(
-        "--arm",
-        type=str,
-        choices=["main_leader", "main_follower"],
-        default="main_leader",
-        help="Which arm to debug: 'main_leader' or 'main_follower'.",
-    )
-    args = parser.parse_args()
-    cfg = So100RobotConfig()
-    debug_feetech_positions(cfg, arm_arg=args.arm)

lerobot/scripts/configure_motor.py

@@ -145,12 +145,13 @@ def configure_motor(port, brand, model, motor_idx_des, baudrate_des):
             # the motors. Note: this configuration is not in the official STS3215 Memory Table
             motor_bus.write("Lock", 0)
             motor_bus.write("Maximum_Acceleration", 254)
-            motor_bus.write("Max_Angle_Limit", 4095)  # default 4095
-            motor_bus.write("Min_Angle_Limit", 0)  # default 0
-            motor_bus.write("Offset", 0)
-            motor_bus.write("Mode", 0)
+
             motor_bus.write("Goal_Position", 2048)
-            motor_bus.write("Lock", 1)
+            time.sleep(4)
+            print("Present Position", motor_bus.read("Present_Position"))
+
+            motor_bus.write("Offset", 0)
+            time.sleep(4)
             print("Offset", motor_bus.read("Offset"))
 
     except Exception as e:

pyproject.toml

@@ -69,6 +69,7 @@ dependencies = [
     "rerun-sdk>=0.21.0",
     "termcolor>=2.4.0",
     "torch>=2.2.1",
+    "torchcodec>=0.2.1",
     "torchvision>=0.21.0",
     "wandb>=0.16.3",
     "zarr>=2.17.0",
@@ -102,7 +103,30 @@ requires-poetry = ">=2.1"
 [tool.ruff]
 line-length = 110
 target-version = "py310"
-exclude = ["tests/artifacts/**/*.safetensors"]
+exclude = [
+    "tests/data",
+    ".bzr",
+    ".direnv",
+    ".eggs",
+    ".git",
+    ".git-rewrite",
+    ".hg",
+    ".mypy_cache",
+    ".nox",
+    ".pants.d",
+    ".pytype",
+    ".ruff_cache",
+    ".svn",
+    ".tox",
+    ".venv",
+    "__pypackages__",
+    "_build",
+    "buck-out",
+    "build",
+    "dist",
+    "node_modules",
+    "venv",
+]
 
 [tool.ruff.lint]
 select = ["E4", "E7", "E9", "F", "I", "N", "B", "C4", "SIM"]

tests/lerobot/common/datasets/test_utils.py

@@ -0,0 +1,38 @@
+#!/usr/bin/env python
+
+# Copyright 2024 The HuggingFace Inc. team. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from huggingface_hub import DatasetCard
+
+from lerobot.common.datasets.utils import create_lerobot_dataset_card
+
+
+def test_default_parameters():
+    card = create_lerobot_dataset_card()
+    assert isinstance(card, DatasetCard)
+    assert card.data.tags == ["LeRobot"]
+    assert card.data.task_categories == ["robotics"]
+    assert card.data.configs == [
+        {
+            "config_name": "default",
+            "data_files": "data/*/*.parquet",
+        }
+    ]
+
+
+def test_with_tags():
+    tags = ["tag1", "tag2"]
+    card = create_lerobot_dataset_card(tags=tags)
+    assert card.data.tags == ["LeRobot", "tag1", "tag2"]

tests/scripts/save_dataset_to_safetensors.py

@@ -23,7 +23,7 @@ If you know that your change will break backward compatibility, you should write
 doesnt need to be merged into the `main` branch. Then you need to run this script and update the tests artifacts.
 
 Example usage:
-    `python tests/artifacts/datasets/save_dataset_to_safetensors.py`
+    `python tests/scripts/save_dataset_to_safetensors.py`
 """
 
 import shutil
@@ -88,4 +88,4 @@ if __name__ == "__main__":
         "lerobot/nyu_franka_play_dataset",
         "lerobot/cmu_stretch",
     ]:
-        save_dataset_to_safetensors("tests/artifacts/datasets", repo_id=dataset)
+        save_dataset_to_safetensors("tests/data/save_dataset_to_safetensors", repo_id=dataset)

tests/scripts/save_image_transforms_to_safetensors.py

@@ -27,7 +27,7 @@ from lerobot.common.datasets.transforms import (
 )
 from lerobot.common.utils.random_utils import seeded_context
 
-ARTIFACT_DIR = Path("tests/artifacts/image_transforms")
+ARTIFACT_DIR = Path("tests/data/save_image_transforms_to_safetensors")
 DATASET_REPO_ID = "lerobot/aloha_mobile_shrimp"
 
 

tests/scripts/save_policy_to_safetensors.py

@@ -141,5 +141,5 @@ if __name__ == "__main__":
         raise RuntimeError("No policies were provided!")
     for ds_repo_id, policy, policy_kwargs, file_name_extra in artifacts_cfg:
         ds_name = ds_repo_id.split("/")[-1]
-        output_dir = Path("tests/artifacts/policies") / f"{ds_name}_{policy}_{file_name_extra}"
+        output_dir = Path("tests/data/save_policy_to_safetensors") / f"{ds_name}_{policy}_{file_name_extra}"
         save_policy_to_safetensors(output_dir, ds_repo_id, policy, policy_kwargs)

tests/test_cameras.py

@@ -146,7 +146,7 @@ def test_camera(request, camera_type, mock):
         camera.connect()
 
         if mock:
-            import tests.cameras.mock_cv2 as cv2
+            import tests.mock_cv2 as cv2
         else:
             import cv2
 

tests/test_control_robot.py

@@ -51,7 +51,7 @@ from lerobot.common.robot_devices.control_configs import (
 )
 from lerobot.configs.policies import PreTrainedConfig
 from lerobot.scripts.control_robot import calibrate, record, replay, teleoperate
-from tests.robots.test_robots import make_robot
+from tests.test_robots import make_robot
 from tests.utils import TEST_ROBOT_TYPES, mock_calibration_dir, require_robot
 
 

tests/test_datasets.py

@@ -473,12 +473,12 @@ def test_flatten_unflatten_dict():
 )
 @require_x86_64_kernel
 def test_backward_compatibility(repo_id):
-    """The artifacts for this test have been generated by `tests/artifacts/datasets/save_dataset_to_safetensors.py`."""
+    """The artifacts for this test have been generated by `tests/scripts/save_dataset_to_safetensors.py`."""
 
     # TODO(rcadene, aliberts): remove dataset download
     dataset = LeRobotDataset(repo_id, episodes=[0])
 
-    test_dir = Path("tests/artifacts/datasets") / repo_id
+    test_dir = Path("tests/data/save_dataset_to_safetensors") / repo_id
 
     def load_and_compare(i):
         new_frame = dataset[i]  # noqa: B023

tests/test_envs.py

@@ -23,7 +23,8 @@ from gymnasium.utils.env_checker import check_env
 import lerobot
 from lerobot.common.envs.factory import make_env, make_env_config
 from lerobot.common.envs.utils import preprocess_observation
-from tests.utils import require_env
+
+from .utils import require_env
 
 OBS_TYPES = ["state", "pixels", "pixels_agent_pos"]
 

tests/test_image_transforms.py

@@ -33,7 +33,7 @@ from lerobot.scripts.visualize_image_transforms import (
     save_all_transforms,
     save_each_transform,
 )
-from tests.artifacts.image_transforms.save_image_transforms_to_safetensors import ARTIFACT_DIR
+from tests.scripts.save_image_transforms_to_safetensors import ARTIFACT_DIR
 from tests.utils import require_x86_64_kernel
 
 

tests/test_policies.py

@@ -40,7 +40,7 @@ from lerobot.common.utils.random_utils import seeded_context
 from lerobot.configs.default import DatasetConfig
 from lerobot.configs.train import TrainPipelineConfig
 from lerobot.configs.types import FeatureType, NormalizationMode, PolicyFeature
-from tests.artifacts.policies.save_policy_to_safetensors import get_policy_stats
+from tests.scripts.save_policy_to_safetensors import get_policy_stats
 from tests.utils import DEVICE, require_cpu, require_env, require_x86_64_kernel
 
 
@@ -407,10 +407,12 @@ def test_backward_compatibility(ds_repo_id: str, policy_name: str, policy_kwargs
            should be updated.
         4. Check that this test now passes.
         5. Remember to restore `tests/scripts/save_policy_to_safetensors.py` to its original state.
-        6. Remember to stage and commit the resulting changes to `tests/artifacts`.
+        6. Remember to stage and commit the resulting changes to `tests/data`.
     """
     ds_name = ds_repo_id.split("/")[-1]
-    artifact_dir = Path("tests/artifacts/policies") / f"{ds_name}_{policy_name}_{file_name_extra}"
+    artifact_dir = (
+        Path("tests/data/save_policy_to_safetensors") / f"{ds_name}_{policy_name}_{file_name_extra}"
+    )
     saved_output_dict = load_file(artifact_dir / "output_dict.safetensors")
     saved_grad_stats = load_file(artifact_dir / "grad_stats.safetensors")
     saved_param_stats = load_file(artifact_dir / "param_stats.safetensors")

tests/test_utils.py

@@ -1,5 +1,3 @@
-#!/usr/bin/env python
-
 # Copyright 2024 The HuggingFace Inc. team. All rights reserved.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
@@ -13,32 +11,13 @@
 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 # See the License for the specific language governing permissions and
 # limitations under the License.
-
 import torch
 from datasets import Dataset
-from huggingface_hub import DatasetCard
 
 from lerobot.common.datasets.push_dataset_to_hub.utils import calculate_episode_data_index
-from lerobot.common.datasets.utils import create_lerobot_dataset_card, hf_transform_to_torch
-
-
-def test_default_parameters():
-    card = create_lerobot_dataset_card()
-    assert isinstance(card, DatasetCard)
-    assert card.data.tags == ["LeRobot"]
-    assert card.data.task_categories == ["robotics"]
-    assert card.data.configs == [
-        {
-            "config_name": "default",
-            "data_files": "data/*/*.parquet",
-        }
-    ]
-
-
-def test_with_tags():
-    tags = ["tag1", "tag2"]
-    card = create_lerobot_dataset_card(tags=tags)
-    assert card.data.tags == ["LeRobot", "tag1", "tag2"]
+from lerobot.common.datasets.utils import (
+    hf_transform_to_torch,
+)
 
 
 def test_calculate_episode_data_index():