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

6
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:
@@ -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

8
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()
@@ -540,8 +539,10 @@ class ManipulatorRobot:
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]]:

21
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:
@@ -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}")
@@ -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}")