feat: add scan_motors.py tool for troubleshooting
Signed-off-by: ivelin <ivelin117@gmail.com>
This commit is contained in:
ivelin
parent
8c0ad754db
commit
0825e2e14c
|
|
@ -153,3 +153,6 @@ dmypy.json
|
|||
|
||||
# Cython debug symbols
|
||||
cython_debug/
|
||||
|
||||
# lerobot calibration cache
|
||||
.cache
|
||||
|
|
|
|||
|
|
@ -140,10 +140,54 @@ Try to avoid rotating the motor while doing so to keep position 2048 set during
|
|||
|
||||
Follow step 4 of the [assembly video](https://youtu.be/FioA2oeFZ5I?t=610). The first arm should take a bit more than 1 hour to assemble, but once you get use to it, you can do it under 1 hour for the second arm.
|
||||
|
||||
|
||||
### c. Troubleshooting
|
||||
|
||||
Sometimes during assembly, the cables connecting the motors or the power adapter cable may be accidentally disconnected. To be sure that the motors are properly connected and functioning after assembly, use the scan_motors tool to test each arm.
|
||||
The output should look similar to the example below. If the list of motor IDs is shorter than 6, there is probably a poorly connected cable or a motor is misconfigured.
|
||||
|
||||
```bash
|
||||
lerobot$ python lerobot/scripts/scan_motors.py --port /dev/ttyACM0 --brand feetech --model sts3215
|
||||
Connected on port /dev/ttyACM0
|
||||
Scanning all baudrates and motor indices
|
||||
100%|████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████| 9/9 [00:00<00:00, 43.11it/s]
|
||||
6 motor ID(s) detected for baud rate 1000000. Motor IDs: [1, 2, 3, 4, 5, 6].
|
||||
Present Position 1997
|
||||
Offset 0
|
||||
Present Position 2098
|
||||
Offset 0
|
||||
Present Position 1124
|
||||
Offset 0
|
||||
Present Position 253
|
||||
Offset 0
|
||||
Present Position 3769
|
||||
Offset 0
|
||||
Present Position 926
|
||||
Offset 0
|
||||
Setting bus baud rate to 128000. Previously 1000000.
|
||||
100%|████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████| 9/9 [00:00<00:00, 14.42it/s]
|
||||
Setting bus baud rate to 500000. Previously 128000.
|
||||
100%|████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████| 9/9 [00:00<00:00, 14.50it/s]
|
||||
Setting bus baud rate to 115200. Previously 500000.
|
||||
100%|████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████| 9/9 [00:00<00:00, 14.40it/s]
|
||||
Setting bus baud rate to 57600. Previously 115200.
|
||||
100%|████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████| 9/9 [00:00<00:00, 14.08it/s]
|
||||
Setting bus baud rate to 38400. Previously 57600.
|
||||
100%|████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████| 9/9 [00:00<00:00, 13.88it/s]
|
||||
Setting bus baud rate to 19200. Previously 38400.
|
||||
100%|████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████| 9/9 [00:00<00:00, 13.24it/s]
|
||||
Setting bus baud rate to 250000. Previously 19200.
|
||||
100%|████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████| 9/9 [00:00<00:00, 14.47it/s]
|
||||
Scan finished.
|
||||
Disconnected from motor bus.
|
||||
```
|
||||
|
||||
|
||||
## E. Calibrate
|
||||
|
||||
Next, you'll need to calibrate your SO-100 robot to ensure that the leader and follower arms have the same position values when they are in the same physical position. This calibration is essential because it allows a neural network trained on one SO-100 robot to work on another.
|
||||
|
||||
|
||||
#### a. Manual calibration of follower arm
|
||||
/!\ Contrarily to step 6 of the [assembly video](https://youtu.be/FioA2oeFZ5I?t=724) which illustrates the auto calibration, we will actually do manual calibration of follower for now.
|
||||
|
||||
|
|
@ -160,6 +204,7 @@ python lerobot/scripts/control_robot.py calibrate \
|
|||
--robot-overrides '~cameras' --arms main_follower
|
||||
```
|
||||
|
||||
|
||||
#### b. Manual calibration of leader arm
|
||||
Follow step 6 of the [assembly video](https://youtu.be/FioA2oeFZ5I?t=724) which illustrates the manual calibration. You will need to move the leader arm to these positions sequentially:
|
||||
|
||||
|
|
@ -174,6 +219,7 @@ python lerobot/scripts/control_robot.py calibrate \
|
|||
--robot-overrides '~cameras' --arms main_leader
|
||||
```
|
||||
|
||||
|
||||
## F. Teleoperate
|
||||
|
||||
**Simple teleop**
|
||||
|
|
|
|||
|
|
@ -0,0 +1,97 @@
|
|||
"""
|
||||
This script is helpful to diagnose issues with motors during calibration or later operation.
|
||||
It scans all motors on a given port and displays their settings.
|
||||
|
||||
Example of usage:
|
||||
```bash
|
||||
python lerobot/scripts/scan_motors.py \
|
||||
--port /dev/tty.usbmodem585A0080521 \
|
||||
--brand feetech \
|
||||
--model sts3215 \
|
||||
```
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import time
|
||||
|
||||
|
||||
def scan_motors(port, brand, model):
|
||||
if brand == "feetech":
|
||||
from lerobot.common.robot_devices.motors.feetech import MODEL_BAUDRATE_TABLE
|
||||
from lerobot.common.robot_devices.motors.feetech import (
|
||||
SCS_SERIES_BAUDRATE_TABLE as SERIES_BAUDRATE_TABLE,
|
||||
)
|
||||
from lerobot.common.robot_devices.motors.feetech import FeetechMotorsBus as MotorsBusClass
|
||||
elif brand == "dynamixel":
|
||||
from lerobot.common.robot_devices.motors.dynamixel import MODEL_BAUDRATE_TABLE
|
||||
from lerobot.common.robot_devices.motors.dynamixel import (
|
||||
X_SERIES_BAUDRATE_TABLE as SERIES_BAUDRATE_TABLE,
|
||||
)
|
||||
from lerobot.common.robot_devices.motors.dynamixel import DynamixelMotorsBus as MotorsBusClass
|
||||
else:
|
||||
raise ValueError(
|
||||
f"Currently we do not support this motor brand: {brand}. We currently support feetech and dynamixel motors."
|
||||
)
|
||||
|
||||
# Check if the provided model exists in the model_baud_rate_table
|
||||
if model not in MODEL_BAUDRATE_TABLE:
|
||||
raise ValueError(
|
||||
f"Invalid model '{model}' for brand '{brand}'. Supported models: {list(MODEL_BAUDRATE_TABLE.keys())}"
|
||||
)
|
||||
|
||||
# Setup motor names, indices, and models
|
||||
motor_name = "motor"
|
||||
motor_index_arbitrary = -1 # Use an arbitrary out of range motor ID
|
||||
motor_model = model # Use the motor model passed via argument
|
||||
|
||||
# Initialize the MotorBus with the correct port and motor configurations
|
||||
motor_bus = MotorsBusClass(port=port, motors={motor_name: (motor_index_arbitrary, motor_model)})
|
||||
|
||||
# Try to connect to the motor bus and handle any connection-specific errors
|
||||
try:
|
||||
motor_bus.connect()
|
||||
print(f"Connected on port {motor_bus.port}")
|
||||
except OSError as e:
|
||||
print(f"Error occurred when connecting to the motor bus: {e}")
|
||||
return
|
||||
|
||||
# Motor bus is connected, proceed with the rest of the operations
|
||||
try:
|
||||
print("Scanning all baudrates and motor indices")
|
||||
all_baudrates = set(SERIES_BAUDRATE_TABLE.values())
|
||||
motors_detected = False
|
||||
|
||||
for baudrate in all_baudrates:
|
||||
motor_bus.set_bus_baudrate(baudrate)
|
||||
present_ids = motor_bus.find_motor_indices(list(range(1, 10)))
|
||||
if len(present_ids) > 0:
|
||||
print(f"{len(present_ids)} motor ID(s) detected for baud rate {baudrate}. Motor IDs: {present_ids}.")
|
||||
motors_detected = True
|
||||
|
||||
for motor_idx in present_ids:
|
||||
present_idx = motor_bus.read_with_motor_ids(motor_bus.motor_models, motor_idx, "ID", num_retry=2)
|
||||
if present_idx != motor_idx:
|
||||
raise OSError(f"Failed to access motor index {motor_idx}.")
|
||||
|
||||
if brand == "feetech":
|
||||
print("Present Position", motor_bus.read_with_motor_ids(motor_bus.motor_models, motor_idx, "Present_Position", num_retry=2))
|
||||
print("Offset", motor_bus.read_with_motor_ids(motor_bus.motor_models, motor_idx, "Offset", num_retry=2))
|
||||
|
||||
if not motors_detected:
|
||||
print("No motors detected.")
|
||||
|
||||
print("Scan finished.")
|
||||
|
||||
finally:
|
||||
motor_bus.disconnect()
|
||||
print("Disconnected from motor bus.")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--port", type=str, required=True, help="Motors bus port (e.g. dynamixel,feetech)")
|
||||
parser.add_argument("--brand", type=str, required=True, help="Motor brand (e.g. dynamixel,feetech)")
|
||||
parser.add_argument("--model", type=str, required=True, help="Motor model (e.g. xl330-m077,sts3215)")
|
||||
args = parser.parse_args()
|
||||
|
||||
scan_motors(args.port, args.brand, args.model)
|
||||
Loading…
Reference in New Issue