From 409601474f58c140ce5f244badeb103ad4375180 Mon Sep 17 00:00:00 2001
From: Pepijn <pepijn@huggingface.co>
Date: Mon, 27 Jan 2025 16:24:41 +0100
Subject: [PATCH 01/19] test emoji

---
 examples/10_use_so100.md | 76 ++++++++++++++++++++++++++++++----------
 1 file changed, 58 insertions(+), 18 deletions(-)

diff --git a/examples/10_use_so100.md b/examples/10_use_so100.md
index 155bbe519..70ffc9e0a 100644
--- a/examples/10_use_so100.md
+++ b/examples/10_use_so100.md
@@ -3,52 +3,92 @@
 
 ## A. Source the parts
 
-Follow this [README](https://github.com/TheRobotStudio/SO-ARM100). It contains the bill of materials, with link to source the parts, as well as the instructions to 3D print the parts, and advices if it's your first time printing or if you don't own a 3D printer already.
-
-**Important**: Before assembling, you will first need to configure your motors. To this end, we provide a nice script, so let's first install LeRobot. After configuration, we will also guide you through assembly.
+Follow this [README](https://github.com/TheRobotStudio/SO-ARM100). It contains the bill of materials, with link to source the parts, as well as the instructions to 3D print the parts,
+and advices if it's your first time printing or if you don't own a 3D printer already.
 
 ## B. Install LeRobot
 
 On your computer:
+#### 1. [Install Miniconda](https://docs.anaconda.com/miniconda/install/#):
+<details>
+<summary><strong>Windows</strong></summary>
+
+
+Please follow the steps for Windows here: [Install Miniconda Windows](https://docs.anaconda.com/miniconda/install/#):
+</details>
+
+
+<details>
+<summary><strong>Mac M-series</strong></summary>
 
-1. [Install Miniconda](https://docs.anaconda.com/miniconda/#quick-command-line-install):
 ```bash
 mkdir -p ~/miniconda3
-# Linux:
-wget https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh -O ~/miniconda3/miniconda.sh
-# Mac M-series: 
-# curl https://repo.anaconda.com/miniconda/Miniconda3-latest-MacOSX-arm64.sh -o ~/miniconda3/miniconda.sh
-# Mac Intel: 
-# curl https://repo.anaconda.com/miniconda/Miniconda3-latest-MacOSX-x86_64.sh -o ~/miniconda3/miniconda.sh
+curl https://repo.anaconda.com/miniconda/Miniconda3-latest-MacOSX-arm64.sh -o ~/miniconda3/miniconda.sh
 bash ~/miniconda3/miniconda.sh -b -u -p ~/miniconda3
 rm ~/miniconda3/miniconda.sh
 ~/miniconda3/bin/conda init bash
 ```
 
-2. Restart shell or `source ~/.bashrc` (*Mac*: `source ~/.bash_profile`) or `source ~/.zshrc` if you're using zshell
+</details>
+
+<details>
+<summary><strong>Mac Intel</strong></summary>
 
-3. Create and activate a fresh conda environment for lerobot
 ```bash
-conda create -y -n lerobot python=3.10 && conda activate lerobot
+mkdir -p ~/miniconda3
+curl https://repo.anaconda.com/miniconda/Miniconda3-latest-MacOSX-x86_64.sh -o ~/miniconda3/miniconda.sh
+bash ~/miniconda3/miniconda.sh -b -u -p ~/miniconda3
+rm ~/miniconda3/miniconda.sh
+~/miniconda3/bin/conda init bash
 ```
 
-4. Clone LeRobot:
+</details>
+
+
+<details>
+<summary><strong>Linux</strong></summary>
+
+```bash
+mkdir -p ~/miniconda3
+wget https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh -O ~/miniconda3/miniconda.sh
+bash ~/miniconda3/miniconda.sh -b -u -p ~/miniconda3
+rm ~/miniconda3/miniconda.sh
+~/miniconda3/bin/conda init bash
+```
+
+</details>
+
+
+#### 2. Restart shell
+Copy paste in your shell: `source ~/.bashrc` or for Mac: `source ~/.bash_profile` or `source ~/.zshrc` if you're using zshell
+
+#### 3. Create and activate a fresh conda environment for lerobot
+```bash
+conda create -y -n lerobot python=3.10
+```
+
+then do to activate your conda envoirment (do this each time you open a shell to use lerobot!):
+```bash
+conda activate lerobot
+```
+
+#### 4. Clone LeRobot:
 ```bash
 git clone https://github.com/huggingface/lerobot.git ~/lerobot
 ```
 
-5. Install LeRobot with dependencies for the feetech motors:
+#### 5. Install LeRobot with dependencies for the feetech motors:
 ```bash
 cd ~/lerobot && pip install -e ".[feetech]"
 ```
 
-*For Linux only (not Mac)*: install extra dependencies for recording datasets:
+*EXTRA: For Linux only (not Mac)*: install extra dependencies for recording datasets:
 ```bash
 conda install -y -c conda-forge ffmpeg
 pip uninstall -y opencv-python
 conda install -y -c conda-forge "opencv>=4.10.0"
 ```
-
+Great! You are now done with installing LeRobot and we can begin assembeling the SO100 robots. :robot:
 ## C. Configure the motors
 
 ### 1. Find the USB ports associated to each arm
@@ -98,7 +138,7 @@ sudo chmod 666 /dev/ttyACM1
 
 #### d. Update YAML file
 
-Now that you have the ports, modify the *port* sections in `so100.yaml` 
+Now that you have the ports, modify the *port* sections in `so100.yaml`
 
 ### 2. Configure the motors
 

From 3d6f245aa7535d89f205f5c9ac9206cfbec06fff Mon Sep 17 00:00:00 2001
From: Pepijn <pepijn@huggingface.co>
Date: Mon, 27 Jan 2025 16:25:46 +0100
Subject: [PATCH 02/19] add test

---
 examples/test.md | 53 ++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 53 insertions(+)
 create mode 100644 examples/test.md

diff --git a/examples/test.md b/examples/test.md
new file mode 100644
index 000000000..477ed3c1e
--- /dev/null
+++ b/examples/test.md
@@ -0,0 +1,53 @@
+# Robot Arm Assembly Instructions
+
+Follow these steps to assemble your robot arm.
+
+---
+
+## Step 1: Unbox the Components
+Unpack the components and verify you have all the required parts.
+
+![Unboxing](images/unboxing.gif)
+
+---
+
+### Navigation
+<button onclick="location.href='#step-2-assemble-the-base';" style="padding: 10px 20px; font-size: 16px;">Next Step</button>
+
+---
+
+## Step 2: Assemble the Base
+Attach the base to the platform using the provided screws.
+
+![Assembling Base](images/assembling-base.gif)
+
+---
+
+### Navigation
+<button onclick="location.href='#step-1-unbox-the-components';" style="padding: 10px 20px; font-size: 16px;">Previous Step</button>
+<button onclick="location.href='#step-3-attach-the-arm-segments';" style="padding: 10px 20px; font-size: 16px;">Next Step</button>
+
+---
+
+## Step 3: Attach the Arm Segments
+1. Connect each segment in order, ensuring they are aligned.
+2. Tighten the screws securely.
+
+![Attaching Arm Segments](images/attaching-arm.gif)
+
+---
+
+### Navigation
+<button onclick="location.href='#step-2-assemble-the-base';" style="padding: 10px 20px; font-size: 16px;">Previous Step</button>
+<button onclick="location.href='#step-4-initialize-the-motor-controller';" style="padding: 10px 20px; font-size: 16px;">Next Step</button>
+
+---
+
+## Step 4: Initialize the Motor Controller
+Install the motor driver software and upload the configuration script:
+
+```python
+import tinymovr
+
+controller = tinymovr.Controller('can0')
+controller.calibrate_motor()

From 0193294898f97fd2d4c8b52c0ab3727dbbe97d60 Mon Sep 17 00:00:00 2001
From: Pepijn <pepijn@huggingface.co>
Date: Tue, 28 Jan 2025 09:41:36 +0100
Subject: [PATCH 03/19] add table of contents

---
 examples/10_use_so100.md | 16 +++++++++++++++-
 1 file changed, 15 insertions(+), 1 deletion(-)

diff --git a/examples/10_use_so100.md b/examples/10_use_so100.md
index 70ffc9e0a..e31140c1b 100644
--- a/examples/10_use_so100.md
+++ b/examples/10_use_so100.md
@@ -1,5 +1,19 @@
 # Using the [SO-100](https://github.com/TheRobotStudio/SO-ARM100) with LeRobot
 
+## Table of Contents
+
+  - [ ] [A. Source the parts](#a-source-the-parts)
+  - [B. Install LeRobot](#b-install-lerobot)
+  - [C. Configure the motors](#c-configure-the-motors)
+  - [D. Assemble the arms](#d-assemble-the-arms)
+  - [E. Calibrate](#e-calibrate)
+  - [F. Teleoperate](#f-teleoperate)
+  - [G. Record a dataset](#g-record-a-dataset)
+  - [H. Visualize a dataset](#h-visualize-a-dataset)
+  - [I. Replay an episode](#i-replay-an-episode)
+  - [J. Train a policy](#j-train-a-policy)
+  - [K. Evaluate your policy](#k-evaluate-your-policy)
+  - [L. More Information](#l-more-information)
 
 ## A. Source the parts
 
@@ -67,7 +81,7 @@ Copy paste in your shell: `source ~/.bashrc` or for Mac: `source ~/.bash_profile
 conda create -y -n lerobot python=3.10
 ```
 
-then do to activate your conda envoirment (do this each time you open a shell to use lerobot!):
+then activate your conda envoirment (do this each time you open a shell to use lerobot!):
 ```bash
 conda activate lerobot
 ```

From 5eefafa95863d60131d5b74bd6245ae7ff227da7 Mon Sep 17 00:00:00 2001
From: Pepijn <pepijn@huggingface.co>
Date: Tue, 28 Jan 2025 16:44:56 +0100
Subject: [PATCH 04/19] test formatting github

---
 examples/10_use_so100.md | 51 +++++++++++++++++++++++++++++++-------
 examples/test.md         | 53 ----------------------------------------
 2 files changed, 42 insertions(+), 62 deletions(-)
 delete mode 100644 examples/test.md

diff --git a/examples/10_use_so100.md b/examples/10_use_so100.md
index e31140c1b..076b46627 100644
--- a/examples/10_use_so100.md
+++ b/examples/10_use_so100.md
@@ -2,7 +2,7 @@
 
 ## Table of Contents
 
-  - [ ] [A. Source the parts](#a-source-the-parts)
+  - [A. Source the parts](#a-source-the-parts)
   - [B. Install LeRobot](#b-install-lerobot)
   - [C. Configure the motors](#c-configure-the-motors)
   - [D. Assemble the arms](#d-assemble-the-arms)
@@ -18,11 +18,12 @@
 ## A. Source the parts
 
 Follow this [README](https://github.com/TheRobotStudio/SO-ARM100). It contains the bill of materials, with link to source the parts, as well as the instructions to 3D print the parts,
-and advices if it's your first time printing or if you don't own a 3D printer already.
+and advices if it's your first time printing or if you don't own a 3D printer.
 
 ## B. Install LeRobot
 
-On your computer:
+We make heavy use of the Command Prompt (cmd). If you are not comfortable using the cmd or want to brush up using the command line you can have a look here: [Command line crash course](https://developer.mozilla.org/en-US/docs/Learn_web_development/Getting_started/Environment_setup/Command_line)
+
 #### 1. [Install Miniconda](https://docs.anaconda.com/miniconda/install/#):
 <details>
 <summary><strong>Windows</strong></summary>
@@ -102,14 +103,15 @@ conda install -y -c conda-forge ffmpeg
 pip uninstall -y opencv-python
 conda install -y -c conda-forge "opencv>=4.10.0"
 ```
-Great! You are now done with installing LeRobot and we can begin assembeling the SO100 robots. :robot:
-## C. Configure the motors
+Great :hugs:! You are now done with installing LeRobot and we can begin assembling the SO100 arms. :robot:. Every time you now want to use LeRobot you can go to the folder (/lerobot) where we installed LeRobot via the cmd and run it scripts with the provided command you can find in this document.
+
+## C. Assembling
 
 ### 1. Find the USB ports associated to each arm
 
-Designate one bus servo adapter and 6 motors for your leader arm, and similarly the other bus servo adapter and 6 motors for the follower arm.
+Designate one bus servo adapter and 6 motors for your leader arm, and similarly the other bus servo adapter and 6 motors for the follower arm. It's convenient to label them and write on each motor if it's for the follower `F` or for the leader `L` and it's ID from 1 to 6 (F1...F6 and L1...L6).
 
-#### a. Run the script to find ports
+#### a. Run the script to find port
 
 Follow Step 1 of the [assembly video](https://www.youtube.com/watch?v=FioA2oeFZ5I), which illustrates the use of our scripts below.
 
@@ -154,10 +156,13 @@ sudo chmod 666 /dev/ttyACM1
 
 Now that you have the ports, modify the *port* sections in `so100.yaml`
 
-### 2. Configure the motors
+### 2. Assembling the Base
+Let's begin with assembling the follower arm base
+
+HERE, TODO: Assemble wires in first motor, insert first moter in base and place screws, set if of motor, calibrate that motor, move on to next assmbling, repeat...
 
 #### a. Set IDs for all 12 motors
-Plug your first motor and run this script to set its ID to 1. It will also set its present position to 2048, so expect your motor to rotate:
+Plug your first motor F1 and run this script to set its ID to 1. It will also set its present position to 2048, so expect your motor to rotate. Replaxe the text after --port to the corresponding follower control board port and run this command in cmd:
 ```bash
 python lerobot/scripts/configure_motor.py \
   --port /dev/tty.usbmodem58760432961 \
@@ -348,3 +353,31 @@ As you can see, it's almost the same command as previously used to record your t
 Follow this [previous tutorial](https://github.com/huggingface/lerobot/blob/main/examples/7_get_started_with_real_robot.md#4-train-a-policy-on-your-data) for a more in-depth tutorial on controlling real robots with LeRobot.
 
 If you have any question or need help, please reach out on Discord in the channel [`#so100-arm`](https://discord.com/channels/1216765309076115607/1237741463832363039).
+
+> [!NOTE]
+> Useful information that users should know, even when skimming content.
+
+> [!TIP]
+> Helpful advice for doing things better or more easily.
+
+> [!IMPORTANT]
+> Key information users need to know to achieve their goal.
+
+> [!WARNING]
+> Urgent info that needs immediate user attention to avoid problems.
+
+> [!CAUTION]
+> Advises about risks or negative outcomes of certain actions.
+
+<textarea id="inputBox" placeholder="Paste your text here"></textarea>
+<button onclick="displayText()">Submit</button>
+<p id="displayTextHere"></p>
+
+<script>
+  function displayText() {
+    const input = document.getElementById("inputBox").value;
+    document.getElementById("displayTextHere").innerText = input;
+  }
+</script>
+
+<p align="center">Centered </p>
diff --git a/examples/test.md b/examples/test.md
deleted file mode 100644
index 477ed3c1e..000000000
--- a/examples/test.md
+++ /dev/null
@@ -1,53 +0,0 @@
-# Robot Arm Assembly Instructions
-
-Follow these steps to assemble your robot arm.
-
----
-
-## Step 1: Unbox the Components
-Unpack the components and verify you have all the required parts.
-
-![Unboxing](images/unboxing.gif)
-
----
-
-### Navigation
-<button onclick="location.href='#step-2-assemble-the-base';" style="padding: 10px 20px; font-size: 16px;">Next Step</button>
-
----
-
-## Step 2: Assemble the Base
-Attach the base to the platform using the provided screws.
-
-![Assembling Base](images/assembling-base.gif)
-
----
-
-### Navigation
-<button onclick="location.href='#step-1-unbox-the-components';" style="padding: 10px 20px; font-size: 16px;">Previous Step</button>
-<button onclick="location.href='#step-3-attach-the-arm-segments';" style="padding: 10px 20px; font-size: 16px;">Next Step</button>
-
----
-
-## Step 3: Attach the Arm Segments
-1. Connect each segment in order, ensuring they are aligned.
-2. Tighten the screws securely.
-
-![Attaching Arm Segments](images/attaching-arm.gif)
-
----
-
-### Navigation
-<button onclick="location.href='#step-2-assemble-the-base';" style="padding: 10px 20px; font-size: 16px;">Previous Step</button>
-<button onclick="location.href='#step-4-initialize-the-motor-controller';" style="padding: 10px 20px; font-size: 16px;">Next Step</button>
-
----
-
-## Step 4: Initialize the Motor Controller
-Install the motor driver software and upload the configuration script:
-
-```python
-import tinymovr
-
-controller = tinymovr.Controller('can0')
-controller.calibrate_motor()

From fa8ed524fa75651fd5b7c60b5b1a3ed1c816a7e7 Mon Sep 17 00:00:00 2001
From: Pepijn <pepijn@huggingface.co>
Date: Wed, 29 Jan 2025 16:45:47 +0100
Subject: [PATCH 05/19] add first setup new calibration

---
 examples/10_use_so100.md                      |  11 -
 .../common/robot_devices/motors/feetech.py    | 196 ++----------------
 lerobot/scripts/read_pos.py                   | 174 ++++++++++++++++
 3 files changed, 192 insertions(+), 189 deletions(-)
 create mode 100644 lerobot/scripts/read_pos.py

diff --git a/examples/10_use_so100.md b/examples/10_use_so100.md
index 076b46627..1ce4cc418 100644
--- a/examples/10_use_so100.md
+++ b/examples/10_use_so100.md
@@ -369,15 +369,4 @@ If you have any question or need help, please reach out on Discord in the channe
 > [!CAUTION]
 > Advises about risks or negative outcomes of certain actions.
 
-<textarea id="inputBox" placeholder="Paste your text here"></textarea>
-<button onclick="displayText()">Submit</button>
-<p id="displayTextHere"></p>
-
-<script>
-  function displayText() {
-    const input = document.getElementById("inputBox").value;
-    document.getElementById("displayTextHere").innerText = input;
-  }
-</script>
-
 <p align="center">Centered </p>
diff --git a/lerobot/common/robot_devices/motors/feetech.py b/lerobot/common/robot_devices/motors/feetech.py
index 0d5480f7a..06589bd9e 100644
--- a/lerobot/common/robot_devices/motors/feetech.py
+++ b/lerobot/common/robot_devices/motors/feetech.py
@@ -1,6 +1,4 @@
 import enum
-import logging
-import math
 import time
 import traceback
 from copy import deepcopy
@@ -98,10 +96,6 @@ SCS_SERIES_BAUDRATE_TABLE = {
     7: 19_200,
 }
 
-CALIBRATION_REQUIRED = ["Goal_Position", "Present_Position"]
-CONVERT_UINT32_TO_INT32_REQUIRED = ["Goal_Position", "Present_Position"]
-
-
 MODEL_CONTROL_TABLE = {
     "scs_series": SCS_SERIES_CONTROL_TABLE,
     "sts3215": SCS_SERIES_CONTROL_TABLE,
@@ -133,6 +127,17 @@ def convert_degrees_to_steps(degrees: float | np.ndarray, models: str | list[str
     return steps
 
 
+def convert_steps_to_degrees(steps: int | np.ndarray, models: str | list[str]) -> np.ndarray:
+    """This function converts the step range to the degrees 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.
+    """
+    resolutions = [MODEL_RESOLUTION[model] for model in models]
+    steps = np.array(steps, dtype=float)
+    degrees = steps * (360.0 / resolutions)
+    return degrees
+
+
 def convert_to_bytes(value, bytes, mock=False):
     if mock:
         return value
@@ -395,33 +400,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
 
@@ -482,103 +461,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:
@@ -618,43 +500,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 occured
-            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.mock_scservo_sdk as scs
@@ -724,7 +569,11 @@ class FeetechMotorsBus:
         group_key = get_group_sync_key(data_name, motor_names)
 
         if data_name not in self.group_readers:
-            # create new group reader
+            # Very Important to flush the buffer!
+            self.port_handler.ser.reset_output_buffer()
+            self.port_handler.ser.reset_input_buffer()
+
+            # Create new group reader
             self.group_readers[group_key] = scs.GroupSyncRead(
                 self.port_handler, self.packet_handler, addr, bytes
             )
@@ -749,15 +598,7 @@ 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)
+        # TODO: Apply calibration and homign offset, output is homeing = 0 and direction.
 
         # 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)
@@ -829,8 +670,7 @@ class FeetechMotorsBus:
             motor_ids.append(motor_idx)
             models.append(model)
 
-        if data_name in CALIBRATION_REQUIRED and self.calibration is not None:
-            values = self.revert_calibration(values, motor_names)
+        # TODO: add invesre of apply homing to go back to motor ticks
 
         values = values.tolist()
 
diff --git a/lerobot/scripts/read_pos.py b/lerobot/scripts/read_pos.py
new file mode 100644
index 000000000..d310418a0
--- /dev/null
+++ b/lerobot/scripts/read_pos.py
@@ -0,0 +1,174 @@
+import time
+
+
+def calibrate_motor(motor_id, motor_bus):
+    """
+    1) Prompts user to move the motor to the "home" position.
+    2) Reads servo ticks.
+    3) Calculates the offset so that 'home_ticks' becomes 0.
+    4) Returns the offset
+    """
+    print(f"\n--- Calibrating Motor {motor_id} ---")
+    input("Move the motor to middle (home) position, then press Enter...")
+    home_ticks = motor_bus.read("Present_Position")[0]
+    print(f"  [Motor {motor_id}] middle position recorded: {home_ticks}")
+
+    # Calculate how many ticks to shift so that 'home_ticks' becomes 0
+    raw_offset = -home_ticks  # negative of home_ticks
+    encoder_offset = raw_offset % 4096  # wrap to [0..4095]
+
+    # Convert to a signed range [-2048..2047]
+    if encoder_offset > 2047:
+        encoder_offset -= 4096
+
+    print(f"Encoder offset: {encoder_offset}")
+
+    return encoder_offset
+
+
+def adjusted__to_homing_ticks(raw_motor_ticks, encoder_offset):
+    shifted = (raw_motor_ticks + encoder_offset) % 4096
+    if shifted > 2047:
+        shifted -= 4096
+    return shifted
+
+
+def adjusted_to_motor_ticks(adjusted_pos, encoder_offset):
+    """
+    Inverse of read_adjusted_position().
+
+    adjusted_pos : int in [-2048 .. +2047]  (the homed, shifted value)
+    encoder_offset : int (the offset computed so that `home` becomes zero)
+
+    Returns the raw servo ticks in [0..4095].
+    """
+    if adjusted_pos < 0:
+        adjusted_pos += 4096
+
+    raw_ticks = (adjusted_pos - encoder_offset) % 4096
+    return raw_ticks
+
+
+def configure_and_calibrate():
+    from lerobot.common.robot_devices.motors.feetech import (
+        SCS_SERIES_BAUDRATE_TABLE as SERIES_BAUDRATE_TABLE,
+    )
+    from lerobot.common.robot_devices.motors.feetech import FeetechMotorsBus as MotorsBusClass
+
+    motor_idx_des = 1  # index you want to assign
+
+    # Setup motor names, indices, and models
+    motor_name = "motor"
+    motor_index_arbitrary = motor_idx_des  # Use the motor ID passed via argument
+    motor_model = "sts3215"  # Use the motor model passed via argument
+
+    # Initialize the MotorBus with the correct port and motor configurations
+    motor_bus = MotorsBusClass(
+        port="/dev/tty.usbmodem585A0078271", motors={motor_name: (motor_index_arbitrary, motor_model)}
+    )
+
+    # Try to connect to the motor bus and handle any connection-specific errors
+    try:
+        motor_bus.connect()
+        print(f"Connected on port {motor_bus.port}")
+    except OSError as e:
+        print(f"Error occurred when connecting to the motor bus: {e}")
+        return
+
+    try:
+        print("Scanning all baudrates and motor indices")
+        all_baudrates = set(SERIES_BAUDRATE_TABLE.values())
+        motor_index = -1  # Set the motor index to an out-of-range value.
+
+        for baudrate in all_baudrates:
+            motor_bus.set_bus_baudrate(baudrate)
+            present_ids = motor_bus.find_motor_indices(list(range(1, 10)))
+            if len(present_ids) > 1:
+                raise ValueError(
+                    "Error: More than one motor ID detected. This script is designed to only handle one motor at a time. Please disconnect all but one motor."
+                )
+
+            if len(present_ids) == 1:
+                if motor_index != -1:
+                    raise ValueError(
+                        "Error: More than one motor ID detected. This script is designed to only handle one motor at a time. Please disconnect all but one motor."
+                    )
+                motor_index = present_ids[0]
+
+        if motor_index == -1:
+            raise ValueError("No motors detected. Please ensure you have one motor connected.")
+
+        print(f"Motor index found at: {motor_index}")
+
+        motor_bus.write_with_motor_ids(motor_bus.motor_models, motor_index, "Lock", 0)
+
+        baudrate_des = 1000000
+
+        if baudrate != baudrate_des:
+            print(f"Setting its baudrate to {baudrate_des}")
+            baudrate_idx = list(SERIES_BAUDRATE_TABLE.values()).index(baudrate_des)
+
+            # The write can fail, so we allow retries
+            motor_bus.write_with_motor_ids(motor_bus.motor_models, motor_index, "Baud_Rate", baudrate_idx)
+            time.sleep(0.5)
+            motor_bus.set_bus_baudrate(baudrate_des)
+            present_baudrate_idx = motor_bus.read_with_motor_ids(
+                motor_bus.motor_models, motor_index, "Baud_Rate", num_retry=2
+            )
+
+            if present_baudrate_idx != baudrate_idx:
+                raise OSError("Failed to write baudrate.")
+
+        print(f"Setting its index to desired index {motor_idx_des}")
+        motor_bus.write_with_motor_ids(motor_bus.motor_models, motor_index, "Lock", 0)
+        motor_bus.write_with_motor_ids(motor_bus.motor_models, motor_index, "ID", motor_idx_des)
+
+        present_idx = motor_bus.read_with_motor_ids(motor_bus.motor_models, motor_idx_des, "ID", num_retry=2)
+        if present_idx != motor_idx_des:
+            raise OSError("Failed to write index.")
+
+        # Set Maximum_Acceleration to 254 to speedup acceleration and deceleration of
+        # 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", 0)
+        motor_bus.write("Torque_Enable", 0)
+
+        motor_bus.write("Lock", 1)
+
+        # Calibration
+        print("Offset", motor_bus.read("Offset"))
+        print("Max_Angle_Limit", motor_bus.read("Max_Angle_Limit"))
+        print("Min_Angle_Limit", motor_bus.read("Min_Angle_Limit"))
+        print("Goal_Position", motor_bus.read("Goal_Position"))
+        print("Present Position", motor_bus.read("Present_Position"))
+
+        encoder_offset = calibrate_motor(motor_idx_des, motor_bus)
+
+        try:
+            while True:
+                raw_motor_ticks = motor_bus.read("Present_Position")[0]
+                adjusted_ticks = adjusted__to_homing_ticks(raw_motor_ticks, encoder_offset)
+                inverted_adjusted_ticks = adjusted_to_motor_ticks(adjusted_ticks, encoder_offset)
+                print(
+                    f"Raw Motor ticks: {raw_motor_ticks} | Adjusted ticks: {adjusted_ticks} | Invert adjusted ticks: {inverted_adjusted_ticks}"
+                )
+                time.sleep(0.3)
+        except KeyboardInterrupt:
+            print("Stopped reading positions.")
+
+    except Exception as e:
+        print(f"Error occurred during motor configuration: {e}")
+
+    finally:
+        motor_bus.disconnect()
+        print("Disconnected from motor bus.")
+
+
+if __name__ == "__main__":
+    configure_and_calibrate()

From 0ffe2520f4703cd2fd1779e6ed7eeaaeb5e2a5af Mon Sep 17 00:00:00 2001
From: Pepijn <pepijn@huggingface.co>
Date: Thu, 30 Jan 2025 13:01:00 +0100
Subject: [PATCH 06/19] change manual motor calib

---
 .cache/calibration/so100/main_follower.json   |   1 +
 .cache/calibration/so100/main_leader.json     |   1 +
 .../common/robot_devices/motors/feetech.py    |  73 ++-
 .../robots/feetech_calibration.py             | 519 ++++--------------
 .../robot_devices/robots/manipulator.py       |   4 +-
 lerobot/scripts/configure_motor.py            |  17 +-
 6 files changed, 161 insertions(+), 454 deletions(-)
 create mode 100644 .cache/calibration/so100/main_follower.json
 create mode 100644 .cache/calibration/so100/main_leader.json

diff --git a/.cache/calibration/so100/main_follower.json b/.cache/calibration/so100/main_follower.json
new file mode 100644
index 000000000..7279b79bc
--- /dev/null
+++ b/.cache/calibration/so100/main_follower.json
@@ -0,0 +1 @@
+{"homing_offset": [-1985, 1915, 1984, 2008, 1765, 0], "drive_mode": [0, 0, 0, 0, 0, 0], "start_pos": [0, 0, 0, 0, 0, 1007], "end_pos": [0, 0, 0, 0, 0, 2448], "calib_mode": ["DEGREE", "DEGREE", "DEGREE", "DEGREE", "DEGREE", "LINEAR"], "motor_names": ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll", "gripper"]}
diff --git a/.cache/calibration/so100/main_leader.json b/.cache/calibration/so100/main_leader.json
new file mode 100644
index 000000000..70516c1c8
--- /dev/null
+++ b/.cache/calibration/so100/main_leader.json
@@ -0,0 +1 @@
+{"homing_offset": [1433, 4, 1917, -263, 972, 0], "drive_mode": [0, 0, 0, 0, 0, 0], "start_pos": [0, 0, 0, 0, 0, 1534], "end_pos": [0, 0, 0, 0, 0, 2590], "calib_mode": ["DEGREE", "DEGREE", "DEGREE", "DEGREE", "DEGREE", "LINEAR"], "motor_names": ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll", "gripper"]}
diff --git a/lerobot/common/robot_devices/motors/feetech.py b/lerobot/common/robot_devices/motors/feetech.py
index 06589bd9e..219e5ab83 100644
--- a/lerobot/common/robot_devices/motors/feetech.py
+++ b/lerobot/common/robot_devices/motors/feetech.py
@@ -20,8 +20,8 @@ MAX_ID_RANGE = 252
 # 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
+LOWER_BOUND_DEGREE = -180
+UPPER_BOUND_DEGREE = 180
 # 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
@@ -116,28 +116,59 @@ NUM_READ_RETRY = 20
 NUM_WRITE_RETRY = 20
 
 
-def convert_degrees_to_steps(degrees: float | np.ndarray, models: str | list[str]) -> np.ndarray:
+def convert_degrees_to_steps(degrees: float | np.ndarray, models: 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.
     """
-    resolutions = [MODEL_RESOLUTION[model] for model in models]
+    resolutions = np.array([MODEL_RESOLUTION[m] for m in models], dtype=float)
     steps = degrees / 180 * np.array(resolutions) / 2
     steps = steps.astype(int)
     return steps
 
 
-def convert_steps_to_degrees(steps: int | np.ndarray, models: str | list[str]) -> np.ndarray:
+def convert_steps_to_degrees(steps: int | np.ndarray, models: list[str]) -> np.ndarray:
     """This function converts the step range to the degrees 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.
     """
-    resolutions = [MODEL_RESOLUTION[model] for model in models]
+    resolutions = np.array([MODEL_RESOLUTION[m] for m in models], dtype=float)
     steps = np.array(steps, dtype=float)
     degrees = steps * (360.0 / resolutions)
     return degrees
 
 
+def adjusted__to_homing_ticks(
+    raw_motor_ticks: int | np.ndarray, encoder_offset: int, models: list[str]
+) -> np.ndarray:
+    """
+    raw_motor_ticks : int in [0..4095]  (the homed, shifted value)
+    encoder_offset : int (the offset computed so that `home` becomes zero)
+    Returns the homed servo ticks in [-2048 .. +2047].
+    """
+    resolutions = np.array([MODEL_RESOLUTION[m] for m in models], dtype=float)
+    raw_motor_ticks = np.asarray(raw_motor_ticks)
+    shifted = (raw_motor_ticks + encoder_offset) % resolutions
+    shifted = np.where(shifted > 2047, shifted - 4096, shifted)
+    return shifted
+
+
+def adjusted_to_motor_ticks(
+    adjusted_pos: int | np.ndarray, encoder_offset: int, models: list[str]
+) -> np.ndarray:
+    """
+    Inverse of read_adjusted_position().
+    adjusted_pos : int in [-2048 .. +2047]  (the homed, shifted value)
+    encoder_offset : int (the offset computed so that `home` becomes zero)
+    Returns the raw servo ticks in [0..4095].
+    """
+    resolutions = np.array([MODEL_RESOLUTION[m] for m in models], dtype=float)
+    adjusted_pos = np.asarray(adjusted_pos)
+    adjusted_pos = np.where(adjusted_pos < 0, adjusted_pos + 4096, adjusted_pos)
+    raw_ticks = (adjusted_pos - encoder_offset) % resolutions
+    return raw_ticks
+
+
 def convert_to_bytes(value, bytes, mock=False):
     if mock:
         return value
@@ -415,7 +446,10 @@ class FeetechMotorsBus:
                 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], homing_offset, [model])
+                values[i] = convert_steps_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
@@ -423,14 +457,6 @@ class FeetechMotorsBus:
                 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}. "
@@ -474,15 +500,10 @@ class FeetechMotorsBus:
                 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 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)
-
-                # Substract the homing offsets to come back to actual motor range of values
-                # which can be arbitrary.
-                values[i] -= homing_offset
+                # Convert degrees to homed ticks, then convert the homed ticks to raw ticks
+                values[i] = convert_degrees_to_steps(values[i], [model])
+                values[i] = adjusted_to_motor_ticks(values[i], homing_offset, [model])
 
                 # Remove drive mode, which is the rotation direction of the motor, to come back to
                 # actual motor rotation direction which can be arbitrary.
@@ -598,7 +619,8 @@ class FeetechMotorsBus:
 
         values = np.array(values)
 
-        # TODO: Apply calibration and homign offset, output is homeing = 0 and direction.
+        if self.calibration is not None:
+            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)
@@ -670,7 +692,8 @@ class FeetechMotorsBus:
             motor_ids.append(motor_idx)
             models.append(model)
 
-        # TODO: add invesre of apply homing to go back to motor ticks
+        if self.calibration is not None:
+            values = self.revert_calibration(values, motor_names)
 
         values = values.tolist()
 
diff --git a/lerobot/common/robot_devices/robots/feetech_calibration.py b/lerobot/common/robot_devices/robots/feetech_calibration.py
index b015951a0..5816126e9 100644
--- a/lerobot/common/robot_devices/robots/feetech_calibration.py
+++ b/lerobot/common/robot_devices/robots/feetech_calibration.py
@@ -1,14 +1,11 @@
 """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
 
@@ -16,469 +13,149 @@ 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 calibrate_homing_motor(motor_id, motor_bus):
+    """
+    1) Reads servo ticks.
+    2) Calculates the offset so that 'home_ticks' becomes 0.
+    3) Returns the offset
+    """
 
-    arm.write("Torque_Enable", TorqueMode.ENABLED.value)
+    home_ticks = motor_bus.read("Present_Position")[motor_id - 1]  # Read index starts at 0
 
-    print(f'{arm.read("Present_Position", "elbow_flex")=}')
+    # Calculate how many ticks to shift so that 'home_ticks' becomes 0
+    raw_offset = -home_ticks  # negative of home_ticks
+    encoder_offset = raw_offset % 4096  # wrap to [0..4095]
 
-    calib = {}
+    # Convert to a signed range [-2048..2047]
+    if encoder_offset > 2047:
+        encoder_offset -= 4096
 
-    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(f"Encoder offset: {encoder_offset}")
 
-    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)
+    return encoder_offset
 
-    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")
-    calib["wrist_flex"] = apply_offset(calib["wrist_flex"], offset=80)
+def calibrate_linear_motor(motor_id, motor_bus):
+    motor_names = motor_bus.motor_names
+    motor_name = motor_names[motor_id - 1]
 
-    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)
+    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 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)
+    input("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}")
 
-    arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"] + 1024 + 512, "elbow_flex")
-    time.sleep(1)
+    return start_pos, end_pos
 
-    def in_between_move_hook():
-        nonlocal arm, calib
-        arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"], "elbow_flex")
 
-    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)
+def single_motor_calibration(arm: MotorsBus, motor_id: int):
+    """Calibrates a single motor and returns its calibration data for updating the calibration file."""
 
-    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()))
+    disable_torque(arm)
+    print(f"\n--- Calibrating Motor {motor_id} ---")
 
-    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)
+    start_pos = 0
+    end_pos = 0
+    encoder_offset = 0
 
-    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
-    )
+    if motor_id == 6:
+        start_pos, end_pos = calibrate_linear_motor(motor_id, arm)
+    else:
+        input("Move the motor to middle (home) position, then press Enter...")
+        encoder_offset = calibrate_homing_motor(motor_id, arm)
 
-    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)
-
-    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"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")
+    print(
+        "See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="zero")
+    )  # TODO: replace with new instruction homing pos (all motors in center)
     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
-
-    # 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 arbitrarely 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)
+    # Call single motor calibration for each motor id
+    for i, id in enumerate(arm.motor_indices):
+        if id == 6:
+            start_positions[i], end_positions[i] = calibrate_linear_motor(id, arm)
+            encoder_offsets[i] = 0
         else:
-            calib_modes.append(CalibrationMode.DEGREE.name)
+            encoder_offsets[i] = calibrate_homing_motor(id, arm)
+            start_positions[i] = 0
+            end_positions[i] = 0
+
+    print(f"\n calibration of {robot_type} {arm_name} {arm_type} done!")
 
     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": np.zeros(len(arm.motor_indices), dtype=int).tolist(),
+        "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: 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}...")
diff --git a/lerobot/common/robot_devices/robots/manipulator.py b/lerobot/common/robot_devices/robots/manipulator.py
index 618105064..b4b9235f9 100644
--- a/lerobot/common/robot_devices/robots/manipulator.py
+++ b/lerobot/common/robot_devices/robots/manipulator.py
@@ -374,10 +374,10 @@ class ManipulatorRobot:
 
                 elif self.robot_type in ["so100", "moss"]:
                     from lerobot.common.robot_devices.robots.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)
diff --git a/lerobot/scripts/configure_motor.py b/lerobot/scripts/configure_motor.py
index 18707397f..1b51a50bf 100644
--- a/lerobot/scripts/configure_motor.py
+++ b/lerobot/scripts/configure_motor.py
@@ -114,15 +114,20 @@ 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", 0)
+            motor_bus.write("Torque_Enable", 0)
+            motor_bus.write("Lock", 1)
             print("Offset", motor_bus.read("Offset"))
 
+            # TODO(pepijn):
+            # single_calibration = single_motor_calibration(motor_bus, motor_idx_des)
+
+            # TODO(pepijn): store single_calibration
+
     except Exception as e:
         print(f"Error occurred during motor configuration: {e}")
 

From f540eb81ff90bed1476510d6ff0341677966e5af Mon Sep 17 00:00:00 2001
From: Pepijn <pepijn@huggingface.co>
Date: Thu, 30 Jan 2025 13:32:07 +0100
Subject: [PATCH 07/19] fix write issue

---
 .../common/robot_devices/motors/feetech.py    |   8 +-
 .../robots/feetech_calibration.py             |   2 +-
 lerobot/scripts/read_pos.py                   | 174 ------------------
 3 files changed, 7 insertions(+), 177 deletions(-)
 delete mode 100644 lerobot/scripts/read_pos.py

diff --git a/lerobot/common/robot_devices/motors/feetech.py b/lerobot/common/robot_devices/motors/feetech.py
index 219e5ab83..441d1ee08 100644
--- a/lerobot/common/robot_devices/motors/feetech.py
+++ b/lerobot/common/robot_devices/motors/feetech.py
@@ -96,6 +96,8 @@ SCS_SERIES_BAUDRATE_TABLE = {
     7: 19_200,
 }
 
+CALIBRATION_REQUIRED = ["Goal_Position", "Present_Position"]
+
 MODEL_CONTROL_TABLE = {
     "scs_series": SCS_SERIES_CONTROL_TABLE,
     "sts3215": SCS_SERIES_CONTROL_TABLE,
@@ -471,6 +473,8 @@ class FeetechMotorsBus:
                 start_pos = self.calibration["start_pos"][calib_idx]
                 end_pos = self.calibration["end_pos"][calib_idx]
 
+                # TODO(pepijn): Check if a homing (something similar to homing) should be applied to linear joint (think so)
+
                 # Rescale the present position to a nominal range [0, 100] %,
                 # useful for joints with linear motions like Aloha gripper
                 values[i] = (values[i] - start_pos) / (end_pos - start_pos) * 100
@@ -619,7 +623,7 @@ class FeetechMotorsBus:
 
         values = np.array(values)
 
-        if self.calibration is not None:
+        if data_name in CALIBRATION_REQUIRED and self.calibration is not None:
             values = self.apply_calibration(values, motor_names)
 
         # log the number of seconds it took to read the data from the motors
@@ -692,7 +696,7 @@ class FeetechMotorsBus:
             motor_ids.append(motor_idx)
             models.append(model)
 
-        if self.calibration is not None:
+        if data_name in CALIBRATION_REQUIRED and self.calibration is not None:
             values = self.revert_calibration(values, motor_names)
 
         values = values.tolist()
diff --git a/lerobot/common/robot_devices/robots/feetech_calibration.py b/lerobot/common/robot_devices/robots/feetech_calibration.py
index 5816126e9..73e718e85 100644
--- a/lerobot/common/robot_devices/robots/feetech_calibration.py
+++ b/lerobot/common/robot_devices/robots/feetech_calibration.py
@@ -57,7 +57,7 @@ def calibrate_linear_motor(motor_id, motor_bus):
     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}")
 
-    input("Open the {motor_name} fully, then press Enter...")
+    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}")
 
diff --git a/lerobot/scripts/read_pos.py b/lerobot/scripts/read_pos.py
deleted file mode 100644
index d310418a0..000000000
--- a/lerobot/scripts/read_pos.py
+++ /dev/null
@@ -1,174 +0,0 @@
-import time
-
-
-def calibrate_motor(motor_id, motor_bus):
-    """
-    1) Prompts user to move the motor to the "home" position.
-    2) Reads servo ticks.
-    3) Calculates the offset so that 'home_ticks' becomes 0.
-    4) Returns the offset
-    """
-    print(f"\n--- Calibrating Motor {motor_id} ---")
-    input("Move the motor to middle (home) position, then press Enter...")
-    home_ticks = motor_bus.read("Present_Position")[0]
-    print(f"  [Motor {motor_id}] middle position recorded: {home_ticks}")
-
-    # Calculate how many ticks to shift so that 'home_ticks' becomes 0
-    raw_offset = -home_ticks  # negative of home_ticks
-    encoder_offset = raw_offset % 4096  # wrap to [0..4095]
-
-    # Convert to a signed range [-2048..2047]
-    if encoder_offset > 2047:
-        encoder_offset -= 4096
-
-    print(f"Encoder offset: {encoder_offset}")
-
-    return encoder_offset
-
-
-def adjusted__to_homing_ticks(raw_motor_ticks, encoder_offset):
-    shifted = (raw_motor_ticks + encoder_offset) % 4096
-    if shifted > 2047:
-        shifted -= 4096
-    return shifted
-
-
-def adjusted_to_motor_ticks(adjusted_pos, encoder_offset):
-    """
-    Inverse of read_adjusted_position().
-
-    adjusted_pos : int in [-2048 .. +2047]  (the homed, shifted value)
-    encoder_offset : int (the offset computed so that `home` becomes zero)
-
-    Returns the raw servo ticks in [0..4095].
-    """
-    if adjusted_pos < 0:
-        adjusted_pos += 4096
-
-    raw_ticks = (adjusted_pos - encoder_offset) % 4096
-    return raw_ticks
-
-
-def configure_and_calibrate():
-    from lerobot.common.robot_devices.motors.feetech import (
-        SCS_SERIES_BAUDRATE_TABLE as SERIES_BAUDRATE_TABLE,
-    )
-    from lerobot.common.robot_devices.motors.feetech import FeetechMotorsBus as MotorsBusClass
-
-    motor_idx_des = 1  # index you want to assign
-
-    # Setup motor names, indices, and models
-    motor_name = "motor"
-    motor_index_arbitrary = motor_idx_des  # Use the motor ID passed via argument
-    motor_model = "sts3215"  # Use the motor model passed via argument
-
-    # Initialize the MotorBus with the correct port and motor configurations
-    motor_bus = MotorsBusClass(
-        port="/dev/tty.usbmodem585A0078271", motors={motor_name: (motor_index_arbitrary, motor_model)}
-    )
-
-    # Try to connect to the motor bus and handle any connection-specific errors
-    try:
-        motor_bus.connect()
-        print(f"Connected on port {motor_bus.port}")
-    except OSError as e:
-        print(f"Error occurred when connecting to the motor bus: {e}")
-        return
-
-    try:
-        print("Scanning all baudrates and motor indices")
-        all_baudrates = set(SERIES_BAUDRATE_TABLE.values())
-        motor_index = -1  # Set the motor index to an out-of-range value.
-
-        for baudrate in all_baudrates:
-            motor_bus.set_bus_baudrate(baudrate)
-            present_ids = motor_bus.find_motor_indices(list(range(1, 10)))
-            if len(present_ids) > 1:
-                raise ValueError(
-                    "Error: More than one motor ID detected. This script is designed to only handle one motor at a time. Please disconnect all but one motor."
-                )
-
-            if len(present_ids) == 1:
-                if motor_index != -1:
-                    raise ValueError(
-                        "Error: More than one motor ID detected. This script is designed to only handle one motor at a time. Please disconnect all but one motor."
-                    )
-                motor_index = present_ids[0]
-
-        if motor_index == -1:
-            raise ValueError("No motors detected. Please ensure you have one motor connected.")
-
-        print(f"Motor index found at: {motor_index}")
-
-        motor_bus.write_with_motor_ids(motor_bus.motor_models, motor_index, "Lock", 0)
-
-        baudrate_des = 1000000
-
-        if baudrate != baudrate_des:
-            print(f"Setting its baudrate to {baudrate_des}")
-            baudrate_idx = list(SERIES_BAUDRATE_TABLE.values()).index(baudrate_des)
-
-            # The write can fail, so we allow retries
-            motor_bus.write_with_motor_ids(motor_bus.motor_models, motor_index, "Baud_Rate", baudrate_idx)
-            time.sleep(0.5)
-            motor_bus.set_bus_baudrate(baudrate_des)
-            present_baudrate_idx = motor_bus.read_with_motor_ids(
-                motor_bus.motor_models, motor_index, "Baud_Rate", num_retry=2
-            )
-
-            if present_baudrate_idx != baudrate_idx:
-                raise OSError("Failed to write baudrate.")
-
-        print(f"Setting its index to desired index {motor_idx_des}")
-        motor_bus.write_with_motor_ids(motor_bus.motor_models, motor_index, "Lock", 0)
-        motor_bus.write_with_motor_ids(motor_bus.motor_models, motor_index, "ID", motor_idx_des)
-
-        present_idx = motor_bus.read_with_motor_ids(motor_bus.motor_models, motor_idx_des, "ID", num_retry=2)
-        if present_idx != motor_idx_des:
-            raise OSError("Failed to write index.")
-
-        # Set Maximum_Acceleration to 254 to speedup acceleration and deceleration of
-        # 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", 0)
-        motor_bus.write("Torque_Enable", 0)
-
-        motor_bus.write("Lock", 1)
-
-        # Calibration
-        print("Offset", motor_bus.read("Offset"))
-        print("Max_Angle_Limit", motor_bus.read("Max_Angle_Limit"))
-        print("Min_Angle_Limit", motor_bus.read("Min_Angle_Limit"))
-        print("Goal_Position", motor_bus.read("Goal_Position"))
-        print("Present Position", motor_bus.read("Present_Position"))
-
-        encoder_offset = calibrate_motor(motor_idx_des, motor_bus)
-
-        try:
-            while True:
-                raw_motor_ticks = motor_bus.read("Present_Position")[0]
-                adjusted_ticks = adjusted__to_homing_ticks(raw_motor_ticks, encoder_offset)
-                inverted_adjusted_ticks = adjusted_to_motor_ticks(adjusted_ticks, encoder_offset)
-                print(
-                    f"Raw Motor ticks: {raw_motor_ticks} | Adjusted ticks: {adjusted_ticks} | Invert adjusted ticks: {inverted_adjusted_ticks}"
-                )
-                time.sleep(0.3)
-        except KeyboardInterrupt:
-            print("Stopped reading positions.")
-
-    except Exception as e:
-        print(f"Error occurred during motor configuration: {e}")
-
-    finally:
-        motor_bus.disconnect()
-        print("Disconnected from motor bus.")
-
-
-if __name__ == "__main__":
-    configure_and_calibrate()

From 3ef54ab019da702e2d5c266f27b161e24d1d46a7 Mon Sep 17 00:00:00 2001
From: Pepijn <pepijn@huggingface.co>
Date: Thu, 30 Jan 2025 13:58:44 +0100
Subject: [PATCH 08/19] remove cache files

---
 .cache/calibration/so100/main_follower.json                | 1 -
 .cache/calibration/so100/main_leader.json                  | 1 -
 lerobot/common/robot_devices/robots/feetech_calibration.py | 5 ++++-
 3 files changed, 4 insertions(+), 3 deletions(-)
 delete mode 100644 .cache/calibration/so100/main_follower.json
 delete mode 100644 .cache/calibration/so100/main_leader.json

diff --git a/.cache/calibration/so100/main_follower.json b/.cache/calibration/so100/main_follower.json
deleted file mode 100644
index 7279b79bc..000000000
--- a/.cache/calibration/so100/main_follower.json
+++ /dev/null
@@ -1 +0,0 @@
-{"homing_offset": [-1985, 1915, 1984, 2008, 1765, 0], "drive_mode": [0, 0, 0, 0, 0, 0], "start_pos": [0, 0, 0, 0, 0, 1007], "end_pos": [0, 0, 0, 0, 0, 2448], "calib_mode": ["DEGREE", "DEGREE", "DEGREE", "DEGREE", "DEGREE", "LINEAR"], "motor_names": ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll", "gripper"]}
diff --git a/.cache/calibration/so100/main_leader.json b/.cache/calibration/so100/main_leader.json
deleted file mode 100644
index 70516c1c8..000000000
--- a/.cache/calibration/so100/main_leader.json
+++ /dev/null
@@ -1 +0,0 @@
-{"homing_offset": [1433, 4, 1917, -263, 972, 0], "drive_mode": [0, 0, 0, 0, 0, 0], "start_pos": [0, 0, 0, 0, 0, 1534], "end_pos": [0, 0, 0, 0, 0, 2590], "calib_mode": ["DEGREE", "DEGREE", "DEGREE", "DEGREE", "DEGREE", "LINEAR"], "motor_names": ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll", "gripper"]}
diff --git a/lerobot/common/robot_devices/robots/feetech_calibration.py b/lerobot/common/robot_devices/robots/feetech_calibration.py
index 73e718e85..c1734c09d 100644
--- a/lerobot/common/robot_devices/robots/feetech_calibration.py
+++ b/lerobot/common/robot_devices/robots/feetech_calibration.py
@@ -121,7 +121,7 @@ def run_full_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm
     print("\nMove arm to homing position")
     print(
         "See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="zero")
-    )  # TODO: replace with new instruction homing pos (all motors in center)
+    )  # TODO(pepijn): replace with new instruction homing pos (all motors in center)
     input("Press Enter to continue...")
 
     start_positions = np.zeros(len(arm.motor_indices))
@@ -138,6 +138,9 @@ def run_full_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm
             start_positions[i] = 0
             end_positions[i] = 0
 
+    print("\nMove arm to rest position")
+    input("Press Enter to continue...")
+
     print(f"\n calibration of {robot_type} {arm_name} {arm_type} done!")
 
     calib_dict = {

From 1d121e3321a300f6b562f0edd6bcf06a7fd9bd4f Mon Sep 17 00:00:00 2001
From: Pepijn <pepijn@huggingface.co>
Date: Thu, 30 Jan 2025 19:22:21 +0100
Subject: [PATCH 09/19] update configure

---
 .../common/robot_devices/motors/feetech.py    | 27 ++++++++-----------
 .../robots/feetech_calibration.py             | 17 +++++++-----
 2 files changed, 21 insertions(+), 23 deletions(-)

diff --git a/lerobot/common/robot_devices/motors/feetech.py b/lerobot/common/robot_devices/motors/feetech.py
index 441d1ee08..d3024f204 100644
--- a/lerobot/common/robot_devices/motors/feetech.py
+++ b/lerobot/common/robot_devices/motors/feetech.py
@@ -140,33 +140,27 @@ def convert_steps_to_degrees(steps: int | np.ndarray, models: list[str]) -> np.n
     return degrees
 
 
-def adjusted__to_homing_ticks(
-    raw_motor_ticks: int | np.ndarray, encoder_offset: int, models: list[str]
-) -> np.ndarray:
+def adjusted_to_homing_ticks(raw_motor_ticks: int, encoder_offset: int, model: str) -> int:
     """
     raw_motor_ticks : int in [0..4095]  (the homed, shifted value)
     encoder_offset : int (the offset computed so that `home` becomes zero)
     Returns the homed servo ticks in [-2048 .. +2047].
     """
-    resolutions = np.array([MODEL_RESOLUTION[m] for m in models], dtype=float)
-    raw_motor_ticks = np.asarray(raw_motor_ticks)
+    resolutions = MODEL_RESOLUTION[model]
     shifted = (raw_motor_ticks + encoder_offset) % resolutions
-    shifted = np.where(shifted > 2047, shifted - 4096, shifted)
+    shifted = shifted - 4096 if shifted > 2047 else shifted
     return shifted
 
 
-def adjusted_to_motor_ticks(
-    adjusted_pos: int | np.ndarray, encoder_offset: int, models: list[str]
-) -> np.ndarray:
+def adjusted_to_motor_ticks(adjusted_pos: int, encoder_offset: int, model: str) -> int:
     """
-    Inverse of read_adjusted_position().
+    Inverse of adjusted_to_homing_ticks().
     adjusted_pos : int in [-2048 .. +2047]  (the homed, shifted value)
     encoder_offset : int (the offset computed so that `home` becomes zero)
     Returns the raw servo ticks in [0..4095].
     """
-    resolutions = np.array([MODEL_RESOLUTION[m] for m in models], dtype=float)
-    adjusted_pos = np.asarray(adjusted_pos)
-    adjusted_pos = np.where(adjusted_pos < 0, adjusted_pos + 4096, adjusted_pos)
+    resolutions = MODEL_RESOLUTION[model]
+    adjusted_pos = adjusted_pos + 4096 if adjusted_pos < 0 else adjusted_pos
     raw_ticks = (adjusted_pos - encoder_offset) % resolutions
     return raw_ticks
 
@@ -335,7 +329,8 @@ class FeetechMotorsBus:
         self.group_writers = {}
         self.logs = {}
 
-        self.track_positions = {}
+        # TODO(pepijn): Add multi turn support
+        self.multi_turn_index = self.multi_turn_index = [0] * len(motors)
 
     def connect(self):
         if self.is_connected:
@@ -450,7 +445,7 @@ class FeetechMotorsBus:
                 _, model = self.motors[name]
 
                 # Convert raw motor ticks to homed ticks, then convert the homed ticks to degrees
-                values[i] = adjusted__to_homing_ticks(values[i], homing_offset, [model])
+                values[i] = adjusted_to_homing_ticks(values[i], homing_offset, model)
                 values[i] = convert_steps_to_degrees(values[i], [model])
 
                 # Update direction of rotation of the motor to match between leader and follower.
@@ -507,7 +502,7 @@ class FeetechMotorsBus:
 
                 # Convert degrees to homed ticks, then convert the homed ticks to raw ticks
                 values[i] = convert_degrees_to_steps(values[i], [model])
-                values[i] = adjusted_to_motor_ticks(values[i], homing_offset, [model])
+                values[i] = adjusted_to_motor_ticks(values[i], homing_offset, model)
 
                 # Remove drive mode, which is the rotation direction of the motor, to come back to
                 # actual motor rotation direction which can be arbitrary.
diff --git a/lerobot/common/robot_devices/robots/feetech_calibration.py b/lerobot/common/robot_devices/robots/feetech_calibration.py
index c1734c09d..56d1ed1ca 100644
--- a/lerobot/common/robot_devices/robots/feetech_calibration.py
+++ b/lerobot/common/robot_devices/robots/feetech_calibration.py
@@ -128,16 +128,19 @@ def run_full_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm
     end_positions = np.zeros(len(arm.motor_indices))
     encoder_offsets = np.zeros(len(arm.motor_indices))
 
-    # Call single motor calibration for each motor id
-    for i, id in enumerate(arm.motor_indices):
-        if id == 6:
-            start_positions[i], end_positions[i] = calibrate_linear_motor(id, arm)
-            encoder_offsets[i] = 0
-        else:
-            encoder_offsets[i] = calibrate_homing_motor(id, arm)
+    modes = get_calibration_modes(arm)
+
+    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
 
+    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")
     input("Press Enter to continue...")
 

From b7a343e79737bf034c077d3c405cafe4be5bc14c Mon Sep 17 00:00:00 2001
From: Pepijn <pepijn@huggingface.co>
Date: Thu, 30 Jan 2025 19:59:26 +0100
Subject: [PATCH 10/19] add multi turn support

---
 .../common/robot_devices/motors/feetech.py    | 70 ++++++++++++++-----
 1 file changed, 54 insertions(+), 16 deletions(-)

diff --git a/lerobot/common/robot_devices/motors/feetech.py b/lerobot/common/robot_devices/motors/feetech.py
index d3024f204..c85ab8dec 100644
--- a/lerobot/common/robot_devices/motors/feetech.py
+++ b/lerobot/common/robot_devices/motors/feetech.py
@@ -140,27 +140,65 @@ def convert_steps_to_degrees(steps: int | np.ndarray, models: list[str]) -> np.n
     return degrees
 
 
-def adjusted_to_homing_ticks(raw_motor_ticks: int, encoder_offset: int, model: str) -> int:
+def adjusted_to_homing_ticks(
+    raw_motor_ticks: int, encoder_offset: int, model: str, motorbus, motor_id: int
+) -> int:
     """
-    raw_motor_ticks : int in [0..4095]  (the homed, shifted value)
-    encoder_offset : int (the offset computed so that `home` becomes zero)
-    Returns the homed servo ticks in [-2048 .. +2047].
+    Converts raw motor ticks [0..4095] to homed servo ticks in [-2048..2047].
+    Tracks multi-turn rotations by adjusting a multi-turn index.
     """
+
     resolutions = MODEL_RESOLUTION[model]
+    # Retrieve "previous shifted" and the multi-turn index
+    prev_value = motorbus.previous_value[motor_id - 1]
+    multi_turn_index = motorbus.multi_turn_index[motor_id - 1]
+
+    # Compute the new shifted value in the range [-2048..2047]
     shifted = (raw_motor_ticks + encoder_offset) % resolutions
-    shifted = shifted - 4096 if shifted > 2047 else shifted
-    return shifted
+    if shifted > 2047:
+        shifted -= 4096
+
+    # If we have a valid previous value, detect big jumps
+    if prev_value is not None:
+        delta = shifted - prev_value
+
+        # If we jump forward by more than half a turn, we must have wrapped negatively
+        if delta > 2048:
+            multi_turn_index -= 1
+
+        # If we jump backward by more than half a turn, we must have wrapped positively
+        elif delta < -2048:
+            multi_turn_index += 1
+
+    # Store the new shifted value and updated multi-turn index
+    motorbus.previous_value[motor_id - 1] = shifted
+    motorbus.multi_turn_index[motor_id - 1] = multi_turn_index
+
+    # Return the final multi-turn adjusted position
+    return shifted + multi_turn_index * 4096
 
 
-def adjusted_to_motor_ticks(adjusted_pos: int, encoder_offset: int, model: str) -> int:
+def adjusted_to_motor_ticks(
+    adjusted_pos: int, encoder_offset: int, model: str, motorbus, motor_id: int
+) -> int:
     """
     Inverse of adjusted_to_homing_ticks().
-    adjusted_pos : int in [-2048 .. +2047]  (the homed, shifted value)
-    encoder_offset : int (the offset computed so that `home` becomes zero)
-    Returns the raw servo ticks in [0..4095].
+    Converts homed servo ticks (with multi-turn indexing) back to [0..4095].
     """
+
     resolutions = MODEL_RESOLUTION[model]
-    adjusted_pos = adjusted_pos + 4096 if adjusted_pos < 0 else adjusted_pos
+
+    # Get the current multi-turn index and remove that offset
+    multi_turn_index = motorbus.multi_turn_index[motor_id - 1]
+    adjusted_pos -= multi_turn_index * 4096
+
+    #  Convert back into [−2048..2047] before final modulo
+    if adjusted_pos > 2047:
+        adjusted_pos -= 4096
+    elif adjusted_pos < -2048:
+        adjusted_pos += 4096
+
+    # Map back to raw ticks [0..4095]
     raw_ticks = (adjusted_pos - encoder_offset) % resolutions
     return raw_ticks
 
@@ -329,8 +367,8 @@ class FeetechMotorsBus:
         self.group_writers = {}
         self.logs = {}
 
-        # TODO(pepijn): Add multi turn support
         self.multi_turn_index = self.multi_turn_index = [0] * len(motors)
+        self.previous_value = self.previous_value = [0] * len(motors)
 
     def connect(self):
         if self.is_connected:
@@ -442,10 +480,10 @@ class FeetechMotorsBus:
             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]
+                motor_idx, model = self.motors[name]
 
                 # Convert raw motor ticks to homed ticks, then convert the homed ticks to degrees
-                values[i] = adjusted_to_homing_ticks(values[i], homing_offset, model)
+                values[i] = adjusted_to_homing_ticks(values[i], homing_offset, model, self, motor_idx)
                 values[i] = convert_steps_to_degrees(values[i], [model])
 
                 # Update direction of rotation of the motor to match between leader and follower.
@@ -498,11 +536,11 @@ class FeetechMotorsBus:
             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]
+                motor_idx, model = self.motors[name]
 
                 # Convert degrees to homed ticks, then convert the homed ticks to raw ticks
                 values[i] = convert_degrees_to_steps(values[i], [model])
-                values[i] = adjusted_to_motor_ticks(values[i], homing_offset, model)
+                values[i] = adjusted_to_motor_ticks(values[i], homing_offset, model, self, motor_idx)
 
                 # Remove drive mode, which is the rotation direction of the motor, to come back to
                 # actual motor rotation direction which can be arbitrary.

From 514ce5d0b19b5c2aa1e06c8b2c72ff51e4b47fb0 Mon Sep 17 00:00:00 2001
From: Pepijn <pepijn@huggingface.co>
Date: Fri, 31 Jan 2025 09:26:42 +0100
Subject: [PATCH 11/19] improve test method

---
 lerobot/scripts/test_homing.py | 112 +++++++++++++++++++++++++++++++++
 1 file changed, 112 insertions(+)
 create mode 100644 lerobot/scripts/test_homing.py

diff --git a/lerobot/scripts/test_homing.py b/lerobot/scripts/test_homing.py
new file mode 100644
index 000000000..b9839b4ac
--- /dev/null
+++ b/lerobot/scripts/test_homing.py
@@ -0,0 +1,112 @@
+from lerobot.common.robot_devices.motors.feetech import (
+    adjusted_to_homing_ticks,
+    adjusted_to_motor_ticks,
+    convert_steps_to_degrees,
+)
+
+# TODO(pepijn): remove this file!
+
+MODEL_RESOLUTION = {
+    "scs_series": 4096,
+    "sts3215": 4096,
+}
+
+
+def calibrate_homing_motor(motor_id, motor_bus):
+    """
+    1) Reads servo ticks.
+    2) Calculates the offset so that 'home_ticks' becomes 0.
+    3) Returns the offset
+    """
+
+    home_ticks = motor_bus.read("Present_Position")[0]  # Read index starts at 0
+
+    # Calculate how many ticks to shift so that 'home_ticks' becomes 0
+    raw_offset = -home_ticks  # negative of home_ticks
+    encoder_offset = raw_offset % 4096  # wrap to [0..4095]
+
+    # Convert to a signed range [-2048..2047]
+    if encoder_offset > 2047:
+        encoder_offset -= 4096
+
+    print(f"Encoder offset: {encoder_offset}")
+
+    return encoder_offset
+
+
+def configure_and_calibrate():
+    from lerobot.common.robot_devices.motors.feetech import FeetechMotorsBus as MotorsBusClass
+
+    motor_idx_des = 2  # index of motor
+
+    # Setup motor names, indices, and models
+    motor_name = "motor"
+    motor_index_arbitrary = motor_idx_des  # Use the motor ID passed via argument
+    motor_model = "sts3215"  # Use the motor model passed via argument
+
+    # Initialize the MotorBus with the correct port and motor configurations
+    motor_bus = MotorsBusClass(
+        port="/dev/tty.usbmodem58760431631", motors={motor_name: (motor_index_arbitrary, motor_model)}
+    )
+
+    # Try to connect to the motor bus and handle any connection-specific errors
+    try:
+        motor_bus.connect()
+        print(f"Connected on port {motor_bus.port}")
+    except OSError as e:
+        print(f"Error occurred when connecting to the motor bus: {e}")
+        return
+
+    present_idx = motor_bus.read_with_motor_ids(motor_bus.motor_models, motor_idx_des, "ID", num_retry=2)
+    if present_idx != motor_idx_des:
+        raise OSError("Failed to write index.")
+
+    # Set Maximum_Acceleration to 254 to speedup acceleration and deceleration of
+    # 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", 0)
+    motor_bus.write("Torque_Enable", 0)
+
+    motor_bus.write("Lock", 1)
+
+    # Calibration
+    print("Offset", motor_bus.read("Offset"))
+    print("Max_Angle_Limit", motor_bus.read("Max_Angle_Limit"))
+    print("Min_Angle_Limit", motor_bus.read("Min_Angle_Limit"))
+    print("Goal_Position", motor_bus.read("Goal_Position"))
+    print("Present Position", motor_bus.read("Present_Position"))
+
+    input("Move the motor to middle (home) position, then press Enter...")
+    encoder_offset = calibrate_homing_motor(motor_idx_des, motor_bus)
+
+    try:
+        while True:
+            name = motor_bus.motor_names[0]
+            _, model = motor_bus.motors[name]
+            raw_motor_ticks = motor_bus.read("Present_Position")[0]
+            adjusted_ticks = adjusted_to_homing_ticks(raw_motor_ticks, encoder_offset, model, motor_bus, 1)
+            inverted_ticks = adjusted_to_motor_ticks(adjusted_ticks, encoder_offset, model, motor_bus, 1)
+            adjusted_degrees = convert_steps_to_degrees([adjusted_ticks], [model])
+            print(
+                f"Raw Motor ticks: {raw_motor_ticks} | Adjusted ticks: {adjusted_ticks} | Adjusted degrees: {adjusted_degrees} | Invert adjusted ticks: {inverted_ticks}"
+            )
+            # time.sleep(0.3)
+    except KeyboardInterrupt:
+        print("Stopped reading positions.")
+
+    except Exception as e:
+        print(f"Error occurred during motor configuration: {e}")
+
+    finally:
+        motor_bus.disconnect()
+        print("Disconnected from motor bus.")
+
+
+if __name__ == "__main__":
+    configure_and_calibrate()

From 63e6f7901e35108ea6b098347314cc6d216a1b8b Mon Sep 17 00:00:00 2001
From: Pepijn <pepijn@huggingface.co>
Date: Tue, 18 Feb 2025 15:04:25 +0100
Subject: [PATCH 12/19] Added calibration visualization

---
 .../common/robot_devices/motors/feetech.py    |  84 ++++++------
 .../robots/feetech_calibration.py             |   4 +-
 lerobot/scripts/calibration_visualization.py  | 121 ++++++++++++++++++
 lerobot/scripts/configure_motor.py            |   5 +-
 lerobot/scripts/test_homing.py                | 112 ----------------
 5 files changed, 163 insertions(+), 163 deletions(-)
 create mode 100644 lerobot/scripts/calibration_visualization.py
 delete mode 100644 lerobot/scripts/test_homing.py

diff --git a/lerobot/common/robot_devices/motors/feetech.py b/lerobot/common/robot_devices/motors/feetech.py
index 7e23a3a14..d87c1653c 100644
--- a/lerobot/common/robot_devices/motors/feetech.py
+++ b/lerobot/common/robot_devices/motors/feetech.py
@@ -16,13 +16,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 = -180
-UPPER_BOUND_DEGREE = 180
 # 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
@@ -32,7 +25,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)
@@ -119,25 +111,39 @@ NUM_READ_RETRY = 20
 NUM_WRITE_RETRY = 20
 
 
-def convert_degrees_to_steps(degrees: float | np.ndarray, models: 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_degrees_to_steps(
+    degrees: float | np.ndarray, models: list[str], multi_turn_index: int
+) -> np.ndarray:
     """
+    Converts degrees to motor steps with multi-turn tracking.
+    - Each full rotation (360°) corresponds to an additional 4096 steps.
+    """
+
     resolutions = np.array([MODEL_RESOLUTION[m] for m in models], dtype=float)
-    steps = degrees / 180 * np.array(resolutions) / 2
-    steps = steps.astype(int)
-    return steps
+
+    # Remove full rotations from degrees
+    base_degrees = degrees - (multi_turn_index * 360.0)
+
+    # Convert degrees to motor steps
+    steps = base_degrees / 180.0 * (resolutions / 2)
+
+    # Add back multi-turn steps
+    steps += multi_turn_index * resolutions
+
+    return steps.astype(int)
 
 
 def convert_steps_to_degrees(steps: int | np.ndarray, models: list[str]) -> np.ndarray:
-    """This function converts the step range to the degrees 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.
+    """
+    Converts motor steps to degrees while accounting for multi-turn motion.
+    - Each full rotation (4096 steps) adds ±360°.
     """
     resolutions = np.array([MODEL_RESOLUTION[m] for m in models], dtype=float)
     steps = np.array(steps, dtype=float)
+
+    # Convert steps to degrees
     degrees = steps * (360.0 / resolutions)
+
     return degrees
 
 
@@ -145,38 +151,36 @@ def adjusted_to_homing_ticks(
     raw_motor_ticks: int, encoder_offset: int, model: str, motorbus, motor_id: int
 ) -> int:
     """
-    Converts raw motor ticks [0..4095] to homed servo ticks in [-2048..2047].
-    Tracks multi-turn rotations by adjusting a multi-turn index.
+    Converts raw motor ticks [0..4095] to homed servo ticks with multi-turn indexing.
+    - Uses a rolling index to track full rotations (4096 steps each)
     """
 
     resolutions = MODEL_RESOLUTION[model]
-    # Retrieve "previous shifted" and the multi-turn index
+
+    # Retrieve previous values for tracking
     prev_value = motorbus.previous_value[motor_id - 1]
     multi_turn_index = motorbus.multi_turn_index[motor_id - 1]
 
-    # Compute the new shifted value in the range [-2048..2047]
+    # Compute new shifted position
     shifted = (raw_motor_ticks + encoder_offset) % resolutions
-    if shifted > 2047:
-        shifted -= 4096
+    if shifted > resolutions // 2:
+        shifted -= resolutions  # Normalize to [-2048, 2047]
 
-    # If we have a valid previous value, detect big jumps
     if prev_value is not None:
         delta = shifted - prev_value
 
-        # If we jump forward by more than half a turn, we must have wrapped negatively
-        if delta > 2048:
+        # If jump forward > 180° (2048 steps), assume full rotation
+        if delta > resolutions // 2:
             multi_turn_index -= 1
-
-        # If we jump backward by more than half a turn, we must have wrapped positively
-        elif delta < -2048:
+        elif delta < -resolutions // 2:
             multi_turn_index += 1
 
-    # Store the new shifted value and updated multi-turn index
+    # Update stored values
     motorbus.previous_value[motor_id - 1] = shifted
     motorbus.multi_turn_index[motor_id - 1] = multi_turn_index
 
-    # Return the final multi-turn adjusted position
-    return shifted + multi_turn_index * 4096
+    # Return final adjusted ticks with multi-turn indexing
+    return shifted + (multi_turn_index * resolutions)
 
 
 def adjusted_to_motor_ticks(
@@ -485,22 +489,10 @@ class FeetechMotorsBus:
                 if drive_mode:
                     values[i] *= -1
 
-                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]
                 end_pos = self.calibration["end_pos"][calib_idx]
 
-                # TODO(pepijn): Check if a homing (something similar to homing) should be applied to linear joint (think so)
-
                 # Rescale the present position to a nominal range [0, 100] %,
                 # useful for joints with linear motions like Aloha gripper
                 values[i] = (values[i] - start_pos) / (end_pos - start_pos) * 100
@@ -532,7 +524,7 @@ class FeetechMotorsBus:
                 motor_idx, model = self.motors[name]
 
                 # Convert degrees to homed ticks, then convert the homed ticks to raw ticks
-                values[i] = convert_degrees_to_steps(values[i], [model])
+                values[i] = convert_degrees_to_steps(values[i], [model], self.multi_turn_index[motor_idx - 1])
                 values[i] = adjusted_to_motor_ticks(values[i], homing_offset, model, self, motor_idx)
 
                 # Remove drive mode, which is the rotation direction of the motor, to come back to
diff --git a/lerobot/common/robot_devices/robots/feetech_calibration.py b/lerobot/common/robot_devices/robots/feetech_calibration.py
index 56d1ed1ca..45e0e3fd1 100644
--- a/lerobot/common/robot_devices/robots/feetech_calibration.py
+++ b/lerobot/common/robot_devices/robots/feetech_calibration.py
@@ -121,7 +121,7 @@ def run_full_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm
     print("\nMove arm to homing position")
     print(
         "See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="zero")
-    )  # TODO(pepijn): replace with new instruction homing pos (all motors in center)
+    )  # TODO(pepijn): replace with new instruction homing pos (all motors in center) in tutorial
     input("Press Enter to continue...")
 
     start_positions = np.zeros(len(arm.motor_indices))
@@ -158,7 +158,7 @@ def run_full_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm
 
 
 def run_full_auto_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
-    """TODO: Add this method later as extra
+    """TODO(pepijn): Add this method later as extra
     Example of usage:
     ```python
     run_full_auto_arm_calibration(arm, "so100", "left", "follower")
diff --git a/lerobot/scripts/calibration_visualization.py b/lerobot/scripts/calibration_visualization.py
new file mode 100644
index 000000000..6f8a81db4
--- /dev/null
+++ b/lerobot/scripts/calibration_visualization.py
@@ -0,0 +1,121 @@
+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_steps_to_degrees,
+)
+
+# Replace this import with your real config class import
+# (e.g., So100RobotConfig or any other)
+from lerobot.common.robot_devices.robots.configs import So100RobotConfig
+from lerobot.common.robot_devices.robots.utils import make_robot_from_config
+
+
+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 cfg: A config object (e.g. So100RobotConfig).
+    :param arm_arg: One of "main_leader" or "main_follower".
+    """
+    # 1) Make the Robot from your config
+    #    e.g. `So100RobotConfig` might already be the "robot object",
+    #    or if it is purely a config structure, do: robot = make_robot_from_config(cfg).
+    robot = make_robot_from_config(cfg)
+
+    # 2) 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"]
+
+    # 3) Create the Feetech bus from that config
+    bus = FeetechMotorsBus(bus_config)
+
+    # 4) (Optional) 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.")
+
+    # 5) Connect to the bus
+    bus.connect()
+    print(f"Connected to Feetech bus on port: {bus.port}")
+
+    # 6) 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:
+            # (a) Read *raw* positions (no calibration)
+            raw_positions = bus.read_with_motor_ids(
+                bus.motor_models, bus.motor_indices, data_name="Present_Position"
+            )
+
+            # (b) Read *already-homed* positions (calibration is applied automatically)
+            homed_positions = bus.read("Present_Position")
+
+            # Print them side by side
+            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
+
+                # If you want to see how offset is used inside bus.read(), do it manually:
+                offset = 0
+                if bus.calibration and name in bus.calibration["motor_names"]:
+                    offset_idx = bus.calibration["motor_names"].index(name)
+                    offset = bus.calibration["homing_offset"][offset_idx]
+
+                # Manually compute "adjusted ticks" from raw ticks
+                manual_adjusted = adjusted_to_homing_ticks(raw_ticks, offset, model, bus, motor_idx)
+                # Convert to degrees
+                manual_degs = convert_steps_to_degrees(manual_adjusted, [model])[0]
+                # Invert
+                inv_ticks = adjusted_to_motor_ticks(manual_adjusted, offset, model, bus, motor_idx)
+
+                print(
+                    f"{name:15s} | "
+                    f"RAW={raw_ticks:4d} | "
+                    f"HOMED={homed_val:7.2f} | "
+                    f"MANUAL_ADJ={manual_adjusted:6d} | "
+                    f"DEG={manual_degs:7.2f} | "
+                    f"INV={inv_ticks:4d}"
+                )
+            print("----------------------------------------------------")
+            time.sleep(0.25)  # slow down loop
+    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()
+
+    # 1) Instantiate the config you want (So100RobotConfig, or whichever your project uses).
+    cfg = So100RobotConfig()
+
+    # 2) Call the function with (cfg, args.arm)
+    debug_feetech_positions(cfg, arm_arg=args.arm)
diff --git a/lerobot/scripts/configure_motor.py b/lerobot/scripts/configure_motor.py
index dc0d68261..f2e4090b6 100644
--- a/lerobot/scripts/configure_motor.py
+++ b/lerobot/scripts/configure_motor.py
@@ -141,10 +141,9 @@ def configure_motor(port, brand, model, motor_idx_des, baudrate_des):
             motor_bus.write("Lock", 1)
             print("Offset", motor_bus.read("Offset"))
 
-            # TODO(pepijn):
+            # TODO(pepijn): Add when doing a motor configuration, you instantainesly home the position (while assembling)
             # single_calibration = single_motor_calibration(motor_bus, motor_idx_des)
-
-            # TODO(pepijn): store single_calibration
+            # Then store single_calibration in yaml via dict (not overwrite but add the single calibration each time for motor id)
 
     except Exception as e:
         print(f"Error occurred during motor configuration: {e}")
diff --git a/lerobot/scripts/test_homing.py b/lerobot/scripts/test_homing.py
deleted file mode 100644
index b9839b4ac..000000000
--- a/lerobot/scripts/test_homing.py
+++ /dev/null
@@ -1,112 +0,0 @@
-from lerobot.common.robot_devices.motors.feetech import (
-    adjusted_to_homing_ticks,
-    adjusted_to_motor_ticks,
-    convert_steps_to_degrees,
-)
-
-# TODO(pepijn): remove this file!
-
-MODEL_RESOLUTION = {
-    "scs_series": 4096,
-    "sts3215": 4096,
-}
-
-
-def calibrate_homing_motor(motor_id, motor_bus):
-    """
-    1) Reads servo ticks.
-    2) Calculates the offset so that 'home_ticks' becomes 0.
-    3) Returns the offset
-    """
-
-    home_ticks = motor_bus.read("Present_Position")[0]  # Read index starts at 0
-
-    # Calculate how many ticks to shift so that 'home_ticks' becomes 0
-    raw_offset = -home_ticks  # negative of home_ticks
-    encoder_offset = raw_offset % 4096  # wrap to [0..4095]
-
-    # Convert to a signed range [-2048..2047]
-    if encoder_offset > 2047:
-        encoder_offset -= 4096
-
-    print(f"Encoder offset: {encoder_offset}")
-
-    return encoder_offset
-
-
-def configure_and_calibrate():
-    from lerobot.common.robot_devices.motors.feetech import FeetechMotorsBus as MotorsBusClass
-
-    motor_idx_des = 2  # index of motor
-
-    # Setup motor names, indices, and models
-    motor_name = "motor"
-    motor_index_arbitrary = motor_idx_des  # Use the motor ID passed via argument
-    motor_model = "sts3215"  # Use the motor model passed via argument
-
-    # Initialize the MotorBus with the correct port and motor configurations
-    motor_bus = MotorsBusClass(
-        port="/dev/tty.usbmodem58760431631", motors={motor_name: (motor_index_arbitrary, motor_model)}
-    )
-
-    # Try to connect to the motor bus and handle any connection-specific errors
-    try:
-        motor_bus.connect()
-        print(f"Connected on port {motor_bus.port}")
-    except OSError as e:
-        print(f"Error occurred when connecting to the motor bus: {e}")
-        return
-
-    present_idx = motor_bus.read_with_motor_ids(motor_bus.motor_models, motor_idx_des, "ID", num_retry=2)
-    if present_idx != motor_idx_des:
-        raise OSError("Failed to write index.")
-
-    # Set Maximum_Acceleration to 254 to speedup acceleration and deceleration of
-    # 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", 0)
-    motor_bus.write("Torque_Enable", 0)
-
-    motor_bus.write("Lock", 1)
-
-    # Calibration
-    print("Offset", motor_bus.read("Offset"))
-    print("Max_Angle_Limit", motor_bus.read("Max_Angle_Limit"))
-    print("Min_Angle_Limit", motor_bus.read("Min_Angle_Limit"))
-    print("Goal_Position", motor_bus.read("Goal_Position"))
-    print("Present Position", motor_bus.read("Present_Position"))
-
-    input("Move the motor to middle (home) position, then press Enter...")
-    encoder_offset = calibrate_homing_motor(motor_idx_des, motor_bus)
-
-    try:
-        while True:
-            name = motor_bus.motor_names[0]
-            _, model = motor_bus.motors[name]
-            raw_motor_ticks = motor_bus.read("Present_Position")[0]
-            adjusted_ticks = adjusted_to_homing_ticks(raw_motor_ticks, encoder_offset, model, motor_bus, 1)
-            inverted_ticks = adjusted_to_motor_ticks(adjusted_ticks, encoder_offset, model, motor_bus, 1)
-            adjusted_degrees = convert_steps_to_degrees([adjusted_ticks], [model])
-            print(
-                f"Raw Motor ticks: {raw_motor_ticks} | Adjusted ticks: {adjusted_ticks} | Adjusted degrees: {adjusted_degrees} | Invert adjusted ticks: {inverted_ticks}"
-            )
-            # time.sleep(0.3)
-    except KeyboardInterrupt:
-        print("Stopped reading positions.")
-
-    except Exception as e:
-        print(f"Error occurred during motor configuration: {e}")
-
-    finally:
-        motor_bus.disconnect()
-        print("Disconnected from motor bus.")
-
-
-if __name__ == "__main__":
-    configure_and_calibrate()

From e8f90ef3f548f202c226d80895a397c7e06d3870 Mon Sep 17 00:00:00 2001
From: Pepijn <pepijn@huggingface.co>
Date: Tue, 18 Feb 2025 18:28:00 +0100
Subject: [PATCH 13/19] Fix with virtual zero in same position as old
 calibration virtual zero

---
 .../common/robot_devices/motors/feetech.py    | 59 +++++++------------
 lerobot/scripts/calibration_visualization.py  | 16 +++--
 2 files changed, 30 insertions(+), 45 deletions(-)

diff --git a/lerobot/common/robot_devices/motors/feetech.py b/lerobot/common/robot_devices/motors/feetech.py
index d87c1653c..e22727931 100644
--- a/lerobot/common/robot_devices/motors/feetech.py
+++ b/lerobot/common/robot_devices/motors/feetech.py
@@ -111,60 +111,44 @@ NUM_READ_RETRY = 20
 NUM_WRITE_RETRY = 20
 
 
-def convert_degrees_to_steps(
-    degrees: float | np.ndarray, models: list[str], multi_turn_index: int
-) -> np.ndarray:
-    """
-    Converts degrees to motor steps with multi-turn tracking.
-    - Each full rotation (360°) corresponds to an additional 4096 steps.
-    """
+def convert_ticks_to_degrees(ticks, model):
+    resolutions = MODEL_RESOLUTION[model]
+    degrees = (ticks / resolutions) * 360.0  # Convert to 0-360 range
 
-    resolutions = np.array([MODEL_RESOLUTION[m] for m in models], dtype=float)
+    # Convert to range [-180, 180]
+    degrees = (degrees + 180) % 360 - 180
+    return degrees
 
+
+def convert_degrees_to_ticks(degrees, model, motorbus, motor_id: int):
+    multi_turn_index = motorbus.multi_turn_index[motor_id - 1]
+    resolutions = MODEL_RESOLUTION[model]
     # Remove full rotations from degrees
     base_degrees = degrees - (multi_turn_index * 360.0)
 
-    # Convert degrees to motor steps
-    steps = base_degrees / 180.0 * (resolutions / 2)
+    # Convert degrees to motor ticks
+    ticks = base_degrees / 180.0 * (resolutions / 2)
 
-    # Add back multi-turn steps
-    steps += multi_turn_index * resolutions
+    # Add back multi-turn ticks
+    ticks += multi_turn_index * resolutions
 
-    return steps.astype(int)
-
-
-def convert_steps_to_degrees(steps: int | np.ndarray, models: list[str]) -> np.ndarray:
-    """
-    Converts motor steps to degrees while accounting for multi-turn motion.
-    - Each full rotation (4096 steps) adds ±360°.
-    """
-    resolutions = np.array([MODEL_RESOLUTION[m] for m in models], dtype=float)
-    steps = np.array(steps, dtype=float)
-
-    # Convert steps to degrees
-    degrees = steps * (360.0 / resolutions)
-
-    return degrees
+    return int(ticks)
 
 
 def adjusted_to_homing_ticks(
     raw_motor_ticks: int, encoder_offset: int, model: str, motorbus, motor_id: int
 ) -> int:
     """
-    Converts raw motor ticks [0..4095] to homed servo ticks with multi-turn indexing.
-    - Uses a rolling index to track full rotations (4096 steps each)
+    Shifts raw [0..4095] ticks by an encoder offset, modulo a single turn [0..4095].
     """
-
-    resolutions = MODEL_RESOLUTION[model]
-
     # Retrieve previous values for tracking
     prev_value = motorbus.previous_value[motor_id - 1]
     multi_turn_index = motorbus.multi_turn_index[motor_id - 1]
 
-    # Compute new shifted position
+    resolutions = MODEL_RESOLUTION[model]
     shifted = (raw_motor_ticks + encoder_offset) % resolutions
     if shifted > resolutions // 2:
-        shifted -= resolutions  # Normalize to [-2048, 2047]
+        shifted -= resolutions
 
     if prev_value is not None:
         delta = shifted - prev_value
@@ -179,7 +163,6 @@ def adjusted_to_homing_ticks(
     motorbus.previous_value[motor_id - 1] = shifted
     motorbus.multi_turn_index[motor_id - 1] = multi_turn_index
 
-    # Return final adjusted ticks with multi-turn indexing
     return shifted + (multi_turn_index * resolutions)
 
 
@@ -190,9 +173,7 @@ def adjusted_to_motor_ticks(
     Inverse of adjusted_to_homing_ticks().
     Converts homed servo ticks (with multi-turn indexing) back to [0..4095].
     """
-
     resolutions = MODEL_RESOLUTION[model]
-
     # Get the current multi-turn index and remove that offset
     multi_turn_index = motorbus.multi_turn_index[motor_id - 1]
     adjusted_pos -= multi_turn_index * 4096
@@ -481,7 +462,7 @@ class FeetechMotorsBus:
 
                 # Convert raw motor ticks to homed ticks, then convert the homed ticks to degrees
                 values[i] = adjusted_to_homing_ticks(values[i], homing_offset, model, self, motor_idx)
-                values[i] = convert_steps_to_degrees(values[i], [model])
+                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
@@ -524,7 +505,7 @@ class FeetechMotorsBus:
                 motor_idx, model = self.motors[name]
 
                 # Convert degrees to homed ticks, then convert the homed ticks to raw ticks
-                values[i] = convert_degrees_to_steps(values[i], [model], self.multi_turn_index[motor_idx - 1])
+                values[i] = convert_degrees_to_ticks(values[i], model, self, motor_idx)
                 values[i] = adjusted_to_motor_ticks(values[i], homing_offset, model, self, motor_idx)
 
                 # Remove drive mode, which is the rotation direction of the motor, to come back to
diff --git a/lerobot/scripts/calibration_visualization.py b/lerobot/scripts/calibration_visualization.py
index 6f8a81db4..4ada769c3 100644
--- a/lerobot/scripts/calibration_visualization.py
+++ b/lerobot/scripts/calibration_visualization.py
@@ -7,7 +7,8 @@ from lerobot.common.robot_devices.motors.feetech import (
     FeetechMotorsBus,
     adjusted_to_homing_ticks,
     adjusted_to_motor_ticks,
-    convert_steps_to_degrees,
+    convert_degrees_to_ticks,
+    convert_ticks_to_degrees,
 )
 
 # Replace this import with your real config class import
@@ -82,17 +83,20 @@ def debug_feetech_positions(cfg, arm_arg: str):
                 # Manually compute "adjusted ticks" from raw ticks
                 manual_adjusted = adjusted_to_homing_ticks(raw_ticks, offset, model, bus, motor_idx)
                 # Convert to degrees
-                manual_degs = convert_steps_to_degrees(manual_adjusted, [model])[0]
+                manual_degs = convert_ticks_to_degrees(manual_adjusted, model)
+                # Convert to ticks
+                manual_ticks = convert_degrees_to_ticks(manual_degs, model, bus, motor_idx)
                 # Invert
-                inv_ticks = adjusted_to_motor_ticks(manual_adjusted, offset, model, bus, motor_idx)
+                inv_ticks = adjusted_to_motor_ticks(manual_ticks, offset, model, bus, motor_idx)
 
                 print(
                     f"{name:15s} | "
                     f"RAW={raw_ticks:4d} | "
                     f"HOMED={homed_val:7.2f} | "
-                    f"MANUAL_ADJ={manual_adjusted:6d} | "
-                    f"DEG={manual_degs:7.2f} | "
-                    f"INV={inv_ticks:4d}"
+                    f"MANUAL_ADJ_TICKS={manual_adjusted:6d} | "
+                    f"MANUAL_ADJ_DEG={manual_degs:7.2f} | "
+                    f"INV_TICKS={manual_ticks:6d} | "
+                    f"INV_STEPS={inv_ticks:4d}"
                 )
             print("----------------------------------------------------")
             time.sleep(0.25)  # slow down loop

From 8088110aacaf7bad1aa4ef06a62e650c1b66e609 Mon Sep 17 00:00:00 2001
From: Pepijn <pepijn@huggingface.co>
Date: Tue, 18 Feb 2025 19:49:59 +0100
Subject: [PATCH 14/19] add todo

---
 lerobot/common/robot_devices/robots/feetech_calibration.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/lerobot/common/robot_devices/robots/feetech_calibration.py b/lerobot/common/robot_devices/robots/feetech_calibration.py
index 45e0e3fd1..1c50b1e64 100644
--- a/lerobot/common/robot_devices/robots/feetech_calibration.py
+++ b/lerobot/common/robot_devices/robots/feetech_calibration.py
@@ -51,7 +51,7 @@ def calibrate_homing_motor(motor_id, motor_bus):
 
 def calibrate_linear_motor(motor_id, motor_bus):
     motor_names = motor_bus.motor_names
-    motor_name = motor_names[motor_id - 1]
+    motor_name = motor_names[motor_id - 1]  # TODO(pepijn): replace motor_id with motor index when (id-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
@@ -77,7 +77,7 @@ def single_motor_calibration(arm: MotorsBus, motor_id: int):
     if motor_id == 6:
         start_pos, end_pos = calibrate_linear_motor(motor_id, arm)
     else:
-        input("Move the motor to middle (home) position, then press Enter...")
+        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.")

From 042c095ddd14ba8f237bbe5dd3caa280604010ea Mon Sep 17 00:00:00 2001
From: Pepijn <pepijn@huggingface.co>
Date: Wed, 19 Feb 2025 10:38:25 +0100
Subject: [PATCH 15/19] Add back drive mode

---
 .../common/robot_devices/motors/feetech.py    | 34 +++++++++++--------
 lerobot/scripts/calibration_visualization.py  |  3 +-
 2 files changed, 22 insertions(+), 15 deletions(-)

diff --git a/lerobot/common/robot_devices/motors/feetech.py b/lerobot/common/robot_devices/motors/feetech.py
index e22727931..d1d518ffc 100644
--- a/lerobot/common/robot_devices/motors/feetech.py
+++ b/lerobot/common/robot_devices/motors/feetech.py
@@ -141,6 +141,10 @@ def adjusted_to_homing_ticks(
     """
     Shifts raw [0..4095] ticks by an encoder offset, modulo a single turn [0..4095].
     """
+    drive_mode = 0
+    if motorbus.calibration is not None:
+        drive_mode = motorbus.calibration["drive_mode"][motor_id - 1]
+
     # Retrieve previous values for tracking
     prev_value = motorbus.previous_value[motor_id - 1]
     multi_turn_index = motorbus.multi_turn_index[motor_id - 1]
@@ -163,7 +167,15 @@ def adjusted_to_homing_ticks(
     motorbus.previous_value[motor_id - 1] = shifted
     motorbus.multi_turn_index[motor_id - 1] = multi_turn_index
 
-    return shifted + (multi_turn_index * resolutions)
+    ticks = shifted + (multi_turn_index * resolutions)
+
+    # 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:
+        ticks *= -1
+
+    return ticks
 
 
 def adjusted_to_motor_ticks(
@@ -173,6 +185,13 @@ def adjusted_to_motor_ticks(
     Inverse of adjusted_to_homing_ticks().
     Converts homed servo ticks (with multi-turn indexing) back to [0..4095].
     """
+    drive_mode = 0
+    if motorbus.calibration is not None:
+        drive_mode = motorbus.calibration["drive_mode"][motor_id - 1]
+    # If inverted, flip the adjusted value back.
+    if drive_mode:
+        adjusted_pos *= -1
+
     resolutions = MODEL_RESOLUTION[model]
     # Get the current multi-turn index and remove that offset
     multi_turn_index = motorbus.multi_turn_index[motor_id - 1]
@@ -456,7 +475,6 @@ 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]
                 motor_idx, model = self.motors[name]
 
@@ -464,12 +482,6 @@ class FeetechMotorsBus:
                 values[i] = adjusted_to_homing_ticks(values[i], homing_offset, 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
-
             elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
                 start_pos = self.calibration["start_pos"][calib_idx]
                 end_pos = self.calibration["end_pos"][calib_idx]
@@ -500,7 +512,6 @@ 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]
                 motor_idx, model = self.motors[name]
 
@@ -508,11 +519,6 @@ class FeetechMotorsBus:
                 values[i] = convert_degrees_to_ticks(values[i], model, self, motor_idx)
                 values[i] = adjusted_to_motor_ticks(values[i], homing_offset, model, self, motor_idx)
 
-                # 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]
                 end_pos = self.calibration["end_pos"][calib_idx]
diff --git a/lerobot/scripts/calibration_visualization.py b/lerobot/scripts/calibration_visualization.py
index 4ada769c3..cb80716fd 100644
--- a/lerobot/scripts/calibration_visualization.py
+++ b/lerobot/scripts/calibration_visualization.py
@@ -84,6 +84,7 @@ def debug_feetech_positions(cfg, arm_arg: str):
                 manual_adjusted = adjusted_to_homing_ticks(raw_ticks, offset, model, bus, motor_idx)
                 # Convert to degrees
                 manual_degs = convert_ticks_to_degrees(manual_adjusted, model)
+
                 # Convert to ticks
                 manual_ticks = convert_degrees_to_ticks(manual_degs, model, bus, motor_idx)
                 # Invert
@@ -96,7 +97,7 @@ def debug_feetech_positions(cfg, arm_arg: str):
                     f"MANUAL_ADJ_TICKS={manual_adjusted:6d} | "
                     f"MANUAL_ADJ_DEG={manual_degs:7.2f} | "
                     f"INV_TICKS={manual_ticks:6d} | "
-                    f"INV_STEPS={inv_ticks:4d}"
+                    f"INV_TICKS={inv_ticks:4d}"
                 )
             print("----------------------------------------------------")
             time.sleep(0.25)  # slow down loop

From 18ae1b802dd6aaea8a238676b4fb1130e48c7b11 Mon Sep 17 00:00:00 2001
From: Pepijn <pepijn@huggingface.co>
Date: Wed, 19 Feb 2025 11:34:56 +0100
Subject: [PATCH 16/19] set drive mode

---
 lerobot/common/robot_devices/robots/feetech_calibration.py | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/lerobot/common/robot_devices/robots/feetech_calibration.py b/lerobot/common/robot_devices/robots/feetech_calibration.py
index 1c50b1e64..cbdada0b9 100644
--- a/lerobot/common/robot_devices/robots/feetech_calibration.py
+++ b/lerobot/common/robot_devices/robots/feetech_calibration.py
@@ -118,7 +118,7 @@ def run_full_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm
 
     print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...")
 
-    print("\nMove arm to homing position")
+    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 center) in tutorial
@@ -146,9 +146,12 @@ def run_full_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm
 
     print(f"\n calibration of {robot_type} {arm_name} {arm_type} done!")
 
+    # Force drive_mode values: motors 2 and 5 -> drive_mode 1; all others -> 0.
+    drive_modes = [0, 1, 0, 0, 1, 0]
+
     calib_dict = {
         "homing_offset": encoder_offsets.astype(int).tolist(),
-        "drive_mode": np.zeros(len(arm.motor_indices), dtype=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),

From 645bfd78d8b5f16efc7ea2c25390f03db16546ab Mon Sep 17 00:00:00 2001
From: Pepijn <pepijn@huggingface.co>
Date: Wed, 19 Feb 2025 14:51:01 +0100
Subject: [PATCH 17/19] With multi turn (code is good, but feetech can't handle
 multi turn commands)

---
 .../common/robot_devices/motors/feetech.py    | 79 +++++++++----------
 .../robots/feetech_calibration.py             |  2 +-
 lerobot/scripts/calibration_visualization.py  | 12 +--
 3 files changed, 44 insertions(+), 49 deletions(-)

diff --git a/lerobot/common/robot_devices/motors/feetech.py b/lerobot/common/robot_devices/motors/feetech.py
index d1d518ffc..af3dabff2 100644
--- a/lerobot/common/robot_devices/motors/feetech.py
+++ b/lerobot/common/robot_devices/motors/feetech.py
@@ -1,3 +1,4 @@
+
 import enum
 import time
 import traceback
@@ -113,26 +114,15 @@ NUM_WRITE_RETRY = 20
 
 def convert_ticks_to_degrees(ticks, model):
     resolutions = MODEL_RESOLUTION[model]
-    degrees = (ticks / resolutions) * 360.0  # Convert to 0-360 range
-
-    # Convert to range [-180, 180]
-    degrees = (degrees + 180) % 360 - 180
-    return degrees
+    # Convert the ticks to degrees 
+    return ticks * (360.0/resolutions)
 
 
-def convert_degrees_to_ticks(degrees, model, motorbus, motor_id: int):
-    multi_turn_index = motorbus.multi_turn_index[motor_id - 1]
+
+def convert_degrees_to_ticks(degrees, model):
     resolutions = MODEL_RESOLUTION[model]
-    # Remove full rotations from degrees
-    base_degrees = degrees - (multi_turn_index * 360.0)
-
     # Convert degrees to motor ticks
-    ticks = base_degrees / 180.0 * (resolutions / 2)
-
-    # Add back multi-turn ticks
-    ticks += multi_turn_index * resolutions
-
-    return int(ticks)
+    return int(degrees * (resolutions/360.0))
 
 
 def adjusted_to_homing_ticks(
@@ -141,37 +131,39 @@ def adjusted_to_homing_ticks(
     """
     Shifts raw [0..4095] ticks by an encoder offset, modulo a single turn [0..4095].
     """
-    drive_mode = 0
-    if motorbus.calibration is not None:
-        drive_mode = motorbus.calibration["drive_mode"][motor_id - 1]
-
     # Retrieve previous values for tracking
     prev_value = motorbus.previous_value[motor_id - 1]
     multi_turn_index = motorbus.multi_turn_index[motor_id - 1]
-
     resolutions = MODEL_RESOLUTION[model]
+
+    # Add offset and wrap within resolution
     shifted = (raw_motor_ticks + encoder_offset) % resolutions
+
+    # # Re-center into a symmetric range (e.g., [-2048, 2047] if resolutions==4096) Thus the middle homing position will be virtual 0.
     if shifted > resolutions // 2:
         shifted -= resolutions
 
+    # Update multi turn values if needed
     if prev_value is not None:
         delta = shifted - prev_value
-
         # If jump forward > 180° (2048 steps), assume full rotation
-        if delta > resolutions // 2:
+        if delta > (resolutions // 2):
             multi_turn_index -= 1
-        elif delta < -resolutions // 2:
+        elif delta < (-resolutions // 2):
             multi_turn_index += 1
-
-    # Update stored values
     motorbus.previous_value[motor_id - 1] = shifted
     motorbus.multi_turn_index[motor_id - 1] = multi_turn_index
 
+    # Apply the multi turn to output so we can track beyong -180..180 degrees or -2048..2048 ticks
     ticks = shifted + (multi_turn_index * resolutions)
 
     # 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.
+    drive_mode = 0
+    if motorbus.calibration is not None:
+        drive_mode = motorbus.calibration["drive_mode"][motor_id - 1]
+    
     if drive_mode:
         ticks *= -1
 
@@ -185,27 +177,27 @@ def adjusted_to_motor_ticks(
     Inverse of adjusted_to_homing_ticks().
     Converts homed servo ticks (with multi-turn indexing) back to [0..4095].
     """
+    multi_turn_index = motorbus.multi_turn_index[motor_id - 1]
+
+    resolutions = MODEL_RESOLUTION[model]
+
+    # Remove offset and wrap within resolution
+    shifted = (adjusted_pos - encoder_offset) % resolutions
+
+    # Apply the multi turn to output ticks because goal position can have input of -32000...32000
+    ticks = shifted + (multi_turn_index * resolutions)
+
+    # 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.
     drive_mode = 0
     if motorbus.calibration is not None:
         drive_mode = motorbus.calibration["drive_mode"][motor_id - 1]
-    # If inverted, flip the adjusted value back.
+    
     if drive_mode:
-        adjusted_pos *= -1
+        ticks *= -1
 
-    resolutions = MODEL_RESOLUTION[model]
-    # Get the current multi-turn index and remove that offset
-    multi_turn_index = motorbus.multi_turn_index[motor_id - 1]
-    adjusted_pos -= multi_turn_index * 4096
-
-    #  Convert back into [−2048..2047] before final modulo
-    if adjusted_pos > 2047:
-        adjusted_pos -= 4096
-    elif adjusted_pos < -2048:
-        adjusted_pos += 4096
-
-    # Map back to raw ticks [0..4095]
-    raw_ticks = (adjusted_pos - encoder_offset) % resolutions
-    return raw_ticks
+    return ticks
 
 
 def convert_to_bytes(value, bytes, mock=False):
@@ -516,7 +508,7 @@ class FeetechMotorsBus:
                 motor_idx, model = self.motors[name]
 
                 # Convert degrees to homed ticks, then convert the homed ticks to raw ticks
-                values[i] = convert_degrees_to_ticks(values[i], model, self, motor_idx)
+                values[i] = convert_degrees_to_ticks(values[i], model)
                 values[i] = adjusted_to_motor_ticks(values[i], homing_offset, model, self, motor_idx)
 
             elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
@@ -757,3 +749,4 @@ class FeetechMotorsBus:
     def __del__(self):
         if getattr(self, "is_connected", False):
             self.disconnect()
+
diff --git a/lerobot/common/robot_devices/robots/feetech_calibration.py b/lerobot/common/robot_devices/robots/feetech_calibration.py
index cbdada0b9..7a47dac2c 100644
--- a/lerobot/common/robot_devices/robots/feetech_calibration.py
+++ b/lerobot/common/robot_devices/robots/feetech_calibration.py
@@ -147,7 +147,7 @@ def run_full_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm
     print(f"\n calibration of {robot_type} {arm_name} {arm_type} done!")
 
     # Force drive_mode values: motors 2 and 5 -> drive_mode 1; all others -> 0.
-    drive_modes = [0, 1, 0, 0, 1, 0]
+    drive_modes = [0, 0, 0, 0, 0, 0] 
 
     calib_dict = {
         "homing_offset": encoder_offsets.astype(int).tolist(),
diff --git a/lerobot/scripts/calibration_visualization.py b/lerobot/scripts/calibration_visualization.py
index cb80716fd..a1680c962 100644
--- a/lerobot/scripts/calibration_visualization.py
+++ b/lerobot/scripts/calibration_visualization.py
@@ -79,6 +79,7 @@ def debug_feetech_positions(cfg, arm_arg: str):
                 if bus.calibration and name in bus.calibration["motor_names"]:
                     offset_idx = bus.calibration["motor_names"].index(name)
                     offset = bus.calibration["homing_offset"][offset_idx]
+                    multi_turn_index = bus.multi_turn_index[offset_idx]
 
                 # Manually compute "adjusted ticks" from raw ticks
                 manual_adjusted = adjusted_to_homing_ticks(raw_ticks, offset, model, bus, motor_idx)
@@ -86,18 +87,19 @@ def debug_feetech_positions(cfg, arm_arg: str):
                 manual_degs = convert_ticks_to_degrees(manual_adjusted, model)
 
                 # Convert to ticks
-                manual_ticks = convert_degrees_to_ticks(manual_degs, model, bus, motor_idx)
+                manual_ticks = convert_degrees_to_ticks(manual_degs, model)
                 # Invert
                 inv_ticks = adjusted_to_motor_ticks(manual_ticks, offset, model, bus, motor_idx)
 
                 print(
                     f"{name:15s} | "
                     f"RAW={raw_ticks:4d} | "
-                    f"HOMED={homed_val:7.2f} | "
-                    f"MANUAL_ADJ_TICKS={manual_adjusted:6d} | "
+                    f"HOMED_FROM_READ={homed_val:7.2f} | "
+                    f"HOMED_TICKS={manual_adjusted:6d} | "
                     f"MANUAL_ADJ_DEG={manual_degs:7.2f} | "
-                    f"INV_TICKS={manual_ticks:6d} | "
-                    f"INV_TICKS={inv_ticks:4d}"
+                    f"MANUAL_ADJ_TICKS={manual_ticks:6d} | "
+                    f"INV_TICKS={inv_ticks:4d} | "
+                    f"MULTI_TURN_INDEX={multi_turn_index}"
                 )
             print("----------------------------------------------------")
             time.sleep(0.25)  # slow down loop

From 25a5b9ad0b33151807f6323464354762c13bba4f Mon Sep 17 00:00:00 2001
From: Pepijn <pepijn@huggingface.co>
Date: Wed, 19 Feb 2025 15:22:07 +0100
Subject: [PATCH 18/19] Zero homing, same values as previous calib

---
 .../common/robot_devices/motors/feetech.py    | 45 ++++---------------
 .../robots/feetech_calibration.py             |  9 ++--
 lerobot/scripts/calibration_visualization.py  |  4 +-
 3 files changed, 15 insertions(+), 43 deletions(-)

diff --git a/lerobot/common/robot_devices/motors/feetech.py b/lerobot/common/robot_devices/motors/feetech.py
index af3dabff2..f766a199d 100644
--- a/lerobot/common/robot_devices/motors/feetech.py
+++ b/lerobot/common/robot_devices/motors/feetech.py
@@ -118,7 +118,6 @@ def convert_ticks_to_degrees(ticks, model):
     return ticks * (360.0/resolutions)
 
 
-
 def convert_degrees_to_ticks(degrees, model):
     resolutions = MODEL_RESOLUTION[model]
     # Convert degrees to motor ticks
@@ -131,31 +130,14 @@ def adjusted_to_homing_ticks(
     """
     Shifts raw [0..4095] ticks by an encoder offset, modulo a single turn [0..4095].
     """
-    # Retrieve previous values for tracking
-    prev_value = motorbus.previous_value[motor_id - 1]
-    multi_turn_index = motorbus.multi_turn_index[motor_id - 1]
     resolutions = MODEL_RESOLUTION[model]
 
     # Add offset and wrap within resolution
-    shifted = (raw_motor_ticks + encoder_offset) % resolutions
+    ticks = (raw_motor_ticks + encoder_offset) % resolutions
 
     # # Re-center into a symmetric range (e.g., [-2048, 2047] if resolutions==4096) Thus the middle homing position will be virtual 0.
-    if shifted > resolutions // 2:
-        shifted -= resolutions
-
-    # Update multi turn values if needed
-    if prev_value is not None:
-        delta = shifted - prev_value
-        # If jump forward > 180° (2048 steps), assume full rotation
-        if delta > (resolutions // 2):
-            multi_turn_index -= 1
-        elif delta < (-resolutions // 2):
-            multi_turn_index += 1
-    motorbus.previous_value[motor_id - 1] = shifted
-    motorbus.multi_turn_index[motor_id - 1] = multi_turn_index
-
-    # Apply the multi turn to output so we can track beyong -180..180 degrees or -2048..2048 ticks
-    ticks = shifted + (multi_turn_index * resolutions)
+    if ticks > resolutions // 2:
+        ticks -= resolutions
 
     # 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
@@ -175,18 +157,7 @@ def adjusted_to_motor_ticks(
 ) -> int:
     """
     Inverse of adjusted_to_homing_ticks().
-    Converts homed servo ticks (with multi-turn indexing) back to [0..4095].
     """
-    multi_turn_index = motorbus.multi_turn_index[motor_id - 1]
-
-    resolutions = MODEL_RESOLUTION[model]
-
-    # Remove offset and wrap within resolution
-    shifted = (adjusted_pos - encoder_offset) % resolutions
-
-    # Apply the multi turn to output ticks because goal position can have input of -32000...32000
-    ticks = shifted + (multi_turn_index * resolutions)
-
     # 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.
@@ -195,7 +166,12 @@ def adjusted_to_motor_ticks(
         drive_mode = motorbus.calibration["drive_mode"][motor_id - 1]
     
     if drive_mode:
-        ticks *= -1
+        adjusted_pos *= -1
+
+    resolutions = MODEL_RESOLUTION[model]
+
+    # Remove offset and wrap within resolution
+    ticks = (adjusted_pos - encoder_offset) % resolutions
 
     return ticks
 
@@ -356,9 +332,6 @@ class FeetechMotorsBus:
         self.group_writers = {}
         self.logs = {}
 
-        self.multi_turn_index = self.multi_turn_index = [0] * len(self.motors)
-        self.previous_value = self.previous_value = [0] * len(self.motors)
-
     def connect(self):
         if self.is_connected:
             raise RobotDeviceAlreadyConnectedError(
diff --git a/lerobot/common/robot_devices/robots/feetech_calibration.py b/lerobot/common/robot_devices/robots/feetech_calibration.py
index 7a47dac2c..caf80aace 100644
--- a/lerobot/common/robot_devices/robots/feetech_calibration.py
+++ b/lerobot/common/robot_devices/robots/feetech_calibration.py
@@ -118,10 +118,10 @@ def run_full_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm
 
     print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...")
 
-    print("\nMove arm to homing position (middle)")
+    print("\nMove arm to homing position (zero)")
     print(
         "See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="zero")
-    )  # TODO(pepijn): replace with new instruction homing pos (all motors in center) in tutorial
+    )  # TODO(pepijn): replace with new instruction homing pos (all motors in zero) in tutorial
     input("Press Enter to continue...")
 
     start_positions = np.zeros(len(arm.motor_indices))
@@ -146,8 +146,9 @@ def run_full_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm
 
     print(f"\n calibration of {robot_type} {arm_name} {arm_type} done!")
 
-    # Force drive_mode values: motors 2 and 5 -> drive_mode 1; all others -> 0.
-    drive_modes = [0, 0, 0, 0, 0, 0] 
+    # Force drive_mode values: motors 2 and 5 -> drive_mode 1; all others -> 0. 1 = clockwise = positive range (0..180), 0 = clockwise = negative range (0..-180)
+    # 
+    drive_modes = [0, 1, 0, 0, 1, 0] 
 
     calib_dict = {
         "homing_offset": encoder_offsets.astype(int).tolist(),
diff --git a/lerobot/scripts/calibration_visualization.py b/lerobot/scripts/calibration_visualization.py
index a1680c962..c646a21d0 100644
--- a/lerobot/scripts/calibration_visualization.py
+++ b/lerobot/scripts/calibration_visualization.py
@@ -79,7 +79,6 @@ def debug_feetech_positions(cfg, arm_arg: str):
                 if bus.calibration and name in bus.calibration["motor_names"]:
                     offset_idx = bus.calibration["motor_names"].index(name)
                     offset = bus.calibration["homing_offset"][offset_idx]
-                    multi_turn_index = bus.multi_turn_index[offset_idx]
 
                 # Manually compute "adjusted ticks" from raw ticks
                 manual_adjusted = adjusted_to_homing_ticks(raw_ticks, offset, model, bus, motor_idx)
@@ -98,8 +97,7 @@ def debug_feetech_positions(cfg, arm_arg: str):
                     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} | "
-                    f"MULTI_TURN_INDEX={multi_turn_index}"
+                    f"INV_TICKS={inv_ticks:4d} "
                 )
             print("----------------------------------------------------")
             time.sleep(0.25)  # slow down loop

From b5bc3d1a25639be21f2034982ad6022bd37a0739 Mon Sep 17 00:00:00 2001
From: Pepijn <pepijn@huggingface.co>
Date: Wed, 19 Feb 2025 16:34:36 +0100
Subject: [PATCH 19/19] Move to middle

---
 lerobot/common/robot_devices/robots/configs.py         | 10 ++--------
 .../common/robot_devices/robots/feetech_calibration.py |  5 ++---
 2 files changed, 4 insertions(+), 11 deletions(-)

diff --git a/lerobot/common/robot_devices/robots/configs.py b/lerobot/common/robot_devices/robots/configs.py
index a976f601c..13e1cc9d5 100644
--- a/lerobot/common/robot_devices/robots/configs.py
+++ b/lerobot/common/robot_devices/robots/configs.py
@@ -429,7 +429,7 @@ class So100RobotConfig(ManipulatorRobotConfig):
     leader_arms: dict[str, MotorsBusConfig] = field(
         default_factory=lambda: {
             "main": FeetechMotorsBusConfig(
-                port="/dev/tty.usbmodem58760431091",
+                port="/dev/tty.usbmodem585A0077581",
                 motors={
                     # name: (index, model)
                     "shoulder_pan": [1, "sts3215"],
@@ -446,7 +446,7 @@ class So100RobotConfig(ManipulatorRobotConfig):
     follower_arms: dict[str, MotorsBusConfig] = field(
         default_factory=lambda: {
             "main": FeetechMotorsBusConfig(
-                port="/dev/tty.usbmodem585A0076891",
+                port="/dev/tty.usbmodem585A0080521",
                 motors={
                     # name: (index, model)
                     "shoulder_pan": [1, "sts3215"],
@@ -462,12 +462,6 @@ class So100RobotConfig(ManipulatorRobotConfig):
 
     cameras: dict[str, CameraConfig] = field(
         default_factory=lambda: {
-            "laptop": OpenCVCameraConfig(
-                camera_index=0,
-                fps=30,
-                width=640,
-                height=480,
-            ),
             "phone": OpenCVCameraConfig(
                 camera_index=1,
                 fps=30,
diff --git a/lerobot/common/robot_devices/robots/feetech_calibration.py b/lerobot/common/robot_devices/robots/feetech_calibration.py
index caf80aace..677b43f6c 100644
--- a/lerobot/common/robot_devices/robots/feetech_calibration.py
+++ b/lerobot/common/robot_devices/robots/feetech_calibration.py
@@ -118,10 +118,10 @@ def run_full_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm
 
     print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...")
 
-    print("\nMove arm to homing 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 zero) in tutorial
+    )  # TODO(pepijn): replace with new instruction homing pos (all motors in middle) in tutorial
     input("Press Enter to continue...")
 
     start_positions = np.zeros(len(arm.motor_indices))
@@ -147,7 +147,6 @@ def run_full_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm
     print(f"\n calibration of {robot_type} {arm_name} {arm_type} done!")
 
     # Force drive_mode values: motors 2 and 5 -> drive_mode 1; all others -> 0. 1 = clockwise = positive range (0..180), 0 = clockwise = negative range (0..-180)
-    # 
     drive_modes = [0, 1, 0, 0, 1, 0] 
 
     calib_dict = {