fix: calibration in node

docs/conventions.md

@@ -205,3 +205,163 @@ lerobot/
 - **Baudrate Validation**: Only real devices will reveal communication problems
 - **User Flow Testing**: Test complete calibration workflows with actual hardware
 - **Port Management**: Ensure proper port cleanup between testing sessions
+
+### CRITICAL: Calibration Implementation Requirements
+
+#### Calibration File Format (Learned from SO-100 Implementation)
+
+- **NEVER use array-based format**: Calibration files must use motor names as keys, NOT arrays
+- **Python-Compatible Structure**: Each motor must be an object with `id`, `drive_mode`, `homing_offset`, `range_min`, `range_max`
+- **Wrong Format** (causes Python incompatibility):
+  ```json
+  {
+    "homing_offset": [47, 1013, -957, ...],
+    "drive_mode": [0, 0, 0, ...],
+    "motor_names": ["shoulder_pan", ...]
+  }
+  ```
+- **Correct Format** (Python-compatible):
+  ```json
+  {
+    "shoulder_pan": {
+      "id": 1,
+      "drive_mode": 0,
+      "homing_offset": 47,
+      "range_min": 985,
+      "range_max": 3085
+    }
+  }
+  ```
+
+#### Homing Offset Calibration Protocol (Critical for STS3215/Feetech Motors)
+
+- **MUST Reset Existing Offsets**: Before calculating new homing offsets, ALWAYS reset existing homing offsets to 0
+- **Python Reference**: Python's `set_half_turn_homings()` calls `reset_calibration()` first
+- **Missing Reset Causes**: Completely wrong homing offset values (~1000+ unit differences)
+- **Reset Protocol**: Write value 0 to Homing_Offset register (address 31) for each motor before reading positions
+- **Verification**: Ensure reset commands receive successful responses before proceeding
+
+#### STS3215 Sign-Magnitude Encoding
+
+- **Homing_Offset Uses Special Encoding**: Bit 11 is sign bit, lower 11 bits are magnitude
+- **Position Reads**: Some registers may need sign-magnitude decoding - verify against Python behavior
+- **Encoding Functions**: Implement `encodeSignMagnitude()` and `decodeSignMagnitude()` for protocol compatibility
+- **Common Symptom**: Values differing by ~2048 or ~4096 indicate sign-magnitude encoding issues
+
+#### Calibration Process Validation
+
+- **Same Neutral Position**: When comparing calibrations, ensure robot is in identical physical position
+- **Expected Accuracy**: Properly implemented calibration should match Python within 30 units
+- **Debug Protocol**: Log position values, reset confirmations, and calculation steps for troubleshooting
+- **Range Verification**: `wrist_roll` should always use full range (0-4095), other motors use recorded ranges
+
+#### Common Calibration Mistakes to Avoid
+
+1. **Skipping Homing Reset**: Leads to ~1000+ unit differences in homing offsets
+2. **Array-Based File Format**: Makes calibration files incompatible with Python lerobot
+3. **Ignoring Sign-Magnitude Encoding**: Causes specific motors (often wrist_roll) to have wrong values
+4. **Different Physical Positions**: Comparing calibrations done at different robot positions
+5. **Missing Motor ID Assignment**: Forgetting to assign correct motor IDs (1-6 for SO-100)
+
+#### Device-Agnostic Calibration Architecture
+
+- **No Hardcoded Device Values**: Calibration logic must be configurable for different robot types
+- **Configuration-Driven Protocol**: Motor IDs, register addresses, resolution, etc. should come from device config
+- **Extensible Design**: Adding new robot types should only require new config files, not core logic changes
+- **Example Bad Practice**: Hardcoding `const motorIds = [1,2,3,4,5,6]` in calibration logic
+- **Example Good Practice**: Using `config.motorIds` from device-specific configuration
+- **Protocol Abstraction**: Register addresses, resolution, encoding details should be configurable per device type
+
+#### CRITICAL: Calibration Sequence and Hardware State Management
+
+**The exact sequence of calibration operations is critical for Python compatibility. Getting this wrong causes major range/offset discrepancies.**
+
+##### The Correct Calibration Sequence (Matching Python Exactly)
+
+1. **Reset Existing Homing Offsets to 0**: Write 0 to all Homing_Offset registers
+2. **Read Physical Positions**: Get actual motor positions (will be raw, non-centered values)
+3. **Calculate New Homing Offsets**: `offset = position - (resolution-1)/2`
+4. **IMMEDIATELY Write Homing Offsets**: Write new offsets to motor registers **before range recording**
+5. **Read Positions for Range Init**: Now positions will appear centered (~2047) due to applied offsets
+6. **Record Range of Motion**: Use centered positions as starting min/max values
+7. **Write Hardware Position Limits**: Write `range_min`/`range_max` to motor limit registers
+
+##### Critical Implementation Details
+
+**Homing Offset Writing Must Be Immediate:**
+
+```typescript
+// WRONG - Only calculates, doesn't write to motors
+async function setHomingOffsets(config) {
+  const positions = await readMotorPositions(config);
+  const offsets = calculateOffsets(positions);
+  return offsets; // ❌ Not written to motors!
+}
+
+// CORRECT - Writes offsets to motors immediately
+async function setHomingOffsets(config) {
+  await resetHomingOffsets(config); // Reset first
+  const positions = await readMotorPositions(config);
+  const offsets = calculateOffsets(positions);
+  await writeHomingOffsetsToMotors(config, offsets); // ✅ Written immediately
+  return offsets;
+}
+```
+
+**Range Recording Initialization Must Read Actual Positions:**
+
+```typescript
+// WRONG - Hardcoded center values
+const rangeMins = {};
+const rangeMaxes = {};
+for (const motor of motors) {
+  rangeMins[motor] = 2047; // ❌ Hardcoded!
+  rangeMaxes[motor] = 2047;
+}
+
+// CORRECT - Read actual positions (now centered due to applied homing offsets)
+const startPositions = await readMotorPositions(config);
+const rangeMins = {};
+const rangeMaxes = {};
+for (let i = 0; i < motors.length; i++) {
+  rangeMins[motors[i]] = startPositions[i]; // ✅ Uses actual values
+  rangeMaxes[motors[i]] = startPositions[i];
+}
+```
+
+**Hardware Position Limits Must Be Written:**
+
+```typescript
+// Python writes these registers, so we must too
+await writeMotorRegister(config, motorId, MIN_POSITION_LIMIT_ADDR, range_min);
+await writeMotorRegister(config, motorId, MAX_POSITION_LIMIT_ADDR, range_max);
+```
+
+##### Why This Sequence Matters
+
+**Problem**: User moves robot to same physical position, but Python shows ~2047 and Node.js shows wildly different values (3013, 1200, etc.)
+
+**Root Cause**: Python applies homing offsets immediately, making subsequent position reads appear centered. Node.js was calculating offsets but not applying them, so position reads remained raw.
+
+**Evidence of Correct Implementation**: After fixing the sequence, Node.js and Python both show ~2047 for the same physical position, and final calibration ranges match within professional tolerances (±50 units).
+
+##### Register Addresses for STS3215 Motors
+
+```typescript
+const STS3215_REGISTERS = {
+  Present_Position: { address: 56, length: 2 },
+  Homing_Offset: { address: 31, length: 2 }, // Sign-magnitude encoded
+  Min_Position_Limit: { address: 9, length: 2 },
+  Max_Position_Limit: { address: 11, length: 2 },
+};
+```
+
+##### Common Sequence Mistakes That Cause Major Issues
+
+1. **Not Writing Homing Offsets**: Calculates but doesn't apply → position reads remain raw → wrong range initialization
+2. **Hardcoded Range Initialization**: Forces 2047 instead of reading actual positions → doesn't match Python behavior
+3. **Missing Hardware Limit Writing**: Python constrains motors, Node.js doesn't → different range recording behavior
+4. **Wrong Reset Timing**: Not resetting existing offsets first → accumulated offset errors
+5. **Skipping Intermediate Delays**: Not waiting for motor register writes to take effect → inconsistent state
+
+**This sequence debugging took extensive analysis to solve. Future implementations MUST follow this exact pattern to maintain Python compatibility.**

