tangger/lerobot
1
0
Fork 1
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

WIP

Browse Source
This commit is contained in:
Remi Cadene
2024-09-25 11:31:14 +02:00
parent 8b51a20b56
commit 62651cb2a9
3 changed files with 22 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
lerobot/common/robot_devices/motors/feetech.py
View File

@@ -92,6 +92,7 @@ SCS_SERIES_CONTROL_TABLE = {
"Status": (65, 1), "Status": (65, 1),
"Moving": (66, 1), "Moving": (66, 1),
"Present_Current": (69, 2), "Present_Current": (69, 2),
"Maximum_Acceleration": (85, 2),
} }
SCS_SERIES_BAUDRATE_TABLE = { SCS_SERIES_BAUDRATE_TABLE = {
@@ -124,8 +125,8 @@ MODEL_BAUDRATE_TABLE = {
"sts3215": SCS_SERIES_BAUDRATE_TABLE, "sts3215": SCS_SERIES_BAUDRATE_TABLE,
} }
NUM_READ_RETRY = 10 NUM_READ_RETRY = 2
NUM_WRITE_RETRY = 10 NUM_WRITE_RETRY = 2
def convert_degrees_to_steps(degrees: float | np.ndarray, models: str | list[str]) -> np.ndarray: def convert_degrees_to_steps(degrees: float | np.ndarray, models: str | list[str]) -> np.ndarray:
@@ -392,7 +393,7 @@ class FeetechMotorsBus:
def set_calibration(self, calibration: dict[str, list]): def set_calibration(self, calibration: dict[str, list]):
pass pass
#self.calibration = calibration # self.calibration = calibration
def apply_calibration_autocorrect(self, values: np.ndarray | list, motor_names: list[str] | None): def apply_calibration_autocorrect(self, values: np.ndarray | list, motor_names: list[str] | None):
"""This function apply the calibration, automatically detects out of range errors for motors values and attempt to correct. """This function apply the calibration, automatically detects out of range errors for motors values and attempt to correct.
@@ -621,7 +622,7 @@ class FeetechMotorsBus:
if data_name not in self.track_positions: if data_name not in self.track_positions:
self.track_positions[data_name] = { self.track_positions[data_name] = {
"prev": [None] * len(self.motor_names), "prev": [None] * len(self.motor_names),
# Assume False at initialization # Assume False at initialization
"below_zero": [False] * len(self.motor_names), "below_zero": [False] * len(self.motor_names),
"above_max": [False] * len(self.motor_names), "above_max": [False] * len(self.motor_names),
} }
@@ -640,7 +641,6 @@ class FeetechMotorsBus:
# Detect a full rotation occured # Detect a full rotation occured
if abs(track["prev"][idx] - values[i]) > 2048: if abs(track["prev"][idx] - values[i]) > 2048:
# Position went below 0 and got reset to 4095 # Position went below 0 and got reset to 4095
if track["prev"][idx] < values[i]: if track["prev"][idx] < values[i]:
# So we set negative value by adding a full rotation # So we set negative value by adding a full rotation

12
lerobot/common/robot_devices/robots/manipulator.py
View File

@@ -406,7 +406,6 @@ class ManipulatorRobot:
for name in self.leader_arms: for name in self.leader_arms:
self.leader_arms[name].read("Present_Position") self.leader_arms[name].read("Present_Position")
# Connect the cameras # Connect the cameras
for name in self.cameras: for name in self.cameras:
self.cameras[name].connect() self.cameras[name].connect()
@@ -536,12 +535,14 @@ class ManipulatorRobot:
def set_so_100_robot_preset(self): def set_so_100_robot_preset(self):
for name in self.follower_arms: for name in self.follower_arms:
self.follower_arms[name].write("Mode", 0) self.follower_arms[name].write("Mode", 0)
#self.follower_arms[name].write("P_Coefficient", 255, "shoulder_pan") # self.follower_arms[name].write("P_Coefficient", 255, "shoulder_pan")
self.follower_arms[name].write("P_Coefficient", 32, "shoulder_pan") self.follower_arms[name].write("P_Coefficient", 32, "shoulder_pan")
#self.follower_arms[name].write("D_Coefficient", 230, "shoulder_pan") # self.follower_arms[name].write("D_Coefficient", 230, "shoulder_pan")
self.follower_arms[name].write("D_Coefficient", 32, "shoulder_pan") self.follower_arms[name].write("D_Coefficient", 32, "shoulder_pan")
self.follower_arms[name].write("Acceleration", 254) #self.follower_arms[name].write("Acceleration", 254)
self.follower_arms[name].write("Minimum_Startup_Force", 16) #self.follower_arms[name].write("Minimum_Startup_Force", 16)
self.follower_arms[name].write("Lock", 0)
self.follower_arms[name].write("Maximum_Acceleration", 250)
# for name in self.leader_arms: # for name in self.leader_arms:
# self.leader_arms[name].write("Max_Torque_Limit", 50, "gripper") # self.leader_arms[name].write("Max_Torque_Limit", 50, "gripper")
@@ -549,7 +550,6 @@ class ManipulatorRobot:
# self.leader_arms[name].write("Torque_Enable", 1, "gripper") # self.leader_arms[name].write("Torque_Enable", 1, "gripper")
# self.leader_arms[name].write("Goal_Position", 2048, "gripper") # self.leader_arms[name].write("Goal_Position", 2048, "gripper")
def teleop_step( def teleop_step(
self, record_data=False self, record_data=False
) -> None | tuple[dict[str, torch.Tensor], dict[str, torch.Tensor]]: ) -> None | tuple[dict[str, torch.Tensor], dict[str, torch.Tensor]]:

25
lerobot/scripts/configure_motor.py
View File

@@ -13,21 +13,22 @@ python lerobot/scripts/configure_motor.py \
""" """
import argparse import argparse
import importlib
import time import time
def configure_motor(port, brand, model, motor_idx_des, baudrate_des): def configure_motor(port, brand, model, motor_idx_des, baudrate_des):
if brand == "feetech": if brand == "feetech":
from lerobot.common.robot_devices.motors.feetech import FeetechMotorsBus as motor_bus_class
from lerobot.common.robot_devices.motors.feetech import SCS_SERIES_BAUDRATE_TABLE as baudrate_table
from lerobot.common.robot_devices.motors.feetech import NUM_WRITE_RETRY as num_write_retry
from lerobot.common.robot_devices.motors.feetech import MODEL_BAUDRATE_TABLE as model_baud_rate_table from lerobot.common.robot_devices.motors.feetech import MODEL_BAUDRATE_TABLE as model_baud_rate_table
from lerobot.common.robot_devices.motors.feetech import NUM_WRITE_RETRY as num_write_retry
from lerobot.common.robot_devices.motors.feetech import SCS_SERIES_BAUDRATE_TABLE as baudrate_table
from lerobot.common.robot_devices.motors.feetech import FeetechMotorsBus as motor_bus_class
elif brand == "dynamixel": elif brand == "dynamixel":
from lerobot.common.robot_devices.motors.dynamixel import DynamixelMotorsBus as motor_bus_class from lerobot.common.robot_devices.motors.dynamixel import (
from lerobot.common.robot_devices.motors.dynamixel import X_SERIES_BAUDRATE_TABLE as baudrate_table MODEL_BAUDRATE_TABLE as model_baud_rate_table,
)
from lerobot.common.robot_devices.motors.dynamixel import NUM_WRITE_RETRY as num_write_retry from lerobot.common.robot_devices.motors.dynamixel import NUM_WRITE_RETRY as num_write_retry
from lerobot.common.robot_devices.motors.dynamixel import MODEL_BAUDRATE_TABLE as model_baud_rate_table from lerobot.common.robot_devices.motors.dynamixel import X_SERIES_BAUDRATE_TABLE as baudrate_table
from lerobot.common.robot_devices.motors.dynamixel import DynamixelMotorsBus as motor_bus_class
else: else:
raise ValueError( raise ValueError(
f"Currently we do not support this motor brand: {brand}. We currently support feetech and dynamixel motors." f"Currently we do not support this motor brand: {brand}. We currently support feetech and dynamixel motors."
@@ -45,9 +46,7 @@ def configure_motor(port, brand, model, motor_idx_des, baudrate_des):
motor_model = model # Use the motor model passed via argument motor_model = model # Use the motor model passed via argument
# Initialize the MotorBus with the correct port and motor configurations # Initialize the MotorBus with the correct port and motor configurations
motor_bus = motor_bus_class( motor_bus = motor_bus_class(port=port, motors={motor_name: (motor_index_arbitrary, motor_model)})
port=port, motors={motor_name: (motor_index_arbitrary, motor_model)}
)
# Try to connect to the motor bus and handle any connection-specific errors # Try to connect to the motor bus and handle any connection-specific errors
try: try:
@@ -65,7 +64,7 @@ def configure_motor(port, brand, model, motor_idx_des, baudrate_des):
for baudrate in all_baudrates: for baudrate in all_baudrates:
motor_bus.set_bus_baudrate(baudrate) motor_bus.set_bus_baudrate(baudrate)
present_ids = motor_bus.find_motor_indices(list(range(1,10))) present_ids = motor_bus.find_motor_indices(list(range(1, 10)))
if len(present_ids) > 1: if len(present_ids) > 1:
raise ValueError( raise ValueError(
"Error: More than one motor ID detected. This script is designed to only handle one motor at a time. Please disconnect all but one motor." "Error: More than one motor ID detected. This script is designed to only handle one motor at a time. Please disconnect all but one motor."
@@ -107,7 +106,6 @@ def configure_motor(port, brand, model, motor_idx_des, baudrate_des):
if present_baudrate_idx != baudrate_idx: if present_baudrate_idx != baudrate_idx:
raise OSError("Failed to write baudrate.") raise OSError("Failed to write baudrate.")
motor_bus.write_with_motor_ids(motor_bus.motor_models, motor_index, "Lock", 0) motor_bus.write_with_motor_ids(motor_bus.motor_models, motor_index, "Lock", 0)
print(f"Setting its index to desired index {motor_idx_des}") print(f"Setting its index to desired index {motor_idx_des}")
@@ -116,7 +114,7 @@ def configure_motor(port, brand, model, motor_idx_des, baudrate_des):
present_idx = motor_bus.read_with_motor_ids(motor_bus.motor_models, motor_idx_des, "ID") present_idx = motor_bus.read_with_motor_ids(motor_bus.motor_models, motor_idx_des, "ID")
if present_idx != motor_idx_des: if present_idx != motor_idx_des:
raise OSError("Failed to write index.") raise OSError("Failed to write index.")
if brand == "feetech": if brand == "feetech":
motor_bus.write("Goal_Position", 2047) motor_bus.write("Goal_Position", 2047)
time.sleep(4) time.sleep(4)
@@ -125,7 +123,6 @@ def configure_motor(port, brand, model, motor_idx_des, baudrate_des):
breakpoint() breakpoint()
motor_bus.read("Present_Position") motor_bus.read("Present_Position")
except Exception as e: except Exception as e:
print(f"Error occurred during motor configuration: {e}") print(f"Error occurred during motor configuration: {e}")