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),
"Moving": (66, 1),
"Present_Current": (69, 2),
"Maximum_Acceleration": (85, 2),
}
SCS_SERIES_BAUDRATE_TABLE = {
@@ -124,8 +125,8 @@ MODEL_BAUDRATE_TABLE = {
"sts3215": SCS_SERIES_BAUDRATE_TABLE,
}
NUM_READ_RETRY = 10
NUM_WRITE_RETRY = 10
NUM_READ_RETRY = 2
NUM_WRITE_RETRY = 2
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]):
pass
#self.calibration = calibration
# self.calibration = calibration
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.
@@ -621,7 +622,7 @@ class FeetechMotorsBus:
if data_name not in self.track_positions:
self.track_positions[data_name] = {
"prev": [None] * len(self.motor_names),
# Assume False at initialization
# Assume False at initialization
"below_zero": [False] * len(self.motor_names),
"above_max": [False] * len(self.motor_names),
}
@@ -640,7 +641,6 @@ class FeetechMotorsBus:
# Detect a full rotation occured
if abs(track["prev"][idx] - values[i]) > 2048:
# Position went below 0 and got reset to 4095
if track["prev"][idx] < values[i]:
# 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:
self.leader_arms[name].read("Present_Position")
# Connect the cameras
for name in self.cameras:
self.cameras[name].connect()
@@ -536,12 +535,14 @@ class ManipulatorRobot:
def set_so_100_robot_preset(self):
for name in self.follower_arms:
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("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("Acceleration", 254)
self.follower_arms[name].write("Minimum_Startup_Force", 16)
#self.follower_arms[name].write("Acceleration", 254)
#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:
# 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("Goal_Position", 2048, "gripper")
def teleop_step(
self, record_data=False
) -> 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 importlib
import time
def configure_motor(port, brand, model, motor_idx_des, baudrate_des):
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 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":
from lerobot.common.robot_devices.motors.dynamixel import DynamixelMotorsBus as motor_bus_class
from lerobot.common.robot_devices.motors.dynamixel import X_SERIES_BAUDRATE_TABLE as baudrate_table
from lerobot.common.robot_devices.motors.dynamixel import (
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 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:
raise ValueError(
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
# Initialize the MotorBus with the correct port and motor configurations
motor_bus = motor_bus_class(
port=port, motors={motor_name: (motor_index_arbitrary, motor_model)}
)
motor_bus = motor_bus_class(port=port, motors={motor_name: (motor_index_arbitrary, motor_model)})
# Try to connect to the motor bus and handle any connection-specific errors
try:
@@ -65,7 +64,7 @@ def configure_motor(port, brand, model, motor_idx_des, baudrate_des):
for baudrate in all_baudrates:
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:
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."
@@ -107,7 +106,6 @@ def configure_motor(port, brand, model, motor_idx_des, baudrate_des):
if present_baudrate_idx != baudrate_idx:
raise OSError("Failed to write baudrate.")
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}")
@@ -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")
if present_idx != motor_idx_des:
raise OSError("Failed to write index.")
if brand == "feetech":
motor_bus.write("Goal_Position", 2047)
time.sleep(4)
@@ -125,7 +123,6 @@ def configure_motor(port, brand, model, motor_idx_des, baudrate_des):
breakpoint()
motor_bus.read("Present_Position")
except Exception as e:
print(f"Error occurred during motor configuration: {e}")