diff --git a/lerobot/common/motors/feetech/__init__.py b/lerobot/common/motors/feetech/__init__.py
index 48e0b1c88..a0c15dbf7 100644
--- a/lerobot/common/motors/feetech/__init__.py
+++ b/lerobot/common/motors/feetech/__init__.py
@@ -1,4 +1,9 @@
 from .feetech import FeetechMotorsBus, TorqueMode
-from .feetech_calibration import run_arm_manual_calibration
+from .feetech_calibration import apply_feetech_offsets_from_calibration, run_full_arm_calibration
 
-__all__ = ["FeetechMotorsBus", "TorqueMode", "run_arm_manual_calibration"]
+__all__ = [
+    "FeetechMotorsBus",
+    "TorqueMode",
+    "apply_feetech_offsets_from_calibration",
+    "run_full_arm_calibration",
+]
diff --git a/lerobot/common/motors/feetech/feetech.py b/lerobot/common/motors/feetech/feetech.py
index 4a9d2cde3..8a34d1d38 100644
--- a/lerobot/common/motors/feetech/feetech.py
+++ b/lerobot/common/motors/feetech/feetech.py
@@ -13,8 +13,6 @@
 # limitations under the License.
 
 import enum
-import logging
-import math
 import time
 import traceback
 from copy import deepcopy
@@ -31,13 +29,6 @@ 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
@@ -47,7 +38,6 @@ 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)
@@ -113,8 +103,6 @@ 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,
@@ -136,15 +124,63 @@ NUM_READ_RETRY = 20
 NUM_WRITE_RETRY = 20
 
 
-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.
+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:
     """
-    resolutions = [MODEL_RESOLUTION[model] for model in models]
-    steps = degrees / 180 * np.array(resolutions) / 2
-    steps = steps.astype(int)
-    return steps
+    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
 
 
 def convert_to_bytes(value, bytes, mock=False):
@@ -304,8 +340,6 @@ class FeetechMotorsBus:
         self.group_writers = {}
         self.logs = {}
 
-        self.track_positions = {}
-
     def connect(self):
         if self.is_connected:
             raise DeviceAlreadyConnectedError(
@@ -402,33 +436,7 @@ 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
 
@@ -440,34 +448,11 @@ class FeetechMotorsBus:
             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]
+                motor_idx, model = self.motors[name]
 
-                # 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`"
-                    )
+                # 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)
 
             elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
                 start_pos = self.calibration["start_pos"][calib_idx]
@@ -489,103 +474,6 @@ 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:
@@ -596,23 +484,11 @@ class FeetechMotorsBus:
             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]
+                motor_idx, model = self.motors[name]
 
-                # 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
+                # 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)
 
             elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
                 start_pos = self.calibration["start_pos"][calib_idx]
@@ -625,43 +501,6 @@ 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
@@ -735,7 +574,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
             )
@@ -760,15 +599,8 @@ 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_autocorrect(values, motor_names)
+            values = self.apply_calibration(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)
diff --git a/lerobot/common/motors/feetech/feetech_calibration.py b/lerobot/common/motors/feetech/feetech_calibration.py
index 778dd0b25..262a5b6e8 100644
--- a/lerobot/common/motors/feetech/feetech_calibration.py
+++ b/lerobot/common/motors/feetech/feetech_calibration.py
@@ -11,488 +11,244 @@
 # 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 ..motors_bus import MotorsBus
 from .feetech import (
     CalibrationMode,
+    FeetechMotorsBus,
     TorqueMode,
-    convert_degrees_to_steps,
 )
 
 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 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"""
+def disable_torque(arm: MotorsBus):
     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.")
 
-    print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...")
+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("\nMove arm to initial position")
-    print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="initial"))
-    input("Press Enter to continue...")
 
-    # 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)
+def reset_offset(motor_id, motor_bus):
+    # Open the write lock, changes to EEPROM do NOT persist yet
+    motor_bus.write("Lock", 1)
 
-    arm.write("Torque_Enable", TorqueMode.ENABLED.value)
+    # Set offset to 0
+    motor_name = motor_bus.motor_names[motor_id - 1]
+    motor_bus.write("Offset", 0, motor_names=[motor_name])
 
-    print(f'{arm.read("Present_Position", "elbow_flex")=}')
+    # Close the write lock, changes to EEPROM do persist
+    motor_bus.write("Lock", 0)
 
-    calib = {}
+    # 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
 
-    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)
 
-    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)
+def calibrate_homing_motor(motor_id, motor_bus):
+    reset_offset(motor_id, motor_bus)
 
-    print("Calibrate gripper")
-    calib["gripper"] = move_to_calibrate(arm, "gripper", invert_drive_mode=True)
-    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 wrist_flex")
-    calib["wrist_flex"] = move_to_calibrate(arm, "wrist_flex")
-    calib["wrist_flex"] = apply_offset(calib["wrist_flex"], offset=80)
+    return home_ticks
 
-    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)
 
-    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)
+def calibrate_linear_motor(motor_id, motor_bus):
+    motor_names = motor_bus.motor_names
+    motor_name = motor_names[motor_id - 1]
 
-    arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"] + 1024 + 512, "elbow_flex")
-    time.sleep(1)
+    reset_offset(motor_id, motor_bus)
 