src/lerobot/node/common/calibration.ts

@@ -11,14 +11,70 @@ import { SerialPort } from "serialport";
 import logUpdate from "log-update";
 
 /**
- * SO-100 device configuration for calibration
+ * Sign-magnitude encoding functions for Feetech STS3215 motors
+ * Mirrors Python lerobot/common/utils/encoding_utils.py
+ */
+
+/**
+ * Encode a signed integer using sign-magnitude format
+ * Bit at sign_bit_index represents sign (0=positive, 1=negative)
+ * Lower bits represent magnitude
+ */
+function encodeSignMagnitude(value: number, signBitIndex: number): number {
+  const maxMagnitude = (1 << signBitIndex) - 1;
+  const magnitude = Math.abs(value);
+
+  if (magnitude > maxMagnitude) {
+    throw new Error(
+      `Magnitude ${magnitude} exceeds ${maxMagnitude} (max for signBitIndex=${signBitIndex})`
+    );
+  }
+
+  const directionBit = value < 0 ? 1 : 0;
+  return (directionBit << signBitIndex) | magnitude;
+}
+
+/**
+ * Decode a sign-magnitude encoded value back to signed integer
+ * Extracts sign bit and magnitude, then applies sign
+ */
+function decodeSignMagnitude(
+  encodedValue: number,
+  signBitIndex: number
+): number {
+  const directionBit = (encodedValue >> signBitIndex) & 1;
+  const magnitudeMask = (1 << signBitIndex) - 1;
+  const magnitude = encodedValue & magnitudeMask;
+  return directionBit ? -magnitude : magnitude;
+}
+
+/**
+ * Device configuration for calibration
+ * Despite the "SO100" name, this interface is now device-agnostic and configurable
+ * for any robot using similar serial protocols (Feetech STS3215, etc.)
  */
 export interface SO100CalibrationConfig {
   deviceType: "so100_follower" | "so100_leader";
   port: SerialPort;
   motorNames: string[];
+  motorIds: number[]; // Device-specific motor IDs (e.g., [1,2,3,4,5,6] for SO-100)
   driveModes: number[];
   calibModes: string[];
+
+  // Protocol-specific configuration
+  protocol: {
+    resolution: number; // Motor resolution (e.g., 4096 for STS3215)
+    homingOffsetAddress: number; // Register address for homing offset (e.g., 31 for STS3215)
+    homingOffsetLength: number; // Length in bytes for homing offset register
+    presentPositionAddress: number; // Register address for present position (e.g., 56 for STS3215)
+    presentPositionLength: number; // Length in bytes for present position register
+    minPositionLimitAddress: number; // Register address for min position limit (e.g., 9 for STS3215)
+    minPositionLimitLength: number; // Length in bytes for min position limit register
+    maxPositionLimitAddress: number; // Register address for max position limit (e.g., 11 for STS3215)
+    maxPositionLimitLength: number; // Length in bytes for max position limit register
+    signMagnitudeBit: number; // Sign bit index for homing offset encoding (e.g., 11 for STS3215)
+  };
+
   limits: {
     position_min: number[];
     position_max: number[];
@@ -29,14 +85,16 @@ export interface SO100CalibrationConfig {
 
 /**
  * Calibration results structure matching Python lerobot format
+ * This should match the MotorCalibration dataclass structure in Python
  */
 export interface CalibrationResults {
-  homing_offset: number[];
-  drive_mode: number[];
-  start_pos: number[];
-  end_pos: number[];
-  calib_mode: string[];
-  motor_names: string[];
+  [motorName: string]: {
+    id: number;
+    drive_mode: number;
+    homing_offset: number;
+    range_min: number;
+    range_max: number;
+  };
 }
 
 /**
@@ -80,32 +138,38 @@ export async function initializeDeviceCommunication(
 
 /**
  * Read current motor positions
- * Uses STS3215 protocol - same for all SO-100 devices
+ * Uses device-specific protocol - configurable for different robot types
  */
 export async function readMotorPositions(
   config: SO100CalibrationConfig,
   quiet: boolean = false
 ): Promise<number[]> {
   const motorPositions: number[] = [];
-  const motorIds = [1, 2, 3, 4, 5, 6]; // SO-100 uses servo IDs 1-6
 
-  for (let i = 0; i < motorIds.length; i++) {
-    const motorId = motorIds[i];
+  for (let i = 0; i < config.motorIds.length; i++) {
+    const motorId = config.motorIds[i];
     const motorName = config.motorNames[i];
 
     try {
-      // Create STS3215 Read Position packet
+      // Create Read Position packet using configurable address
       const packet = Buffer.from([
         0xff,
         0xff,
         motorId,
         0x04,
         0x02,
-        0x38,
+        config.protocol.presentPositionAddress, // Configurable address instead of hardcoded 0x38
         0x02,
         0x00,
       ]);
-      const checksum = ~(motorId + 0x04 + 0x02 + 0x38 + 0x02) & 0xff;
+      const checksum =
+        ~(
+          motorId +
+          0x04 +
+          0x02 +
+          config.protocol.presentPositionAddress +
+          0x02
+        ) & 0xff;
       packet[7] = checksum;
 
       if (!config.port || !config.port.isOpen) {
@@ -131,16 +195,19 @@ export async function readMotorPositions(
             const position = response[5] | (response[6] << 8);
             motorPositions.push(position);
           } else {
-            motorPositions.push(2047); // Fallback to center
+            // Use half of max resolution as fallback instead of hardcoded 2047
+            motorPositions.push(
+              Math.floor((config.protocol.resolution - 1) / 2)
+            );
           }
         } else {
-          motorPositions.push(2047);
+          motorPositions.push(Math.floor((config.protocol.resolution - 1) / 2));
         }
       } catch (readError) {
-        motorPositions.push(2047);
+        motorPositions.push(Math.floor((config.protocol.resolution - 1) / 2));
       }
     } catch (error) {
-      motorPositions.push(2047);
+      motorPositions.push(Math.floor((config.protocol.resolution - 1) / 2));
     }
 
     // Minimal delay between servo reads for 30Hz performance
@@ -158,7 +225,6 @@ export async function performInteractiveCalibration(
   config: SO100CalibrationConfig
 ): Promise<CalibrationResults> {
   // Step 1: Set homing position
-  console.log("📍 STEP 1: Set Homing Position");
   await promptUser(
     `Move the SO-100 ${config.deviceType} to the MIDDLE of its range of motion and press ENTER...`
   );
@@ -166,18 +232,30 @@ export async function performInteractiveCalibration(
   const homingOffsets = await setHomingOffsets(config);
 
   // Step 2: Record ranges of motion with live updates
-  console.log("\n📏 STEP 2: Record Joint Ranges");
   const { rangeMins, rangeMaxes } = await recordRangesOfMotion(config);
 
-  // Compile results silently
-  const results: CalibrationResults = {
-    homing_offset: config.motorNames.map((name) => homingOffsets[name]),
-    drive_mode: config.driveModes,
-    start_pos: config.motorNames.map((name) => rangeMins[name]),
-    end_pos: config.motorNames.map((name) => rangeMaxes[name]),
-    calib_mode: config.calibModes,
-    motor_names: config.motorNames,
-  };
+  // Step 3: Set special range for wrist_roll (full turn motor)
+  rangeMins["wrist_roll"] = 0;
+  rangeMaxes["wrist_roll"] = 4095;
+
+  // Step 4: Write hardware position limits to motors (matching Python behavior)
+  await writeHardwarePositionLimits(config, rangeMins, rangeMaxes);
+
+  // Compile results in Python-compatible format
+  const results: CalibrationResults = {};
+
+  for (let i = 0; i < config.motorNames.length; i++) {
+    const motorName = config.motorNames[i];
+    const motorId = config.motorIds[i];
+
+    results[motorName] = {
+      id: motorId,
+      drive_mode: config.driveModes[i],
+      homing_offset: homingOffsets[motorName],
+      range_min: rangeMins[motorName],
+      range_max: rangeMaxes[motorName],
+    };
+  }
 
   return results;
 }
@@ -200,26 +278,204 @@ export async function verifyCalibration(
   // Silent unless error - calibration verification passed internally
 }
 
+/**
+ * Reset homing offsets to 0 for all motors
+ * Mirrors Python reset_calibration() - critical step before calculating new offsets
+ * This ensures Present_Position reflects true physical position without existing offsets
+ */
+async function resetHomingOffsets(
+  config: SO100CalibrationConfig
+): Promise<void> {
+  for (let i = 0; i < config.motorIds.length; i++) {
+    const motorId = config.motorIds[i];
+    const motorName = config.motorNames[i];
+
+    try {
+      // Write 0 to Homing_Offset register using configurable address
+      const homingOffsetValue = 0;
+
+      // Create Write Homing_Offset packet using configurable address
+      const packet = Buffer.from([
+        0xff,
+        0xff, // Header
+        motorId, // Servo ID
+        0x05, // Length (Instruction + Address + Data + Checksum)
+        0x03, // Instruction: WRITE_DATA
+        config.protocol.homingOffsetAddress, // Configurable address instead of hardcoded 0x1f
+        homingOffsetValue & 0xff, // Data_L (low byte)
+        (homingOffsetValue >> 8) & 0xff, // Data_H (high byte)
+        0x00, // Checksum (will calculate)
+      ]);
+
+      // Calculate checksum using configurable address
+      const checksum =
+        ~(
+          motorId +
+          0x05 +
+          0x03 +
+          config.protocol.homingOffsetAddress +
+          (homingOffsetValue & 0xff) +
+          ((homingOffsetValue >> 8) & 0xff)
+        ) & 0xff;
+      packet[8] = checksum;
+
+      if (!config.port || !config.port.isOpen) {
+        throw new Error("Serial port not open");
+      }
+
+      // Send reset packet
+      await new Promise<void>((resolve, reject) => {
+        config.port.write(packet, (error) => {
+          if (error) {
+            reject(
+              new Error(
+                `Failed to reset homing offset for ${motorName}: ${error.message}`
+              )
+            );
+          } else {
+            resolve();
+          }
+        });
+      });
+
+      // Wait for response (silent unless error)
+      try {
+        await readData(config.port, 200);
+      } catch (error) {
+        // Silent - response not required for successful operation
+      }
+    } catch (error) {
+      throw new Error(
+        `Failed to reset homing offset for ${motorName}: ${
+          error instanceof Error ? error.message : error
+        }`
+      );
+    }
+
+    // Small delay between motor writes
+    await new Promise((resolve) => setTimeout(resolve, 20));
+  }
+}
+
 /**
  * Record homing offsets (current positions as center)
  * Mirrors Python bus.set_half_turn_homings()
+ *
+ * CRITICAL: Must reset existing homing offsets to 0 first (like Python does)
+ * CRITICAL: Must WRITE the new homing offsets to motors immediately (like Python does)
  */
 async function setHomingOffsets(
   config: SO100CalibrationConfig
 ): Promise<{ [motor: string]: number }> {
+  // CRITICAL: Reset existing homing offsets to 0 first (matching Python)
+  await resetHomingOffsets(config);
+
+  // Wait a moment for reset to take effect
+  await new Promise((resolve) => setTimeout(resolve, 100));
+
+  // Now read positions (which will be true physical positions)
   const currentPositions = await readMotorPositions(config);
   const homingOffsets: { [motor: string]: number } = {};
 
   for (let i = 0; i < config.motorNames.length; i++) {
     const motorName = config.motorNames[i];
     const position = currentPositions[i];
-    const maxRes = 4095; // STS3215 resolution
-    homingOffsets[motorName] = position - Math.floor(maxRes / 2);
+
+    // Generic formula: pos - int((max_res - 1) / 2) using configurable resolution
+    const halfTurn = Math.floor((config.protocol.resolution - 1) / 2);
+    homingOffsets[motorName] = position - halfTurn;
   }
 
+  // CRITICAL: Write homing offsets to motors immediately (matching Python exactly)
+  // Python does: for motor, offset in homing_offsets.items(): self.write("Homing_Offset", motor, offset)
+  await writeHomingOffsetsToMotors(config, homingOffsets);
+
   return homingOffsets;
 }
 
+/**
+ * Write homing offsets to motor registers immediately
+ * Mirrors Python's immediate writing in set_half_turn_homings()
+ */
+async function writeHomingOffsetsToMotors(
+  config: SO100CalibrationConfig,
+  homingOffsets: { [motor: string]: number }
+): Promise<void> {
+  for (let i = 0; i < config.motorIds.length; i++) {
+    const motorId = config.motorIds[i];
+    const motorName = config.motorNames[i];
+    const homingOffset = homingOffsets[motorName];
+
+    try {
+      // Encode using sign-magnitude format (like Python)
+      const encodedOffset = encodeSignMagnitude(
+        homingOffset,
+        config.protocol.signMagnitudeBit
+      );
+
+      // Create Write Homing_Offset packet
+      const packet = Buffer.from([
+        0xff,
+        0xff, // Header
+        motorId, // Servo ID
+        0x05, // Length
+        0x03, // Instruction: WRITE_DATA
+        config.protocol.homingOffsetAddress, // Homing_Offset address
+        encodedOffset & 0xff, // Data_L (low byte)
+        (encodedOffset >> 8) & 0xff, // Data_H (high byte)
+        0x00, // Checksum (will calculate)
+      ]);
+
+      // Calculate checksum
+      const checksum =
+        ~(
+          motorId +
+          0x05 +
+          0x03 +
+          config.protocol.homingOffsetAddress +
+          (encodedOffset & 0xff) +
+          ((encodedOffset >> 8) & 0xff)
+        ) & 0xff;
+      packet[8] = checksum;
+
+      if (!config.port || !config.port.isOpen) {
+        throw new Error("Serial port not open");
+      }
+
+      // Send packet
+      await new Promise<void>((resolve, reject) => {
+        config.port.write(packet, (error) => {
+          if (error) {
+            reject(
+              new Error(
+                `Failed to write homing offset for ${motorName}: ${error.message}`
+              )
+            );
+          } else {
+            resolve();
+          }
+        });
+      });
+
+      // Wait for response (silent unless error)
+      try {
+        await readData(config.port, 200);
+      } catch (error) {
+        // Silent - response not required for successful operation
+      }
+    } catch (error) {
+      throw new Error(
+        `Failed to write homing offset for ${motorName}: ${
+          error instanceof Error ? error.message : error
+        }`
+      );
+    }
+
+    // Small delay between motor writes
+    await new Promise((resolve) => setTimeout(resolve, 20));
+  }
+}
+
 /**
  * Record ranges of motion with live updating table
  * Mirrors Python bus.record_ranges_of_motion()
@@ -228,7 +484,6 @@ async function recordRangesOfMotion(config: SO100CalibrationConfig): Promise<{
   rangeMins: { [motor: string]: number };
   rangeMaxes: { [motor: string]: number };
 }> {
-  console.log("\n=== RECORDING RANGES OF MOTION ===");
   console.log(
     "Move all joints sequentially through their entire ranges of motion."
   );
@@ -239,13 +494,16 @@ async function recordRangesOfMotion(config: SO100CalibrationConfig): Promise<{
   const rangeMins: { [motor: string]: number } = {};
   const rangeMaxes: { [motor: string]: number } = {};
 
-  // Initialize with current positions
-  const initialPositions = await readMotorPositions(config);
+  // Read actual current positions (matching Python exactly)
+  // Python does: start_positions = self.sync_read("Present_Position", motors, normalize=False)
+  // mins = start_positions.copy(); maxes = start_positions.copy()
+  const startPositions = await readMotorPositions(config);
+
   for (let i = 0; i < config.motorNames.length; i++) {
     const motorName = config.motorNames[i];
-    const position = initialPositions[i];
-    rangeMins[motorName] = position;
-    rangeMaxes[motorName] = position;
+    const startPosition = startPositions[i];
+    rangeMins[motorName] = startPosition; // Use actual position, not hardcoded 2047
+    rangeMaxes[motorName] = startPosition; // Use actual position, not hardcoded 2047
   }
 
   let recording = true;
@@ -262,9 +520,6 @@ async function recordRangesOfMotion(config: SO100CalibrationConfig): Promise<{
     rl.close();
   });
 
-  console.log("Recording started... (move the robot joints now)");
-  console.log("Live table will appear below - values update in real time!\n");
-
   // Continuous recording loop with live updates - THE LIVE UPDATING TABLE!
   while (recording) {
     try {
@@ -287,8 +542,7 @@ async function recordRangesOfMotion(config: SO100CalibrationConfig): Promise<{
       // Show real-time feedback every 3 reads for faster updates - LIVE TABLE UPDATE
       if (readCount % 3 === 0) {
         // Build the live table content
-        let liveTable = "=== LIVE POSITION RECORDING ===\n";
-        liveTable += `Readings: ${readCount} | Press ENTER to stop\n\n`;
+        let liveTable = `Readings: ${readCount}\n\n`;
         liveTable += "Motor Name       Current    Min      Max      Range\n";
         liveTable += "─".repeat(55) + "\n";
 
@@ -366,3 +620,114 @@ async function readData(
     });
   });
 }
+
+/**
+ * Write hardware position limits to motors
+ * Mirrors Python lerobot write_calibration() behavior where it writes:
+ * - Min_Position_Limit register with calibration.range_min
+ * - Max_Position_Limit register with calibration.range_max
+ * This physically constrains the motors to the calibrated ranges
+ */
+async function writeHardwarePositionLimits(
+  config: SO100CalibrationConfig,
+  rangeMins: { [motor: string]: number },
+  rangeMaxes: { [motor: string]: number }
+): Promise<void> {
+  for (let i = 0; i < config.motorIds.length; i++) {
+    const motorId = config.motorIds[i];
+    const motorName = config.motorNames[i];
+    const minLimit = rangeMins[motorName];
+    const maxLimit = rangeMaxes[motorName];
+
+    try {
+      // Write Min_Position_Limit register
+      await writeMotorRegister(
+        config,
+        motorId,
+        config.protocol.minPositionLimitAddress,
+        minLimit,
+        `Min_Position_Limit for ${motorName}`
+      );
+
+      // Small delay between writes
+      await new Promise((resolve) => setTimeout(resolve, 20));
+
+      // Write Max_Position_Limit register
+      await writeMotorRegister(
+        config,
+        motorId,
+        config.protocol.maxPositionLimitAddress,
+        maxLimit,
+        `Max_Position_Limit for ${motorName}`
+      );
+
+      // Small delay between motors
+      await new Promise((resolve) => setTimeout(resolve, 20));
+    } catch (error) {
+      throw new Error(
+        `Failed to write position limits for ${motorName}: ${
+          error instanceof Error ? error.message : error
+        }`
+      );
+    }
+  }
+}
+
+/**
+ * Generic function to write a 2-byte value to a motor register
+ * Used for both Min_Position_Limit and Max_Position_Limit
+ */
+async function writeMotorRegister(
+  config: SO100CalibrationConfig,
+  motorId: number,
+  registerAddress: number,
+  value: number,
+  description: string
+): Promise<void> {
+  // Create Write Register packet
+  const packet = Buffer.from([
+    0xff,
+    0xff, // Header
+    motorId, // Servo ID
+    0x05, // Length (Instruction + Address + Data + Checksum)
+    0x03, // Instruction: WRITE_DATA
+    registerAddress, // Register address
+    value & 0xff, // Data_L (low byte)
+    (value >> 8) & 0xff, // Data_H (high byte)
+    0x00, // Checksum (will calculate)
+  ]);
+
+  // Calculate checksum
+  const checksum =
+    ~(
+      motorId +
+      0x05 +
+      0x03 +
+      registerAddress +
+      (value & 0xff) +
+      ((value >> 8) & 0xff)
+    ) & 0xff;
+  packet[8] = checksum;
+
+  if (!config.port || !config.port.isOpen) {
+    throw new Error("Serial port not open");
+  }
+
+  // Send packet
+  await new Promise<void>((resolve, reject) => {
+    config.port.write(packet, (error) => {
+      if (error) {
+        reject(new Error(`Failed to write ${description}: ${error.message}`));
+      } else {
+        resolve();
+      }
+    });
+  });
+
+  // Wait for response (silent unless error)
+  try {
+    await readData(config.port, 200);
+  } catch (error) {
+    // Silent - response not required for successful operation
+  }
+}

src/lerobot/node/common/so100_config.ts

@@ -19,10 +19,48 @@ const SO100_MOTOR_NAMES = [
   "gripper",
 ];
 
+/**
+ * Common motor IDs for all SO-100 devices (STS3215 servos)
+ */
+const SO100_MOTOR_IDS = [1, 2, 3, 4, 5, 6];
+
+/**
+ * Protocol configuration for STS3215 motors used in SO-100 devices
+ */
+interface STS3215Protocol {
+  resolution: number;
+  homingOffsetAddress: number;
+  homingOffsetLength: number;
+  presentPositionAddress: number;
+  presentPositionLength: number;
+  minPositionLimitAddress: number;
+  minPositionLimitLength: number;
+  maxPositionLimitAddress: number;
+  maxPositionLimitLength: number;
+  signMagnitudeBit: number; // Bit 11 is sign bit for Homing_Offset encoding
+}
+
+/**
+ * STS3215 Protocol Configuration
+ * These addresses and settings are specific to the STS3215 servo motors
+ */
+export const STS3215_PROTOCOL: STS3215Protocol = {
+  resolution: 4096, // 12-bit resolution (0-4095)
+  homingOffsetAddress: 31, // Address for Homing_Offset register
+  homingOffsetLength: 2, // 2 bytes for Homing_Offset
+  presentPositionAddress: 56, // Address for Present_Position register
+  presentPositionLength: 2, // 2 bytes for Present_Position
+  minPositionLimitAddress: 9, // Address for Min_Position_Limit register
+  minPositionLimitLength: 2, // 2 bytes for Min_Position_Limit
+  maxPositionLimitAddress: 11, // Address for Max_Position_Limit register
+  maxPositionLimitLength: 2, // 2 bytes for Max_Position_Limit
+  signMagnitudeBit: 11, // Bit 11 is sign bit for Homing_Offset encoding
+} as const;
+
 /**
  * SO-100 Follower Configuration
  * Robot arm that performs tasks autonomously
- * Uses standard gear ratios for all motors
+ * Drive modes match Python lerobot exactly: all motors use drive_mode=0
  */
 export function createSO100FollowerConfig(
   port: SerialPort
@@ -31,19 +69,21 @@ export function createSO100FollowerConfig(
     deviceType: "so100_follower",
     port,
     motorNames: SO100_MOTOR_NAMES,
+    motorIds: SO100_MOTOR_IDS,
+    protocol: STS3215_PROTOCOL,
 
-    // Follower uses standard drive modes (all same gear ratio)
-    driveModes: [0, 0, 0, 0, 0, 0], // All 1/345 gear ratio
+    // Python lerobot uses drive_mode=0 for all motors (current format)
+    driveModes: [0, 0, 0, 0, 0, 0],
 
-    // Calibration modes
+    // Calibration modes (not used in current implementation, but kept for compatibility)
     calibModes: ["DEGREE", "DEGREE", "DEGREE", "DEGREE", "DEGREE", "LINEAR"],
 
-    // Follower limits - optimized for autonomous operation
+    // Follower limits - these are not used in calibration file format
     limits: {
       position_min: [-180, -90, -90, -90, -90, -90],
       position_max: [180, 90, 90, 90, 90, 90],
-      velocity_max: [100, 100, 100, 100, 100, 100], // Fast for autonomous tasks
-      torque_max: [50, 50, 50, 50, 25, 25], // Higher torque for carrying loads
+      velocity_max: [100, 100, 100, 100, 100, 100],
+      torque_max: [50, 50, 50, 50, 25, 25],
     },
   };
 }
@@ -51,7 +91,7 @@ export function createSO100FollowerConfig(
 /**
  * SO-100 Leader Configuration
  * Teleoperator arm that humans use to control the follower
- * Uses mixed gear ratios for easier human operation
+ * Drive modes match Python lerobot exactly: all motors use drive_mode=0
  */
 export function createSO100LeaderConfig(
   port: SerialPort
@@ -60,20 +100,21 @@ export function createSO100LeaderConfig(
     deviceType: "so100_leader",
     port,
     motorNames: SO100_MOTOR_NAMES,
+    motorIds: SO100_MOTOR_IDS,
+    protocol: STS3215_PROTOCOL,
 
-    // Leader uses mixed gear ratios for easier human operation
-    // Based on Python lerobot leader calibration data
-    driveModes: [0, 1, 0, 0, 1, 0], // Mixed ratios: some 1/345, some 1/191, some 1/147
+    // Python lerobot uses drive_mode=0 for all motors (current format)
+    driveModes: [0, 0, 0, 0, 0, 0],
 
     // Same calibration modes as follower
     calibModes: ["DEGREE", "DEGREE", "DEGREE", "DEGREE", "DEGREE", "LINEAR"],
 
-    // Leader limits - optimized for human operation (safer, easier to move)
+    // Leader limits - these are not used in calibration file format
     limits: {
       position_min: [-120, -60, -60, -60, -180, -45],
       position_max: [120, 60, 60, 60, 180, 45],
-      velocity_max: [80, 80, 80, 80, 120, 60], // Slower for human control
-      torque_max: [30, 30, 30, 30, 20, 15], // Lower torque for safety
+      velocity_max: [80, 80, 80, 80, 120, 60],
+      torque_max: [30, 30, 30, 30, 20, 15],
     },
   };
 }