chore: recovered node

src/lerobot/node/calibrate.ts

@@ -1,15 +1,12 @@
 /**
  * Helper to recalibrate your device (robot or teleoperator).
  *
- * Direct port of Python lerobot calibrate.py
- *
  * Example:
  * ```
  * npx lerobot calibrate --robot.type=so100_follower --robot.port=COM4 --robot.id=my_follower_arm
  * ```
  */
 
-import type { CalibrateConfig } from "./robots/config.js";
 import { createSO100Follower } from "./robots/so100_follower.js";
 import { createSO100Leader } from "./teleoperators/so100_leader.js";
 import {
@@ -18,8 +15,9 @@ import {
   performInteractiveCalibration,
   setMotorLimits,
   verifyCalibration,
-  type CalibrationResults,
 } from "./common/calibration.js";
+import type { CalibrateConfig } from "./types/robot-config.js";
+import type { CalibrationResults } from "./types/calibration.js";
 import { getSO100Config } from "./common/so100_config.js";
 
 /**

src/lerobot/node/common/calibration.ts

@@ -11,34 +11,53 @@ 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
  */
-export interface SO100CalibrationConfig {
-  deviceType: "so100_follower" | "so100_leader";
-  port: SerialPort;
-  motorNames: string[];
-  driveModes: number[];
-  calibModes: string[];
-  limits: {
-    position_min: number[];
-    position_max: number[];
-    velocity_max: number[];
-    torque_max: number[];
-  };
+
+/**
+ * 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;
 }
 
 /**
- * Calibration results structure matching Python lerobot format
+ * Decode a sign-magnitude encoded value back to signed integer
+ * Extracts sign bit and magnitude, then applies sign
  */
-export interface CalibrationResults {
-  homing_offset: number[];
-  drive_mode: number[];
-  start_pos: number[];
-  end_pos: number[];
-  calib_mode: string[];
-  motor_names: string[];
+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.)
+ */
+import type {
+  SO100CalibrationConfig,
+  CalibrationResults,
+} from "../types/calibration.js";
+
 /**
  * Initialize device communication
  * Common for both SO-100 leader and follower (same hardware)
@@ -80,32 +99,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 +156,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 +186,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 +193,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 +239,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 +445,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 +455,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 +481,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 +503,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 +581,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

@@ -4,7 +4,7 @@
  * Mirrors Python lerobot device configuration approach
  */
 
-import type { SO100CalibrationConfig } from "./calibration.js";
+import type { SO100CalibrationConfig } from "../types/calibration.js";
 import { SerialPort } from "serialport";
 
 /**
@@ -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],
     },
   };
 }

src/lerobot/node/find_port.ts

@@ -1,8 +1,6 @@
 /**
  * Helper to find the USB port associated with your MotorsBus.
  *
- * Direct port of Python lerobot find_port.py
- *
  * Example:
  * ```
  * npx lerobot find-port

src/lerobot/node/robots/robot.ts

@@ -6,9 +6,10 @@
 
 import { SerialPort } from "serialport";
 import { mkdir, writeFile } from "fs/promises";
+import { existsSync, readFileSync, mkdirSync } from "fs";
 import { join } from "path";
-import type { RobotConfig } from "./config.js";
-import { getCalibrationDir, ROBOTS } from "../constants.js";
+import type { RobotConfig } from "../types/robot-config.js";
+import { getCalibrationDir, ROBOTS } from "../utils/constants.js";
 
 export abstract class Robot {
   protected port: SerialPort | null = null;
@@ -16,6 +17,8 @@ export abstract class Robot {
   protected calibrationDir: string;
   protected calibrationPath: string;
   protected name: string;
+  protected calibration: any = {}; // Loaded calibration data
+  protected isCalibrated: boolean = false;
 
   constructor(config: RobotConfig) {
     this.config = config;
@@ -29,6 +32,9 @@ export abstract class Robot {
     // Use robot ID or type as filename
     const robotId = config.id || this.name;
     this.calibrationPath = join(this.calibrationDir, `${robotId}.json`);
+
+    // Auto-load calibration if it exists (like Python version)
+    this.loadCalibration();
   }
 
   /**
@@ -98,7 +104,11 @@ export abstract class Robot {
    */
   protected async saveCalibration(calibrationData: any): Promise<void> {
     // Ensure calibration directory exists
-    await mkdir(this.calibrationDir, { recursive: true });
+    try {
+      mkdirSync(this.calibrationDir, { recursive: true });
+    } catch (error) {
+      // Directory might already exist, that's fine
+    }
 
     // Save calibration data as JSON
     await writeFile(
@@ -109,6 +119,34 @@ export abstract class Robot {
     console.log(`Configuration saved to: ${this.calibrationPath}`);
   }
 
+  /**
+   * Load calibration data from JSON file
+   * Mirrors Python's _load_calibration()
+   */
+  protected loadCalibration(): void {
+    try {
+      if (existsSync(this.calibrationPath)) {
+        const calibrationData = readFileSync(this.calibrationPath, "utf8");
+        this.calibration = JSON.parse(calibrationData);
+        this.isCalibrated = true;
+        console.log(`✅ Loaded calibration from: ${this.calibrationPath}`);
+      } else {
+        console.log(
+          `⚠️  No calibration file found at: ${this.calibrationPath}`
+        );
+        this.isCalibrated = false;
+      }
+    } catch (error) {
+      console.warn(
+        `Failed to load calibration: ${
+          error instanceof Error ? error.message : error
+        }`
+      );
+      this.calibration = {};
+      this.isCalibrated = false;
+    }
+  }
+
   /**
    * Send command to robot via serial port
    */

src/lerobot/node/robots/so100_follower.ts

@@ -4,7 +4,7 @@
  */
 
 import { Robot } from "./robot.js";
-import type { RobotConfig } from "./config.js";
+import type { RobotConfig } from "../types/robot-config.js";
 import * as readline from "readline";
 
 export class SO100Follower extends Robot {
@@ -81,6 +81,148 @@ export class SO100Follower extends Robot {
     console.log("Robot communication test completed.");
   }
 
+  /**
+   * Read current motor positions as a record with motor names
+   * For teleoperation use
+   */
+  async getMotorPositions(): Promise<Record<string, number>> {
+    const positions = await this.readMotorPositions();
+    const motorNames = [
+      "shoulder_pan",
+      "shoulder_lift",
+      "elbow_flex",
+      "wrist_flex",
+      "wrist_roll",
+      "gripper",
+    ];
+
+    const result: Record<string, number> = {};
+    for (let i = 0; i < motorNames.length; i++) {
+      result[motorNames[i]] = positions[i];
+    }
+    return result;
+  }
+
+  /**
+   * Get calibration data for teleoperation
+   * Returns position limits and offsets from calibration file
+   */
+  getCalibrationLimits(): Record<string, { min: number; max: number }> {
+    if (!this.isCalibrated || !this.calibration) {
+      console.warn("No calibration data available, using default limits");
+      // Default STS3215 limits as fallback
+      return {
+        shoulder_pan: { min: 985, max: 3085 },
+        shoulder_lift: { min: 1200, max: 2800 },
+        elbow_flex: { min: 1000, max: 3000 },
+        wrist_flex: { min: 1100, max: 2900 },
+        wrist_roll: { min: 0, max: 4095 }, // Full rotation motor
+        gripper: { min: 1800, max: 2300 },
+      };
+    }
+
+    // Extract limits from calibration data (matches Python format)
+    const limits: Record<string, { min: number; max: number }> = {};
+    for (const [motorName, calibData] of Object.entries(this.calibration)) {
+      if (
+        calibData &&
+        typeof calibData === "object" &&
+        "range_min" in calibData &&
+        "range_max" in calibData
+      ) {
+        limits[motorName] = {
+          min: Number(calibData.range_min),
+          max: Number(calibData.range_max),
+        };
+      }
+    }
+
+    return limits;
+  }
+
+  /**
+   * Set motor positions from a record with motor names
+   * For teleoperation use
+   */
+  async setMotorPositions(positions: Record<string, number>): Promise<void> {
+    const motorNames = [
+      "shoulder_pan",
+      "shoulder_lift",
+      "elbow_flex",
+      "wrist_flex",
+      "wrist_roll",
+      "gripper",
+    ];
+    const motorIds = [1, 2, 3, 4, 5, 6]; // SO-100 has servo IDs 1-6
+
+    for (let i = 0; i < motorNames.length; i++) {
+      const motorName = motorNames[i];
+      const motorId = motorIds[i];
+      const position = positions[motorName];
+
+      if (position !== undefined) {
+        await this.writeMotorPosition(motorId, position);
+      }
+    }
+  }
+
+  /**
+   * Write position to a single motor
+   * Implements STS3215 WRITE_DATA command for position control
+   */
+  private async writeMotorPosition(
+    motorId: number,
+    position: number
+  ): Promise<void> {
+    if (!this.port || !this.port.isOpen) {
+      throw new Error("Serial port not open");
+    }
+
+    // Clamp position to valid range
+    const clampedPosition = Math.max(0, Math.min(4095, Math.round(position)));
+
+    // Create STS3215 Write Position packet
+    // Format: [0xFF, 0xFF, ID, Length, Instruction, Address, Data_L, Data_H, Checksum]
+    // Goal_Position address for STS3215 is 42 (0x2A), length 2 bytes
+    const packet = Buffer.from([
+      0xff,
+      0xff, // Header
+      motorId, // Servo ID
+      0x05, // Length (Instruction + Address + Data_L + Data_H + Checksum)
+      0x03, // Instruction: WRITE_DATA
+      0x2a, // Address: Goal_Position (42)
+      clampedPosition & 0xff, // Data_L (low byte)
+      (clampedPosition >> 8) & 0xff, // Data_H (high byte)
+      0x00, // Checksum (will calculate)
+    ]);
+
+    // Calculate checksum: ~(ID + Length + Instruction + Address + Data_L + Data_H) & 0xFF
+    const checksum =
+      ~(
+        motorId +
+        0x05 +
+        0x03 +
+        0x2a +
+        (clampedPosition & 0xff) +
+        ((clampedPosition >> 8) & 0xff)
+      ) & 0xff;
+    packet[8] = checksum;
+
+    // Send write position packet
+    await new Promise<void>((resolve, reject) => {
+      this.port!.write(packet, (error) => {
+        if (error) {
+          reject(new Error(`Failed to send write packet: ${error.message}`));
+        } else {
+          resolve();
+        }
+      });
+    });
+
+    // Small delay to allow servo to process command
+    await new Promise((resolve) => setTimeout(resolve, 1));
+  }
+
   /**
    * Read current motor positions
    * Implements basic STS3215 servo protocol to read actual positions
@@ -153,7 +295,14 @@ export class SO100Follower extends Robot {
               // Extract 16-bit position from Data_L and Data_H
               const position = response[5] | (response[6] << 8);
               motorPositions.push(position);
-              console.log(`    ${motorName}: ${position} (0-4095 range)`);
+
+              // Show calibrated range if available
+              const calibratedLimits = this.getCalibrationLimits();
+              const limits = calibratedLimits[motorName];
+              const rangeText = limits
+                ? `(${limits.min}-${limits.max} calibrated)`
+                : `(0-4095 raw)`;
+              console.log(`    ${motorName}: ${position} ${rangeText}`);
             } else {
               console.warn(
                 `    ${motorName}: Error response (error code: ${error})`

src/lerobot/node/teleoperate.ts

@@ -0,0 +1,316 @@
+/**
+ * Robot teleoperation using keyboard control
+ *
+ * Example:
+ * ```
+ * npx lerobot teleoperate --robot.type=so100_follower --robot.port=COM4 --teleop.type=keyboard
+ * ```
+ */
+
+import { createSO100Follower } from "./robots/so100_follower.js";
+import { KeyboardController } from "./utils/keyboard-teleop.js";
+import type { TeleoperateConfig } from "./types/teleoperation.js";
+
+/**
+ * Main teleoperate function
+ * Mirrors Python lerobot teleoperate.py structure
+ */
+export async function teleoperate(config: TeleoperateConfig): Promise<void> {
+  // Validate configuration
+  if (!config.robot) {
+    throw new Error("Robot configuration is required");
+  }
+
+  if (!config.teleop || config.teleop.type !== "keyboard") {
+    throw new Error("Only keyboard teleoperation is currently supported");
+  }
+
+  const stepSize = config.step_size || 25;
+  const duration = config.duration_s;
+
+  let robot;
+  let keyboardController;
+
+  try {
+    // Create robot
+    switch (config.robot.type) {
+      case "so100_follower":
+        robot = createSO100Follower(config.robot);
+        break;
+      default:
+        throw new Error(`Unsupported robot type: ${config.robot.type}`);
+    }
+
+    console.log(
+      `Connecting to robot: ${config.robot.type} on ${config.robot.port}`
+    );
+    if (config.robot.id) {
+      console.log(`Robot ID: ${config.robot.id}`);
+    }
+
+    await robot.connect(false); // calibrate=false
+    console.log("Robot connected successfully.");
+
+    // Show calibration status
+    const isCalibrated = (robot as any).isCalibrated;
+    if (isCalibrated) {
+      console.log(
+        `✅ Loaded calibration for: ${config.robot.id || config.robot.type}`
+      );
+    } else {
+      console.log(
+        `⚠️  No calibration found for: ${
+          config.robot.id || config.robot.type
+        } (using defaults)`
+      );
+      console.log(
+        "   Run 'npx lerobot calibrate' first for optimal performance!"
+      );
+    }
+
+    // Create keyboard controller
+    keyboardController = new KeyboardController(robot, stepSize);
+
+    console.log("");
+    console.log("Starting keyboard teleoperation...");
+    console.log("Controls:");
+    console.log("  ↑↓ Arrow Keys: Shoulder Lift");
+    console.log("  ←→ Arrow Keys: Shoulder Pan");
+    console.log("  W/S: Elbow Flex");
+    console.log("  A/D: Wrist Flex");
+    console.log("  Q/E: Wrist Roll");
+    console.log("  Space: Gripper Toggle");
+    console.log("  ESC: Emergency Stop");
+    console.log("  Ctrl+C: Exit");
+    console.log("");
+    console.log("Press any control key to begin...");
+    console.log("");
+
+    // Start teleoperation control loop
+    await teleoperationLoop(keyboardController, robot, duration || null);
+  } catch (error) {
+    // Ensure we disconnect even if there's an error
+    if (keyboardController) {
+      try {
+        await keyboardController.stop();
+      } catch (stopError) {
+        console.warn("Warning: Failed to stop keyboard controller properly");
+      }
+    }
+    if (robot) {
+      try {
+        await robot.disconnect();
+      } catch (disconnectError) {
+        console.warn("Warning: Failed to disconnect robot properly");
+      }
+    }
+    throw error;
+  }
+}
+
+/**
+ * Main teleoperation control loop
+ */
+async function teleoperationLoop(
+  keyboardController: KeyboardController,
+  robot: any,
+  duration: number | null
+): Promise<void> {
+  console.log("Initializing teleoperation...");
+
+  // Start keyboard controller
+  await keyboardController.start();
+
+  const startTime = performance.now();
+
+  // Set up graceful shutdown
+  let running = true;
+  process.on("SIGINT", () => {
+    console.log("\nShutting down gracefully...");
+    running = false;
+  });
+
+  try {
+    // Just wait for the keyboard controller to handle everything
+    while (running) {
+      // Check duration limit
+      if (duration && performance.now() - startTime >= duration * 1000) {
+        console.log(`\nDuration limit reached (${duration}s). Stopping...`);
+        break;
+      }
+
+      // Small delay to prevent busy waiting
+      await new Promise((resolve) => setTimeout(resolve, 100));
+    }
+  } finally {
+    console.log("\nStopping teleoperation...");
+    await keyboardController.stop();
+    await robot.disconnect();
+    console.log("Teleoperation stopped.");
+  }
+}
+
+/**
+ * Parse command line arguments in Python argparse style
+ * Handles --robot.type=so100_follower --teleop.type=keyboard format
+ */
+export function parseArgs(args: string[]): TeleoperateConfig {
+  const config: Partial<TeleoperateConfig> = {};
+
+  for (const arg of args) {
+    if (arg.startsWith("--robot.")) {
+      if (!config.robot) {
+        config.robot = { type: "so100_follower", port: "" };
+      }
+
+      const [key, value] = arg.substring(8).split("=");
+      switch (key) {
+        case "type":
+          if (value !== "so100_follower") {
+            throw new Error(`Unsupported robot type: ${value}`);
+          }
+          config.robot.type = value as "so100_follower";
+          break;
+        case "port":
+          config.robot.port = value;
+          break;
+        case "id":
+          config.robot.id = value;
+          break;
+        default:
+          throw new Error(`Unknown robot parameter: ${key}`);
+      }
+    } else if (arg.startsWith("--teleop.")) {
+      if (!config.teleop) {
+        config.teleop = { type: "keyboard" };
+      }
+
+      const [key, value] = arg.substring(9).split("=");
+      switch (key) {
+        case "type":
+          if (value !== "keyboard") {
+            throw new Error(`Unsupported teleoperator type: ${value}`);
+          }
+          config.teleop.type = value as "keyboard";
+          break;
+        default:
+          throw new Error(`Unknown teleoperator parameter: ${key}`);
+      }
+    } else if (arg.startsWith("--fps=")) {
+      config.fps = parseInt(arg.substring(6));
+      if (isNaN(config.fps) || config.fps <= 0) {
+        throw new Error("FPS must be a positive number");
+      }
+    } else if (arg.startsWith("--step_size=")) {
+      config.step_size = parseInt(arg.substring(12));
+      if (isNaN(config.step_size) || config.step_size <= 0) {
+        throw new Error("Step size must be a positive number");
+      }
+    } else if (arg.startsWith("--duration_s=")) {
+      config.duration_s = parseInt(arg.substring(13));
+      if (isNaN(config.duration_s) || config.duration_s <= 0) {
+        throw new Error("Duration must be a positive number");
+      }
+    } else if (arg === "--help" || arg === "-h") {
+      showUsage();
+      process.exit(0);
+    } else if (!arg.startsWith("--")) {
+      // Skip non-option arguments
+      continue;
+    } else {
+      throw new Error(`Unknown argument: ${arg}`);
+    }
+  }
+
+  // Validate required fields
+  if (!config.robot?.port) {
+    throw new Error("Robot port is required (--robot.port=PORT)");
+  }
+  if (!config.teleop?.type) {
+    throw new Error("Teleoperator type is required (--teleop.type=keyboard)");
+  }
+
+  return config as TeleoperateConfig;
+}
+
+/**
+ * Show usage information matching Python argparse output
+ */
+function showUsage(): void {
+  console.log("Usage: lerobot teleoperate [options]");
+  console.log("");
+  console.log("Control a robot using keyboard input");
+  console.log("");
+  console.log("Options:");
+  console.log("  --robot.type=TYPE        Robot type (so100_follower)");
+  console.log(
+    "  --robot.port=PORT        Robot serial port (e.g., COM4, /dev/ttyUSB0)"
+  );
+  console.log("  --robot.id=ID            Robot identifier");
+  console.log("  --teleop.type=TYPE       Teleoperator type (keyboard)");
+  console.log(
+    "  --fps=FPS                Control loop frame rate (default: 60)"
+  );
+  console.log(
+    "  --step_size=SIZE         Position step size per keypress (default: 10)"
+  );
+  console.log("  --duration_s=SECONDS     Teleoperation duration in seconds");
+  console.log("  -h, --help               Show this help message");
+  console.log("");
+  console.log("Keyboard Controls:");
+  console.log("  ↑↓ Arrow Keys            Shoulder Lift");
+  console.log("  ←→ Arrow Keys            Shoulder Pan");
+  console.log("  W/S                      Elbow Flex");
+  console.log("  A/D                      Wrist Flex");
+  console.log("  Q/E                      Wrist Roll");
+  console.log("  Space                    Gripper Toggle");
+  console.log("  ESC                      Emergency Stop");
+  console.log("  Ctrl+C                   Exit");
+  console.log("");
+  console.log("Examples:");
+  console.log(
+    "  lerobot teleoperate --robot.type=so100_follower --robot.port=COM4 --teleop.type=keyboard"
+  );
+  console.log(
+    "  lerobot teleoperate --robot.type=so100_follower --robot.port=COM4 --teleop.type=keyboard --fps=30 --step_size=50"
+  );
+  console.log("");
+  console.log("Use 'lerobot find-port' to discover available ports.");
+}
+
+/**
+ * CLI entry point when called directly
+ * Mirrors Python's if __name__ == "__main__": pattern
+ */
+export async function main(args: string[]): Promise<void> {
+  try {
+    if (args.length === 0 || args.includes("--help") || args.includes("-h")) {
+      showUsage();
+      return;
+    }
+
+    const config = parseArgs(args);
+    await teleoperate(config);
+  } catch (error) {
+    if (error instanceof Error) {
+      console.error("Error:", error.message);
+    } else {
+      console.error("Error:", error);
+    }
+
+    console.error("");
+    console.error("Please verify:");
+    console.error("1. The robot is connected to the specified port");
+    console.error("2. No other application is using the port");
+    console.error("3. You have permission to access the port");
+    console.error("");
+    console.error("Use 'lerobot find-port' to discover available ports.");
+
+    process.exit(1);
+  }
+}
+
+if (import.meta.url === `file://${process.argv[1]}`) {
+  const args = process.argv.slice(2);
+  main(args);
+}

src/lerobot/node/teleoperators/so100_leader.ts

@@ -6,7 +6,7 @@
  */
 
 import { Teleoperator } from "./teleoperator.js";
-import type { TeleoperatorConfig } from "./config.js";
+import type { TeleoperatorConfig } from "../types/teleoperator-config.js";
 
 export class SO100Leader extends Teleoperator {
   constructor(config: TeleoperatorConfig) {

src/lerobot/node/teleoperators/teleoperator.ts

@@ -7,8 +7,8 @@
 import { SerialPort } from "serialport";
 import { mkdir, writeFile } from "fs/promises";
 import { join } from "path";
-import type { TeleoperatorConfig } from "./config.js";
-import { getCalibrationDir, TELEOPERATORS } from "../constants.js";
+import type { TeleoperatorConfig } from "../types/teleoperator-config.js";
+import { getCalibrationDir, TELEOPERATORS } from "../utils/constants.js";
 
 export abstract class Teleoperator {
   protected port: SerialPort | null = null;

src/lerobot/node/types/calibration.ts

@@ -0,0 +1,45 @@
+/**
+ * Calibration types for Node.js implementation
+ */
+
+import type { SerialPort } from "serialport";
+
+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[];
+    velocity_max: number[];
+    torque_max: number[];
+  };
+}
+
+export interface CalibrationResults {
+  [motorName: string]: {
+    id: number;
+    drive_mode: number;
+    homing_offset: number;
+    range_min: number;
+    range_max: number;
+  };
+}

src/lerobot/node/{robots/config.ts → types/robot-config.ts}

@@ -1,10 +1,7 @@
 /**
- * Robot configuration types
- * Shared between Node.js and Web implementations
+ * Robot configuration types for Node.js implementation
  */
 
-import type { TeleoperatorConfig } from "../teleoperators/config.js";
-
 export interface RobotConfig {
   type: "so100_follower";
   port: string;
@@ -20,3 +17,7 @@ export interface CalibrateConfig {
   robot?: RobotConfig;
   teleop?: TeleoperatorConfig;
 }
+
+// Re-export from teleoperator-config for convenience
+import type { TeleoperatorConfig } from "./teleoperator-config.js";
+export type { TeleoperatorConfig };

src/lerobot/node/types/teleoperation.ts

@@ -0,0 +1,17 @@
+/**
+ * Teleoperation types for Node.js implementation
+ */
+
+import type { RobotConfig } from "./robot-config.js";
+
+export interface TeleoperateConfig {
+  robot: RobotConfig;
+  teleop: KeyboardTeleoperationConfig;
+  fps?: number; // Default: 60
+  step_size?: number; // Default: 10 (motor position units)
+  duration_s?: number | null; // Default: null (infinite)
+}
+
+export interface KeyboardTeleoperationConfig {
+  type: "keyboard"; // Only keyboard for now, expandable later
+}

src/lerobot/node/{teleoperators/config.ts → types/teleoperator-config.ts}

@@ -1,6 +1,5 @@
 /**
- * Teleoperator configuration types
- * Shared between Node.js and Web implementations
+ * Teleoperator configuration types for Node.js implementation
  */
 
 export interface TeleoperatorConfig {

src/lerobot/node/utils/keyboard-teleop.ts

@@ -0,0 +1,284 @@
+/**
+ * Keyboard teleoperation controller for Node.js terminal
+ * Handles raw keyboard input and robot position control using the keypress package.
+ */
+
+import * as readline from "readline";
+import { SO100Follower } from "../robots/so100_follower.js";
+
+/**
+ * Keyboard controller for robot teleoperation
+ * Handles terminal keyboard input and robot position updates
+ */
+export class KeyboardController {
+  private robot: SO100Follower;
+  private stepSize: number;
+  private currentPositions: Record<string, number> = {};
+  private motorNames = [
+    "shoulder_pan",
+    "shoulder_lift",
+    "elbow_flex",
+    "wrist_flex",
+    "wrist_roll",
+    "gripper",
+  ];
+  private running = false;
+  private gripperState = false; // Toggle state for gripper
+
+  constructor(robot: SO100Follower, stepSize: number = 25) {
+    this.robot = robot;
+    this.stepSize = stepSize;
+  }
+
+  /**
+   * Start keyboard teleoperation
+   * Sets up raw keyboard input and initializes robot positions
+   */
+  async start(): Promise<void> {
+    console.log("Initializing keyboard controller...");
+
+    // Initialize current positions from robot
+    try {
+      this.currentPositions = await this.readRobotPositions();
+    } catch (error) {
+      console.warn(
+        "Could not read initial robot positions, using calibrated centers"
+      );
+      // Initialize with calibrated center positions if available, otherwise use middle positions
+      const calibratedLimits = this.robot.getCalibrationLimits();
+      this.motorNames.forEach((motor) => {
+        const limits = calibratedLimits[motor];
+        const centerPosition = limits
+          ? Math.floor((limits.min + limits.max) / 2)
+          : 2047;
+        this.currentPositions[motor] = centerPosition;
+      });
+    }
+
+    // Set up raw keyboard input
+    this.setupKeyboardInput();
+    this.running = true;
+
+    console.log("Keyboard controller ready. Use controls to move robot.");
+  }
+
+  /**
+   * Stop keyboard teleoperation
+   * Cleans up keyboard input handling
+   */
+  async stop(): Promise<void> {
+    this.running = false;
+
+    // Reset terminal to normal mode
+    if (process.stdin.setRawMode) {
+      process.stdin.setRawMode(false);
+    }
+    process.stdin.removeAllListeners("keypress");
+
+    console.log("Keyboard controller stopped.");
+  }
+
+  /**
+   * Get current robot positions
+   */
+  async getCurrentPositions(): Promise<Record<string, number>> {
+    return { ...this.currentPositions };
+  }
+
+  /**
+   * Set up keyboard input handling
+   * Uses readline for cross-platform keyboard input
+   */
+  private setupKeyboardInput(): void {
+    // Set up raw mode for immediate key response
+    if (process.stdin.setRawMode) {
+      process.stdin.setRawMode(true);
+    }
+    process.stdin.resume();
+    process.stdin.setEncoding("utf8");
+
+    // Handle keyboard input
+    process.stdin.on("data", (key: string) => {
+      if (!this.running) return;
+
+      this.handleKeyPress(key);
+    });
+  }
+
+  /**
+   * Handle individual key presses
+   * Maps keys to robot motor movements
+   */
+  private async handleKeyPress(key: string): Promise<void> {
+    let positionChanged = false;
+    const newPositions = { ...this.currentPositions };
+
+    // Handle arrow keys first (they start with ESC but are multi-byte sequences)
+    if (key.startsWith("\u001b[")) {
+      const arrowKey = key.slice(2);
+      switch (arrowKey) {
+        case "A": // Up arrow
+          newPositions.shoulder_lift += this.stepSize;
+          positionChanged = true;
+          break;
+        case "B": // Down arrow
+          newPositions.shoulder_lift -= this.stepSize;
+          positionChanged = true;
+          break;
+        case "C": // Right arrow
+          newPositions.shoulder_pan += this.stepSize;
+          positionChanged = true;
+          break;
+        case "D": // Left arrow
+          newPositions.shoulder_pan -= this.stepSize;
+          positionChanged = true;
+          break;
+      }
+    } else {
+      // Handle single character keys
+      const keyCode = key.charCodeAt(0);
+
+      switch (keyCode) {
+        // Standalone ESC key (emergency stop)
+        case 27:
+          if (key.length === 1) {
+            console.log("\n🛑 EMERGENCY STOP!");
+            await this.emergencyStop();
+            return;
+          }
+          break;
+
+        // Regular character keys
+        case 119: // 'w'
+          newPositions.elbow_flex += this.stepSize;
+          positionChanged = true;
+          break;
+        case 115: // 's'
+          newPositions.elbow_flex -= this.stepSize;
+          positionChanged = true;
+          break;
+        case 97: // 'a'
+          newPositions.wrist_flex -= this.stepSize;
+          positionChanged = true;
+          break;
+        case 100: // 'd'
+          newPositions.wrist_flex += this.stepSize;
+          positionChanged = true;
+          break;
+        case 113: // 'q'
+          newPositions.wrist_roll -= this.stepSize;
+          positionChanged = true;
+          break;
+        case 101: // 'e'
+          newPositions.wrist_roll += this.stepSize;
+          positionChanged = true;
+          break;
+        case 32: // Space
+          // Toggle gripper
+          this.gripperState = !this.gripperState;
+          newPositions.gripper = this.gripperState ? 2300 : 1800;
+          positionChanged = true;
+          break;
+
+        // Ctrl+C
+        case 3:
+          console.log("\nExiting...");
+          process.exit(0);
+      }
+    }
+
+    if (positionChanged) {
+      // Apply position limits using calibration
+      this.enforcePositionLimits(newPositions);
+
+      // Update robot positions - only send changed motors for better performance
+      try {
+        await this.writeRobotPositions(newPositions);
+        this.currentPositions = newPositions;
+      } catch (error) {
+        console.warn(
+          `Failed to update robot positions: ${
+            error instanceof Error ? error.message : error
+          }`
+        );
+      }
+    }
+  }
+
+  /**
+   * Read current positions from robot
+   * Uses SO100Follower position reading methods
+   */
+  private async readRobotPositions(): Promise<Record<string, number>> {
+    try {
+      return await this.robot.getMotorPositions();
+    } catch (error) {
+      console.warn(
+        `Failed to read robot positions: ${
+          error instanceof Error ? error.message : error
+        }`
+      );
+      // Return default positions as fallback
+      const positions: Record<string, number> = {};
+      this.motorNames.forEach((motor, index) => {
+        positions[motor] = 2047; // STS3215 middle position
+      });
+      return positions;
+    }
+  }
+
+  /**
+   * Write positions to robot - optimized to only send changed motors
+   * This was the key to the smooth performance in the working version
+   */
+  private async writeRobotPositions(
+    newPositions: Record<string, number>
+  ): Promise<void> {
+    // Only send commands for motors that actually changed
+    const changedPositions: Record<string, number> = {};
+    let hasChanges = false;
+
+    for (const [motor, newPosition] of Object.entries(newPositions)) {
+      if (Math.abs(this.currentPositions[motor] - newPosition) > 0.5) {
+        changedPositions[motor] = newPosition;
+        hasChanges = true;
+      }
+    }
+
+    if (hasChanges) {
+      await this.robot.setMotorPositions(changedPositions);
+    }
+  }
+
+  /**
+   * Enforce position limits based on calibration data
+   * Uses actual calibrated limits instead of hardcoded defaults
+   */
+  private enforcePositionLimits(positions: Record<string, number>): void {
+    // Get calibrated limits from robot
+    const calibratedLimits = this.robot.getCalibrationLimits();
+
+    for (const [motor, position] of Object.entries(positions)) {
+      const limits = calibratedLimits[motor];
+      if (limits) {
+        positions[motor] = Math.max(limits.min, Math.min(limits.max, position));
+      }
+    }
+  }
+
+  /**
+   * Emergency stop - halt all robot movement
+   */
+  private async emergencyStop(): Promise<void> {
+    try {
+      // Stop all robot movement
+      // TODO: Implement emergency stop in SO100Follower
+      console.log("Emergency stop executed.");
+      await this.stop();
+      process.exit(0);
+    } catch (error) {
+      console.error("Emergency stop failed:", error);
+      process.exit(1);
+    }
+  }
+}