-    def in_between_move_hook():
-        nonlocal arm, calib
-        arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"], "elbow_flex")
+    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}")
 
-    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)
+    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}")
 
-    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()))
+    return start_pos, end_pos
 
-    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)
 
-    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
-    )
+def single_motor_calibration(arm: MotorsBus, motor_id: int):
+    """Calibrates a single motor and returns its calibration data for updating the calibration file."""
 
-    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)
+    disable_torque(arm)
+    print(f"\n--- Calibrating Motor {motor_id} ---")
 
-    calib_modes = []
-    for name in arm.motor_names:
-        if name == "gripper":
-            calib_modes.append(CalibrationMode.LINEAR.name)
-        else:
-            calib_modes.append(CalibrationMode.DEGREE.name)
+    start_pos = 0
+    end_pos = 0
+    encoder_offset = 0
 
+    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": [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,
+        "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],
     }
 
-    # Re-enable original accerlation
-    arm.write("Lock", 0)
-    arm.write("Acceleration", initial_acceleration)
-    time.sleep(1)
-
     return calib_dict
 
 
-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.")
+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.
 
-    if not (robot_type == "moss" and arm_type == "follower"):
-        raise NotImplementedError("Auto calibration only supports the follower of moss arms for now.")
+    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.
 
-    print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...")
+    **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.
 
