Spaces:
Running
Running
File size: 4,214 Bytes
130ae50 ba80dbe 130ae50 ba80dbe 130ae50 ba80dbe 130ae50 ba80dbe 130ae50 ba80dbe 130ae50 ba80dbe 130ae50 ba80dbe 130ae50 ba80dbe 130ae50 ba80dbe 130ae50 ba80dbe 130ae50 ba80dbe 130ae50 ba80dbe 130ae50 ba80dbe 130ae50 |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 |
/**
* SO-100 device configurations
* Defines the differences between leader and follower devices
* Mirrors Python lerobot device configuration approach
*/
import type { SO100CalibrationConfig } from "../types/calibration.js";
import { SerialPort } from "serialport";
/**
* Common motor names for all SO-100 devices
*/
const SO100_MOTOR_NAMES = [
"shoulder_pan",
"shoulder_lift",
"elbow_flex",
"wrist_flex",
"wrist_roll",
"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
* Drive modes match Python lerobot exactly: all motors use drive_mode=0
*/
export function createSO100FollowerConfig(
port: SerialPort
): SO100CalibrationConfig {
return {
deviceType: "so100_follower",
port,
motorNames: SO100_MOTOR_NAMES,
motorIds: SO100_MOTOR_IDS,
protocol: STS3215_PROTOCOL,
// Python lerobot uses drive_mode=0 for all motors (current format)
driveModes: [0, 0, 0, 0, 0, 0],
// Calibration modes (not used in current implementation, but kept for compatibility)
calibModes: ["DEGREE", "DEGREE", "DEGREE", "DEGREE", "DEGREE", "LINEAR"],
// 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],
torque_max: [50, 50, 50, 50, 25, 25],
},
};
}
/**
* SO-100 Leader Configuration
* Teleoperator arm that humans use to control the follower
* Drive modes match Python lerobot exactly: all motors use drive_mode=0
*/
export function createSO100LeaderConfig(
port: SerialPort
): SO100CalibrationConfig {
return {
deviceType: "so100_leader",
port,
motorNames: SO100_MOTOR_NAMES,
motorIds: SO100_MOTOR_IDS,
protocol: STS3215_PROTOCOL,
// 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 - 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],
torque_max: [30, 30, 30, 30, 20, 15],
},
};
}
/**
* Get configuration for any SO-100 device type
*/
export function getSO100Config(
deviceType: "so100_follower" | "so100_leader",
port: SerialPort
): SO100CalibrationConfig {
switch (deviceType) {
case "so100_follower":
return createSO100FollowerConfig(port);
case "so100_leader":
return createSO100LeaderConfig(port);
default:
throw new Error(`Unknown SO-100 device type: ${deviceType}`);
}
}
|