Spaces:
Running
Running
File size: 4,433 Bytes
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 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 |
/**
* Base Robot class for Node.js platform
* Uses serialport package for serial communication
* Mirrors Python lerobot/common/robots/robot.py
*/
import { SerialPort } from "serialport";
import { mkdir, writeFile } from "fs/promises";
import { join } from "path";
import type { RobotConfig } from "./config.js";
import { getCalibrationDir, ROBOTS } from "../constants.js";
export abstract class Robot {
protected port: SerialPort | null = null;
protected config: RobotConfig;
protected calibrationDir: string;
protected calibrationPath: string;
protected name: string;
constructor(config: RobotConfig) {
this.config = config;
this.name = config.type;
// Determine calibration directory
// Mirrors Python: config.calibration_dir if config.calibration_dir else HF_LEROBOT_CALIBRATION / ROBOTS / self.name
this.calibrationDir =
config.calibration_dir || join(getCalibrationDir(), ROBOTS, this.name);
// Use robot ID or type as filename
const robotId = config.id || this.name;
this.calibrationPath = join(this.calibrationDir, `${robotId}.json`);
}
/**
* Connect to the robot
* Mirrors Python robot.connect()
*/
async connect(_calibrate: boolean = false): Promise<void> {
try {
this.port = new SerialPort({
path: this.config.port,
baudRate: 1000000, // Default baud rate for Feetech motors (SO-100) - matches Python lerobot
dataBits: 8, // 8 data bits - matches Python serial.EIGHTBITS
stopBits: 1, // 1 stop bit - matches Python default
parity: "none", // No parity - matches Python default
autoOpen: false,
});
// Open the port
await new Promise<void>((resolve, reject) => {
this.port!.open((error) => {
if (error) {
reject(
new Error(
`Failed to open port ${this.config.port}: ${error.message}`
)
);
} else {
resolve();
}
});
});
} catch (error) {
throw new Error(`Could not connect to robot on port ${this.config.port}`);
}
}
/**
* Calibrate the robot
* Must be implemented by subclasses
*/
abstract calibrate(): Promise<void>;
/**
* Disconnect from the robot
* Mirrors Python robot.disconnect()
*/
async disconnect(): Promise<void> {
if (this.port && this.port.isOpen) {
// Handle torque disable if configured
if (this.config.disable_torque_on_disconnect) {
await this.disableTorque();
}
await new Promise<void>((resolve) => {
this.port!.close(() => {
resolve();
});
});
this.port = null;
}
}
/**
* Save calibration data to JSON file
* Mirrors Python's configuration saving
*/
protected async saveCalibration(calibrationData: any): Promise<void> {
// Ensure calibration directory exists
await mkdir(this.calibrationDir, { recursive: true });
// Save calibration data as JSON
await writeFile(
this.calibrationPath,
JSON.stringify(calibrationData, null, 2)
);
console.log(`Configuration saved to: ${this.calibrationPath}`);
}
/**
* Send command to robot via serial port
*/
protected async sendCommand(command: string): Promise<void> {
if (!this.port || !this.port.isOpen) {
throw new Error("Robot not connected");
}
return new Promise<void>((resolve, reject) => {
this.port!.write(command, (error) => {
if (error) {
reject(new Error(`Failed to send command: ${error.message}`));
} else {
resolve();
}
});
});
}
/**
* Read data from robot
*/
protected async readData(timeout: number = 5000): Promise<Buffer> {
if (!this.port || !this.port.isOpen) {
throw new Error("Robot not connected");
}
return new Promise<Buffer>((resolve, reject) => {
const timer = setTimeout(() => {
reject(new Error("Read timeout"));
}, timeout);
this.port!.once("data", (data: Buffer) => {
clearTimeout(timer);
resolve(data);
});
});
}
/**
* Disable torque on disconnect (SO-100 specific)
*/
protected async disableTorque(): Promise<void> {
try {
await this.sendCommand("TORQUE_DISABLE\r\n");
} catch (error) {
console.warn("Warning: Could not disable torque on disconnect");
}
}
}
|