-    print("\nMove arm to initial position")
-    print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="initial"))
-    input("Press Enter to continue...")
-
-    # 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)
-
-    arm.write("Torque_Enable", TorqueMode.ENABLED.value)
-
-    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)
-
-    calib = {}
-
-    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("Calibrate gripper")
-    calib["gripper"] = move_to_calibrate(arm, "gripper", invert_drive_mode=True)
-    time.sleep(1)
-
-    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": [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:
+    **Example Usage:**
     ```python
-    run_arm_calibration(arm, "so100", "left", "follower")
+    run_full_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.")
+    disable_torque(arm)
 
     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"))
+    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
     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)
+    start_positions = np.zeros(len(arm.motor_indices))
+    end_positions = np.zeros(len(arm.motor_indices))
+    encoder_offsets = np.zeros(len(arm.motor_indices))
 
-    # Compute homing offset so that `present_position + homing_offset ~= target_position`.
-    zero_pos = arm.read("Present_Position")
-    homing_offset = zero_target_pos - zero_pos
+    modes = get_calibration_modes(arm)
 
-    # 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...")
+    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
 
-    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
+    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
 
     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)
+    print(f"\n calibration of {robot_type} {arm_name} {arm_type} done!")
+
+    # Force drive_mode values (can be static)
+    drive_modes = [0, 1, 0, 0, 1, 0]
 
     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,
+        "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),
         "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}...")
+
+
+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.")
diff --git a/lerobot/common/robots/koch/robot_koch.py b/lerobot/common/robots/koch/robot_koch.py
index 9cf471f62..3c34bd7b0 100644
--- a/lerobot/common/robots/koch/robot_koch.py
+++ b/lerobot/common/robots/koch/robot_koch.py
@@ -49,7 +49,6 @@ class KochRobot(Robot):
         super().__init__(config)
         self.config = config
         self.robot_type = config.type
-        self.id = config.id
 
         self.arm = DynamixelMotorsBus(
             port=self.config.port,
@@ -129,19 +128,17 @@ class KochRobot(Robot):
         Rotations are expressed in degrees in nominal range of [-180, 180],
         and linear motions (like gripper of Aloha) in nominal range of [0, 100].
         """
-        arm_calib_path = self.calibration_dir / f"{self.config.id}.json"
-
-        if arm_calib_path.exists():
-            with open(arm_calib_path) as f:
+        if self.calibration_fpath.exists():
+            with open(self.calibration_fpath) as f:
                 calibration = json.load(f)
         else:
             # TODO(rcadene): display a warning in __init__ if calibration file not available
-            logging.info(f"Missing calibration file '{arm_calib_path}'")
+            logging.info(f"Missing calibration file '{self.calibration_fpath}'")
             calibration = run_arm_calibration(self.arm, self.robot_type, self.name, "follower")
 
-            logging.info(f"Calibration is done! Saving calibration file '{arm_calib_path}'")
-            arm_calib_path.parent.mkdir(parents=True, exist_ok=True)
-            with open(arm_calib_path, "w") as f:
+            logging.info(f"Calibration is done! Saving calibration file '{self.calibration_fpath}'")
+            self.calibration_fpath.parent.mkdir(parents=True, exist_ok=True)
+            with open(self.calibration_fpath, "w") as f:
                 json.dump(calibration, f)
 
         self.arm.set_calibration(calibration)
diff --git a/lerobot/common/robots/lekiwi/lekiwi_remote.py b/lerobot/common/robots/lekiwi/lekiwi_remote.py
index 1643cd5ed..1a3af5cf0 100644
--- a/lerobot/common/robots/lekiwi/lekiwi_remote.py
+++ b/lerobot/common/robots/lekiwi/lekiwi_remote.py
@@ -61,7 +61,7 @@ def calibrate_follower_arm(motors_bus, calib_dir_str):
     calib_dir.mkdir(parents=True, exist_ok=True)
     calib_file = calib_dir / "main_follower.json"
     try:
-        from lerobot.common.motors.feetech.feetech_calibration import run_arm_manual_calibration
+        from lerobot.common.motors.feetech.feetech_calibration import run_full_arm_calibration
     except ImportError:
         print("[WARNING] Calibration function not available. Skipping calibration.")
         return
@@ -72,7 +72,7 @@ def calibrate_follower_arm(motors_bus, calib_dir_str):
         print(f"[INFO] Loaded calibration from {calib_file}")
     else:
         print("[INFO] Calibration file not found. Running manual calibration...")
-        calibration = run_arm_manual_calibration(motors_bus, "lekiwi", "follower_arm", "follower")
+        calibration = run_full_arm_calibration(motors_bus, "lekiwi", "follower_arm", "follower")
         print(f"[INFO] Calibration complete. Saving to {calib_file}")
         with open(calib_file, "w") as f:
             json.dump(calibration, f)
diff --git a/lerobot/common/robots/lekiwi/robot_lekiwi.py b/lerobot/common/robots/lekiwi/robot_lekiwi.py
index 386128854..8f2f9037e 100644
--- a/lerobot/common/robots/lekiwi/robot_lekiwi.py
+++ b/lerobot/common/robots/lekiwi/robot_lekiwi.py
@@ -12,7 +12,7 @@ import zmq
 from lerobot.common.cameras.utils import make_cameras_from_configs
 from lerobot.common.errors import DeviceNotConnectedError
 from lerobot.common.motors.feetech.feetech import TorqueMode
-from lerobot.common.motors.feetech.feetech_calibration import run_arm_manual_calibration
+from lerobot.common.motors.feetech.feetech_calibration import run_full_arm_calibration
 from lerobot.common.motors.motors_bus import MotorsBus
 from lerobot.common.motors.utils import make_motors_buses_from_configs
 from lerobot.common.robots.lekiwi.configuration_lekiwi import LeKiwiRobotConfig
@@ -253,7 +253,7 @@ class MobileManipulator:
                 calibration = json.load(f)
         else:
             print(f"Missing calibration file '{arm_calib_path}'")
-            calibration = run_arm_manual_calibration(arm, self.robot_type, name, arm_type)
+            calibration = run_full_arm_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)
             with open(arm_calib_path, "w") as f:
diff --git a/lerobot/common/robots/manipulator.py b/lerobot/common/robots/manipulator.py
index 29f567886..bafc36647 100644
--- a/lerobot/common/robots/manipulator.py
+++ b/lerobot/common/robots/manipulator.py
@@ -81,6 +81,73 @@ class ManipulatorRobotConfig(RobotConfig):
                     )
 
 
+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.
@@ -342,10 +409,10 @@ class ManipulatorRobot:
 
                 elif self.robot_type in ["so100", "moss", "lekiwi"]:
                     from lerobot.common.motors.feetech.feetech_calibration import (
-                        run_arm_manual_calibration,
+                        run_full_arm_calibration,
                     )
 
-                    calibration = run_arm_manual_calibration(arm, self.robot_type, name, arm_type)
+                    calibration = run_full_arm_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)
@@ -354,12 +421,24 @@ class ManipulatorRobot:
 
             return 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)
+            # 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)
 
     def set_koch_robot_preset(self):
         def set_operating_mode_(arm):
diff --git a/lerobot/common/robots/mobile_manipulator.py b/lerobot/common/robots/mobile_manipulator.py
index 87c852459..330277915 100644
--- a/lerobot/common/robots/mobile_manipulator.py
+++ b/lerobot/common/robots/mobile_manipulator.py
@@ -26,7 +26,7 @@ import zmq
 from lerobot.common.cameras.utils import make_cameras_from_configs
 from lerobot.common.errors import DeviceNotConnectedError
 from lerobot.common.motors.feetech.feetech import TorqueMode
-from lerobot.common.motors.feetech.feetech_calibration import run_arm_manual_calibration
+from lerobot.common.motors.feetech.feetech_calibration import run_full_arm_calibration
 from lerobot.common.motors.motors_bus import MotorsBus
 from lerobot.common.motors.utils import make_motors_buses_from_configs
 from lerobot.common.robots.lekiwi.configuration_lekiwi import LeKiwiRobotConfig
@@ -267,7 +267,7 @@ class MobileManipulator:
                 calibration = json.load(f)
         else:
             print(f"Missing calibration file '{arm_calib_path}'")
-            calibration = run_arm_manual_calibration(arm, self.robot_type, name, arm_type)
+            calibration = run_full_arm_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)
             with open(arm_calib_path, "w") as f:
diff --git a/lerobot/common/robots/moss/robot_moss.py b/lerobot/common/robots/moss/robot_moss.py
index 817031358..bc6fd2b16 100644
--- a/lerobot/common/robots/moss/robot_moss.py
+++ b/lerobot/common/robots/moss/robot_moss.py
@@ -26,7 +26,8 @@ from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnecte
 from lerobot.common.motors.feetech import (
     FeetechMotorsBus,
     TorqueMode,
-    run_arm_manual_calibration,
+    apply_feetech_offsets_from_calibration,
+    run_full_arm_calibration,
 )
 
 from ..robot import Robot
@@ -46,7 +47,6 @@ class MossRobot(Robot):
         super().__init__(config)
         self.config = config
         self.robot_type = config.type
-        self.id = config.id
 
         self.arm = FeetechMotorsBus(
             port=self.config.port,
@@ -133,22 +133,21 @@ class MossRobot(Robot):
         Rotations are expressed in degrees in nominal range of [-180, 180],
         and linear motions (like gripper of Aloha) in nominal range of [0, 100].
         """
-        arm_calib_path = self.calibration_dir / f"{self.config.id}.json"
-
-        if arm_calib_path.exists():
-            with open(arm_calib_path) as f:
+        if self.calibration_fpath.exists():
+            with open(self.calibration_fpath) as f:
                 calibration = json.load(f)
         else:
             # TODO(rcadene): display a warning in __init__ if calibration file not available
-            logging.info(f"Missing calibration file '{arm_calib_path}'")
-            calibration = run_arm_manual_calibration(self.arm, self.robot_type, self.name, "follower")
+            logging.info(f"Missing calibration file '{self.calibration_fpath}'")
+            calibration = run_full_arm_calibration(self.arm, self.robot_type, self.name, "follower")
 
-            logging.info(f"Calibration is done! Saving calibration file '{arm_calib_path}'")
-            arm_calib_path.parent.mkdir(parents=True, exist_ok=True)
-            with open(arm_calib_path, "w") as f:
+            logging.info(f"Calibration is done! Saving calibration file '{self.calibration_fpath}'")
+            self.calibration_fpath.parent.mkdir(parents=True, exist_ok=True)
+            with open(self.calibration_fpath, "w") as f:
                 json.dump(calibration, f)
 
         self.arm.set_calibration(calibration)
+        apply_feetech_offsets_from_calibration(self.arm, calibration)
 
     def get_observation(self) -> dict[str, np.ndarray]:
         """The returned observations do not have a batch dimension."""
diff --git a/lerobot/common/robots/robot.py b/lerobot/common/robots/robot.py
index d08b8c329..8b68c09fb 100644
--- a/lerobot/common/robots/robot.py
+++ b/lerobot/common/robots/robot.py
@@ -17,10 +17,12 @@ class Robot(abc.ABC):
 
     def __init__(self, config: RobotConfig):
         self.robot_type = self.name
+        self.id = config.id
         self.calibration_dir = (
             config.calibration_dir if config.calibration_dir else HF_LEROBOT_CALIBRATION / ROBOTS / self.name
         )
         self.calibration_dir.mkdir(parents=True, exist_ok=True)
+        self.calibration_fpath = self.calibration_dir / f"{self.id}.json"
 
     # TODO(aliberts): create a proper Feature class for this that links with datasets
     @abc.abstractproperty
diff --git a/lerobot/common/robots/so100/__init__.py b/lerobot/common/robots/so100/__init__.py
index f3803002a..81b064c4c 100644
--- a/lerobot/common/robots/so100/__init__.py
+++ b/lerobot/common/robots/so100/__init__.py
@@ -1,4 +1,4 @@
-from .configuration_so100 import So100RobotConfig
-from .robot_so100 import So100Robot
+from .configuration_so100 import SO100RobotConfig
+from .robot_so100 import SO100Robot
 
-__all__ = ["So100RobotConfig", "So100Robot"]
+__all__ = ["SO100RobotConfig", "SO100Robot"]
diff --git a/lerobot/common/robots/so100/configuration_so100.py b/lerobot/common/robots/so100/configuration_so100.py
index ac04b8470..969f80600 100644
--- a/lerobot/common/robots/so100/configuration_so100.py
+++ b/lerobot/common/robots/so100/configuration_so100.py
@@ -7,7 +7,7 @@ from ..config import RobotConfig
 
 @RobotConfig.register_subclass("so100")
 @dataclass
-class So100RobotConfig(RobotConfig):
+class SO100RobotConfig(RobotConfig):
     # Port to connect to the robot
     port: str
 
@@ -16,15 +16,5 @@ class So100RobotConfig(RobotConfig):
     # the number of motors in your follower arms.
     max_relative_target: int | None = None
 
-    mock: bool = False
-
-    # motors
-    shoulder_pan: tuple = (1, "sts3215")
-    shoulder_lift: tuple = (2, "sts3215")
-    elbow_flex: tuple = (3, "sts3215")
-    wrist_flex: tuple = (4, "sts3215")
-    wrist_roll: tuple = (5, "sts3215")
-    gripper: tuple = (6, "sts3215")
-
     # cameras
     cameras: dict[str, CameraConfig] = field(default_factory=dict)
diff --git a/lerobot/common/robots/so100/robot_so100.py b/lerobot/common/robots/so100/robot_so100.py
index 1895627eb..3bef70421 100644
--- a/lerobot/common/robots/so100/robot_so100.py
+++ b/lerobot/common/robots/so100/robot_so100.py
@@ -26,37 +26,37 @@ from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnecte
 from lerobot.common.motors.feetech import (
     FeetechMotorsBus,
     TorqueMode,
-    run_arm_manual_calibration,
+    apply_feetech_offsets_from_calibration,
+    run_full_arm_calibration,
 )
 
 from ..robot import Robot
 from ..utils import ensure_safe_goal_position
-from .configuration_so100 import So100RobotConfig
+from .configuration_so100 import SO100RobotConfig
 
 
-class So100Robot(Robot):
+class SO100Robot(Robot):
     """
-    [SO-100 Arm](https://github.com/TheRobotStudio/SO-ARM100) designed by TheRobotStudio
+    [SO-100 Follower Arm](https://github.com/TheRobotStudio/SO-ARM100) designed by TheRobotStudio
     """
 
-    config_class = So100RobotConfig
-    name = "koch"
+    config_class = SO100RobotConfig
+    name = "so100"
 
-    def __init__(self, config: So100RobotConfig):
+    def __init__(self, config: SO100RobotConfig):
         super().__init__(config)
         self.config = config
         self.robot_type = config.type
-        self.id = config.id
 
         self.arm = FeetechMotorsBus(
             port=self.config.port,
             motors={
-                "shoulder_pan": config.shoulder_pan,
-                "shoulder_lift": config.shoulder_lift,
-                "elbow_flex": config.elbow_flex,
-                "wrist_flex": config.wrist_flex,
-                "wrist_roll": config.wrist_roll,
-                "gripper": config.gripper,
+                "shoulder_pan": (1, "sts3215"),
+                "shoulder_lift": (2, "sts3215"),
+                "elbow_flex": (3, "sts3215"),
+                "wrist_flex": (4, "sts3215"),
+                "wrist_roll": (5, "sts3215"),
+                "gripper": (6, "sts3215"),
             },
         )
         self.cameras = make_cameras_from_configs(config.cameras)
@@ -133,22 +133,21 @@ class So100Robot(Robot):
         Rotations are expressed in degrees in nominal range of [-180, 180],
         and linear motions (like gripper of Aloha) in nominal range of [0, 100].
         """
-        arm_calib_path = self.calibration_dir / f"{self.config.id}.json"
-
-        if arm_calib_path.exists():
-            with open(arm_calib_path) as f:
+        if self.calibration_fpath.exists():
+            with open(self.calibration_fpath) as f:
                 calibration = json.load(f)
         else:
             # TODO(rcadene): display a warning in __init__ if calibration file not available
-            logging.info(f"Missing calibration file '{arm_calib_path}'")
-            calibration = run_arm_manual_calibration(self.arm, self.robot_type, self.name, "follower")
+            logging.info(f"Missing calibration file '{self.calibration_fpath}'")
+            calibration = run_full_arm_calibration(self.arm, self.robot_type, self.name, "follower")
 
-            logging.info(f"Calibration is done! Saving calibration file '{arm_calib_path}'")
-            arm_calib_path.parent.mkdir(parents=True, exist_ok=True)
-            with open(arm_calib_path, "w") as f:
+            logging.info(f"Calibration is done! Saving calibration file '{self.calibration_fpath}'")
+            self.calibration_fpath.parent.mkdir(parents=True, exist_ok=True)
+            with open(self.calibration_fpath, "w") as f:
                 json.dump(calibration, f)
 
         self.arm.set_calibration(calibration)
+        apply_feetech_offsets_from_calibration(self.arm, calibration)
 
     def get_observation(self) -> dict[str, np.ndarray]:
         """The returned observations do not have a batch dimension."""
diff --git a/lerobot/common/robots/utils.py b/lerobot/common/robots/utils.py
index db86e6cce..12b9dac5e 100644
--- a/lerobot/common/robots/utils.py
+++ b/lerobot/common/robots/utils.py
@@ -42,9 +42,9 @@ def make_robot_config(robot_type: str, **kwargs) -> RobotConfig:
 
         return MossRobotConfig(**kwargs)
     elif robot_type == "so100":
-        from .so100.configuration_so100 import So100RobotConfig
+        from .so100.configuration_so100 import SO100RobotConfig
 
-        return So100RobotConfig(**kwargs)
+        return SO100RobotConfig(**kwargs)
     elif robot_type == "stretch":
         from .stretch3.configuration_stretch3 import Stretch3RobotConfig
 
diff --git a/lerobot/common/robots/viperx/robot_viperx.py b/lerobot/common/robots/viperx/robot_viperx.py
index 4f076514f..058ac0ffa 100644
--- a/lerobot/common/robots/viperx/robot_viperx.py
+++ b/lerobot/common/robots/viperx/robot_viperx.py
@@ -39,7 +39,6 @@ class ViperXRobot(Robot):
         super().__init__(config)
         self.config = config
         self.robot_type = config.type
-        self.id = config.id
 
         self.arm = DynamixelMotorsBus(
             port=self.config.port,
@@ -150,19 +149,17 @@ class ViperXRobot(Robot):
         Rotations are expressed in degrees in nominal range of [-180, 180],
         and linear motions (like gripper of Aloha) in nominal range of [0, 100].
         """
-        arm_calib_path = self.calibration_dir / f"{self.config.id}.json"
-
-        if arm_calib_path.exists():
-            with open(arm_calib_path) as f:
+        if self.calibration_fpath.exists():
+            with open(self.calibration_fpath) as f:
                 calibration = json.load(f)
         else:
             # TODO(rcadene): display a warning in __init__ if calibration file not available
-            logging.info(f"Missing calibration file '{arm_calib_path}'")
+            logging.info(f"Missing calibration file '{self.calibration_fpath}'")
             calibration = run_arm_calibration(self.arm, self.robot_type, self.name, "follower")
 
-            logging.info(f"Calibration is done! Saving calibration file '{arm_calib_path}'")
-            arm_calib_path.parent.mkdir(parents=True, exist_ok=True)
-            with open(arm_calib_path, "w") as f:
+            logging.info(f"Calibration is done! Saving calibration file '{self.calibration_fpath}'")
+            self.calibration_fpath.parent.mkdir(parents=True, exist_ok=True)
+            with open(self.calibration_fpath, "w") as f:
                 json.dump(calibration, f)
 
         self.arm.set_calibration(calibration)
diff --git a/lerobot/common/teleoperators/keyboard/teleop_keyboard.py b/lerobot/common/teleoperators/keyboard/teleop_keyboard.py
index ff85bb40e..a0f0ab210 100644
--- a/lerobot/common/teleoperators/keyboard/teleop_keyboard.py
+++ b/lerobot/common/teleoperators/keyboard/teleop_keyboard.py
@@ -55,7 +55,6 @@ class KeyboardTeleop(Teleoperator):
         super().__init__(config)
         self.config = config
         self.robot_type = config.type
-        self.id = config.id
 
         self.pressed_keys = {}
         self.listener = None
diff --git a/lerobot/common/teleoperators/koch/teleop_koch.py b/lerobot/common/teleoperators/koch/teleop_koch.py
index 6146d9e15..e5c3fc922 100644
--- a/lerobot/common/teleoperators/koch/teleop_koch.py
+++ b/lerobot/common/teleoperators/koch/teleop_koch.py
@@ -46,7 +46,6 @@ class KochTeleop(Teleoperator):
         super().__init__(config)
         self.config = config
         self.robot_type = config.type
-        self.id = config.id
 
         self.arm = DynamixelMotorsBus(
             port=self.config.port,
@@ -106,19 +105,17 @@ class KochTeleop(Teleoperator):
         Rotations are expressed in degrees in nominal range of [-180, 180],
         and linear motions (like gripper of Aloha) in nominal range of [0, 100].
         """
-        arm_calib_path = self.calibration_dir / f"{self.id}.json"
-
-        if arm_calib_path.exists():
-            with open(arm_calib_path) as f:
+        if self.calibration_fpath.exists():
+            with open(self.calibration_fpath) as f:
                 calibration = json.load(f)
         else:
             # TODO(rcadene): display a warning in __init__ if calibration file not available
-            logging.info(f"Missing calibration file '{arm_calib_path}'")
+            logging.info(f"Missing calibration file '{self.calibration_fpath}'")
             calibration = run_arm_calibration(self.arm, self.robot_type, self.name, "leader")
 
-            logging.info(f"Calibration is done! Saving calibration file '{arm_calib_path}'")
-            arm_calib_path.parent.mkdir(parents=True, exist_ok=True)
-            with open(arm_calib_path, "w") as f:
+            logging.info(f"Calibration is done! Saving calibration file '{self.calibration_fpath}'")
+            self.calibration_fpath.parent.mkdir(parents=True, exist_ok=True)
+            with open(self.calibration_fpath, "w") as f:
                 json.dump(calibration, f)
 
         self.arm.set_calibration(calibration)
diff --git a/lerobot/common/teleoperators/so100/configuration_so100.py b/lerobot/common/teleoperators/so100/configuration_so100.py
index 57a08e157..a6a807e4e 100644
--- a/lerobot/common/teleoperators/so100/configuration_so100.py
+++ b/lerobot/common/teleoperators/so100/configuration_so100.py
@@ -24,13 +24,3 @@ from ..config import TeleoperatorConfig
 class SO100TeleopConfig(TeleoperatorConfig):
     # Port to connect to the teloperator
     port: str
-
-    mock: bool = False
-
-    # motors
-    shoulder_pan: tuple = (1, "sts3215")
-    shoulder_lift: tuple = (2, "sts3215")
-    elbow_flex: tuple = (3, "sts3215")
-    wrist_flex: tuple = (4, "sts3215")
-    wrist_roll: tuple = (5, "sts3215")
-    gripper: tuple = (6, "sts3215")
diff --git a/lerobot/common/teleoperators/so100/teleop_so100.py b/lerobot/common/teleoperators/so100/teleop_so100.py
index 6b9533934..13c928c3f 100644
--- a/lerobot/common/teleoperators/so100/teleop_so100.py
+++ b/lerobot/common/teleoperators/so100/teleop_so100.py
@@ -24,7 +24,8 @@ from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnecte
 from lerobot.common.motors.feetech import (
     FeetechMotorsBus,
     TorqueMode,
-    run_arm_manual_calibration,
+    apply_feetech_offsets_from_calibration,
+    run_full_arm_calibration,
 )
 
 from ..teleoperator import Teleoperator
@@ -33,7 +34,7 @@ from .configuration_so100 import SO100TeleopConfig
 
 class SO100Teleop(Teleoperator):
     """
-    [SO-100 Arm](https://github.com/TheRobotStudio/SO-ARM100) designed by TheRobotStudio
+    [SO-100 Leader Arm](https://github.com/TheRobotStudio/SO-ARM100) designed by TheRobotStudio
     """
 
     config_class = SO100TeleopConfig
@@ -43,17 +44,16 @@ class SO100Teleop(Teleoperator):
         super().__init__(config)
         self.config = config
         self.robot_type = config.type
-        self.id = config.id
 
         self.arm = FeetechMotorsBus(
             port=self.config.port,
             motors={
-                "shoulder_pan": config.shoulder_pan,
-                "shoulder_lift": config.shoulder_lift,
-                "elbow_flex": config.elbow_flex,
-                "wrist_flex": config.wrist_flex,
-                "wrist_roll": config.wrist_roll,
-                "gripper": config.gripper,
+                "shoulder_pan": (1, "sts3215"),
+                "shoulder_lift": (2, "sts3215"),
+                "elbow_flex": (3, "sts3215"),
+                "wrist_flex": (4, "sts3215"),
+                "wrist_roll": (5, "sts3215"),
+                "gripper": (6, "sts3215"),
             },
         )
 
@@ -86,11 +86,6 @@ class SO100Teleop(Teleoperator):
         self.arm.write("Torque_Enable", TorqueMode.DISABLED.value)
         self.calibrate()
 
-        # Enable torque on the gripper and move it to 45 degrees so that we can use it as a trigger.
-        logging.info("Activating torque.")
-        self.arm.write("Torque_Enable", TorqueMode.ENABLED.value, "gripper")
-        self.arm.write("Goal_Position", self.config.gripper_open_degree, "gripper")
-
         # Check arm can be read
         self.arm.read("Present_Position")
 
@@ -101,22 +96,21 @@ class SO100Teleop(Teleoperator):
         Rotations are expressed in degrees in nominal range of [-180, 180],
         and linear motions (like gripper of Aloha) in nominal range of [0, 100].
         """
-        arm_calib_path = self.calibration_dir / f"{self.id}.json"
-
-        if arm_calib_path.exists():
-            with open(arm_calib_path) as f:
+        if self.calibration_fpath.exists():
+            with open(self.calibration_fpath) as f:
                 calibration = json.load(f)
         else:
             # TODO(rcadene): display a warning in __init__ if calibration file not available
-            logging.info(f"Missing calibration file '{arm_calib_path}'")
-            calibration = run_arm_manual_calibration(self.arm, self.robot_type, self.name, "leader")
+            logging.info(f"Missing calibration file '{self.calibration_fpath}'")
+            calibration = run_full_arm_calibration(self.arm, self.robot_type, self.name, "leader")
 
-            logging.info(f"Calibration is done! Saving calibration file '{arm_calib_path}'")
-            arm_calib_path.parent.mkdir(parents=True, exist_ok=True)
-            with open(arm_calib_path, "w") as f:
+            logging.info(f"Calibration is done! Saving calibration file '{self.calibration_fpath}'")
+            self.calibration_fpath.parent.mkdir(parents=True, exist_ok=True)
+            with open(self.calibration_fpath, "w") as f:
                 json.dump(calibration, f)
 
         self.arm.set_calibration(calibration)
+        apply_feetech_offsets_from_calibration(self.arm, calibration)
 
     def get_action(self) -> np.ndarray:
         """The returned action does not have a batch dimension."""
diff --git a/lerobot/common/teleoperators/teleoperator.py b/lerobot/common/teleoperators/teleoperator.py
index 2aa1d6625..3b60372ed 100644
--- a/lerobot/common/teleoperators/teleoperator.py
+++ b/lerobot/common/teleoperators/teleoperator.py
@@ -14,12 +14,14 @@ class Teleoperator(abc.ABC):
     name: str
 
     def __init__(self, config: TeleoperatorConfig):
+        self.id = config.id
         self.calibration_dir = (
             config.calibration_dir
             if config.calibration_dir
             else HF_LEROBOT_CALIBRATION / TELEOPERATORS / self.name
         )
         self.calibration_dir.mkdir(parents=True, exist_ok=True)
+        self.calibration_fpath = self.calibration_dir / f"{self.id}.json"
 
     @abc.abstractproperty
     def action_feature(self) -> dict:
diff --git a/lerobot/common/teleoperators/widowx/teleop_widowx.py b/lerobot/common/teleoperators/widowx/teleop_widowx.py
index c7bcc18b2..fbb658e74 100644
--- a/lerobot/common/teleoperators/widowx/teleop_widowx.py
+++ b/lerobot/common/teleoperators/widowx/teleop_widowx.py
@@ -43,7 +43,6 @@ class WidowXTeleop(Teleoperator):
         super().__init__(config)
         self.config = config
         self.robot_type = config.type
-        self.id = config.id
 
         self.arm = DynamixelMotorsBus(
             port=self.config.port,
@@ -120,19 +119,17 @@ class WidowXTeleop(Teleoperator):
         Rotations are expressed in degrees in nominal range of [-180, 180],
         and linear motions (like gripper of Aloha) in nominal range of [0, 100].
         """
-        arm_calib_path = self.calibration_dir / f"{self.id}.json"
-
-        if arm_calib_path.exists():
-            with open(arm_calib_path) as f:
+        if self.calibration_fpath.exists():
+            with open(self.calibration_fpath) as f:
                 calibration = json.load(f)
         else:
             # TODO(rcadene): display a warning in __init__ if calibration file not available
-            logging.info(f"Missing calibration file '{arm_calib_path}'")
+            logging.info(f"Missing calibration file '{self.calibration_fpath}'")
             calibration = run_arm_calibration(self.arm, self.robot_type, self.name, "leader")
 
-            logging.info(f"Calibration is done! Saving calibration file '{arm_calib_path}'")
-            arm_calib_path.parent.mkdir(parents=True, exist_ok=True)
-            with open(arm_calib_path, "w") as f:
+            logging.info(f"Calibration is done! Saving calibration file '{self.calibration_fpath}'")
+            self.calibration_fpath.parent.mkdir(parents=True, exist_ok=True)
+            with open(self.calibration_fpath, "w") as f:
                 json.dump(calibration, f)
 
         self.arm.set_calibration(calibration)
diff --git a/lerobot/scripts/calibration_visualization_so100.py b/lerobot/scripts/calibration_visualization_so100.py
new file mode 100644
index 000000000..33573d8ba
--- /dev/null
+++ b/lerobot/scripts/calibration_visualization_so100.py
@@ -0,0 +1,100 @@
+"""
+usage:
+
+```python
+python lerobot/scripts/calibration_visualization_so100.py \
+    --teleop.type=so100 \
+    --teleop.port=/dev/tty.usbmodem58760430541
+
+python lerobot/scripts/calibration_visualization_so100.py \
+    --robot.type=so100 \
+    --robot.port=/dev/tty.usbmodem585A0084711
+```
+"""
+
+import time
+from dataclasses import dataclass
+
+import draccus
+
+from lerobot.common.motors.feetech.feetech import (
+    adjusted_to_homing_ticks,
+    adjusted_to_motor_ticks,
+    convert_degrees_to_ticks,
+    convert_ticks_to_degrees,
+)
+from lerobot.common.robots import RobotConfig
+from lerobot.common.robots.so100 import SO100Robot, SO100RobotConfig  # noqa: F401
+from lerobot.common.teleoperators import TeleoperatorConfig
+from lerobot.common.teleoperators.so100 import SO100Teleop, SO100TeleopConfig  # noqa: F401
+
+
+@dataclass
+class DebugFeetechConfig:
+    teleop: TeleoperatorConfig | None = None
+    robot: RobotConfig | None = None
+
+    def __post_init__(self):
+        if bool(self.teleop) == bool(self.robot):
+            raise ValueError("Select a single device.")
+
+
+@draccus.wrap()
+def debug_feetech_positions(cfg: DebugFeetechConfig):
+    """
+    Reads each joint's (1) raw ticks, (2) homed ticks, (3) degrees, and (4) invert-adjusted ticks.
+    """
+    device = SO100Teleop(cfg.teleop) if cfg.teleop else SO100Robot(cfg.robot)
+    device.connect()
+
+    # Disable torque on all motors so you can move them freely by hand
+    device.arm.write("Torque_Enable", 0, motor_names=device.arm.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 = device.arm.read_with_motor_ids(
+                device.arm.motor_models, device.arm.motor_indices, data_name="Present_Position"
+            )
+
+            # Read *already-homed* positions
+            homed_positions = device.arm.read("Present_Position")
+
+            for i, name in enumerate(device.arm.motor_names):
+                motor_idx, model = device.arm.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, device.arm, 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, device.arm, 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...")
+        device.disconnect()
+
+
+if __name__ == "__main__":
+    debug_feetech_positions()
diff --git a/lerobot/scripts/configure_motor.py b/lerobot/scripts/configure_motor.py
index 8b6cd2b02..6569db195 100644
--- a/lerobot/scripts/configure_motor.py
+++ b/lerobot/scripts/configure_motor.py
@@ -145,13 +145,12 @@ 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("Goal_Position", 2048)
-            time.sleep(4)
-            print("Present Position", motor_bus.read("Present_Position"))
-
+            motor_bus.write("Max_Angle_Limit", 4095)  # default 4095
+            motor_bus.write("Min_Angle_Limit", 0)  # default 0
             motor_bus.write("Offset", 0)
-            time.sleep(4)
+            motor_bus.write("Mode", 0)
+            motor_bus.write("Goal_Position", 2048)
+            motor_bus.write("Lock", 1)
             print("Offset", motor_bus.read("Offset"))
 
     except Exception as e:
diff --git a/media/so100/follower_zero.webp b/media/so100/follower_zero.webp
index 411a55545..3ba869902 100644
Binary files a/media/so100/follower_zero.webp and b/media/so100/follower_zero.webp differ
diff --git a/media/so100/leader_zero.webp b/media/so100/leader_zero.webp
index 5f8c235f9..fa2963be1 100644
Binary files a/media/so100/leader_zero.webp and b/media/so100/leader_zero.webp differ