hangX/lerobot
1
0
Fork 0
forked from tangger/lerobot
Code Pull Requests Activity

WIP to remove

Browse Source
This commit is contained in:
Remi Cadene
2024-09-23 22:53:06 +02:00
parent 5aa2a88280
commit 8b51a20b56
5 changed files with 144 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -82,6 +82,7 @@ SCS_SERIES_CONTROL_TABLE = {
"Goal_Position": (42, 2),
"Goal_Time": (44, 2),
"Goal_Speed": (46, 2),
"Torque_Limit": (48, 2),
"Lock": (55, 1),
"Present_Position": (56, 2),
"Present_Speed": (58, 2),
@@ -296,6 +297,18 @@ class FeetechMotorsBus:
self.group_writers = {}
self.logs = {}
self.track_positions = {}
self.present_pos = {
"prev": [None] * len(self.motor_names),
"below_zero": [None] * len(self.motor_names),
"above_max": [None] * len(self.motor_names),
}
self.goal_pos = {
"prev": [None] * len(self.motor_names),
"below_zero": [None] * len(self.motor_names),
"above_max": [None] * len(self.motor_names),
}
def connect(self):
if self.is_connected:
raise RobotDeviceAlreadyConnectedError(
@@ -378,7 +391,8 @@ class FeetechMotorsBus:
return [idx for idx, _ in self.motors.values()]
def set_calibration(self, calibration: dict[str, list]):
self.calibration = calibration
pass
#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.
@@ -603,6 +617,44 @@ class FeetechMotorsBus:
values = np.round(values).astype(np.int32)
return values
def avoid_rotation_reset(self, values, motor_names, data_name):
if data_name not in self.track_positions:
self.track_positions[data_name] = {
"prev": [None] * len(self.motor_names),
# Assume False at initialization
"below_zero": [False] * len(self.motor_names),
"above_max": [False] * len(self.motor_names),
}
track = self.track_positions[data_name]
if motor_names is None:
motor_names = self.motor_names
for i, name in enumerate(motor_names):
idx = self.motor_names.index(name)
if track["prev"][idx] is None:
track["prev"][idx] = values[i]
continue
# 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
values[i] -= 4096
# Position went above 4095 and got reset to 0
elif track["prev"][idx] > values[i]:
# So we add a full rotation
values[i] += 4096
track["prev"][idx] = values[i]
return values
def read_with_motor_ids(self, motor_models, motor_ids, data_name):
return_list = True
if not isinstance(motor_ids, list):
@@ -615,7 +667,11 @@ class FeetechMotorsBus:
for idx in motor_ids:
group.addParam(idx)
comm = group.txRxPacket()
for _ in range(NUM_READ_RETRY):
comm = group.txRxPacket()
if comm == COMM_SUCCESS:
break
if comm != COMM_SUCCESS:
raise ConnectionError(
f"Read failed due to communication error on port {self.port_handler.port_name} for indices {motor_ids}: "
@@ -685,6 +741,11 @@ class FeetechMotorsBus:
if data_name in CONVERT_UINT32_TO_INT32_REQUIRED:
values = values.astype(np.int32)
print(values)
if data_name in CALIBRATION_REQUIRED:
values = self.avoid_rotation_reset(values, motor_names, data_name)
if data_name in CALIBRATION_REQUIRED and self.calibration is not None:
values = self.apply_calibration_autocorrect(values, motor_names)
@@ -737,6 +798,8 @@ class FeetechMotorsBus:
values = np.array(values)
print(values)
motor_ids = []
models = []
for name in motor_names:

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

@@ -49,9 +49,10 @@ def apply_drive_mode(position, drive_mode):
def compute_nearest_rounded_position(position, models):
delta_turn = convert_degrees_to_steps(ROTATED_POSITION_DEGREE, models)
nearest_pos = np.round(position.astype(float) / delta_turn) * delta_turn
return nearest_pos.astype(position.dtype)
# delta_turn = convert_degrees_to_steps(ROTATED_POSITION_DEGREE, models)
# nearest_pos = np.round(position.astype(float) / delta_turn) * delta_turn
# return nearest_pos.astype(position.dtype)
return position
def run_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
@@ -382,6 +383,8 @@ class ManipulatorRobot:
self.set_koch_robot_preset()
elif self.robot_type == "aloha":
self.set_aloha_robot_preset()
elif self.robot_type == "so_100":
self.set_so_100_robot_preset()
else:
warnings.warn(f"No preset found for robot type: {self.robot_type}", stacklevel=1)
@@ -397,6 +400,13 @@ class ManipulatorRobot:
self.leader_arms[name].write("Torque_Enable", 1, "gripper")
self.leader_arms[name].write("Goal_Position", self.config.gripper_open_degree, "gripper")
# Check both arms can be read
for name in self.follower_arms:
self.follower_arms[name].read("Present_Position")
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()
@@ -523,6 +533,23 @@ class ManipulatorRobot:
stacklevel=1,
)
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", 32, "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)
# for name in self.leader_arms:
# self.leader_arms[name].write("Max_Torque_Limit", 50, "gripper")
# self.leader_arms[name].write("Torque_Limit", 1000, "gripper")
# 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]]:

8
lerobot/configs/robot/so_100.yaml
View File

@@ -5,12 +5,12 @@ calibration_dir: .cache/calibration/so_100
# `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes.
# Set this to a positive scalar to have the same value for all motors, or a list that is the same length as
# the number of motors in your follower arms.
max_relative_target: 5
max_relative_target: null
leader_arms:
main:
_target_: lerobot.common.robot_devices.motors.feetech.FeetechMotorsBus
port: /dev/tty.usbmodem585A0080521
port: /dev/tty.usbmodem585A0077581
motors:
# name: (index, model)
shoulder_pan: [1, "sts3215"]
@@ -23,7 +23,7 @@ leader_arms:
follower_arms:
main:
_target_: lerobot.common.robot_devices.motors.feetech.FeetechMotorsBus
port: /dev/tty.usbmodem585A0080971
port: /dev/tty.usbmodem585A0080521
motors:
# name: (index, model)
shoulder_pan: [1, "sts3215"]
@@ -50,4 +50,4 @@ cameras:
# ~ Koch specific settings ~
# Sets the leader arm in torque mode with the gripper motor set to this angle. This makes it possible
# to squeeze the gripper and have it spring back to an open position on its own.
gripper_open_degree: 35.156
# gripper_open_degree: 35.156

82
lerobot/scripts/configure_motor.py
View File

@@ -1,35 +1,33 @@
"""
This script configure a single motor at a time to a given ID and baudrate.
Example of usage:
```bash
python lerobot/scripts/configure_motor.py \
--port /dev/tty.usbmodem585A0080521 \
--brand feetech \
--model sts3215 \
--baudrate 1000000 \
--ID 1
```
"""
import argparse
import importlib
import time
def configure_motor(brand, model, motor_idx_des, baudrate_des):
def configure_motor(port, brand, model, motor_idx_des, baudrate_des):
if brand == "feetech":
motor_bus_class = importlib.import_module(
"lerobot.common.robot_devices.motors.feetech"
).FeetechMotorsBus
baudrate_table = importlib.import_module(
"lerobot.common.robot_devices.motors.feetech"
).SCS_SERIES_BAUDRATE_TABLE
num_write_retry = importlib.import_module(
"lerobot.common.robot_devices.motors.feetech"
).NUM_WRITE_RETRY
model_baud_rate_table = importlib.import_module(
"lerobot.common.robot_devices.motors.feetech"
).MODEL_BAUDRATE_TABLE
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
elif brand == "dynamixel":
motor_bus_class = importlib.import_module(
"lerobot.common.robot_devices.motors.dynamixel"
).DynamixelMotorsBus
baudrate_table = importlib.import_module(
"lerobot.common.robot_devices.motors.dynamixel"
).X_SERIES_BAUDRATE_TABLE
num_write_retry = importlib.import_module(
"lerobot.common.robot_devices.motors.dynamixel"
).NUM_WRITE_RETRY
model_baud_rate_table = importlib.import_module(
"lerobot.common.robot_devices.motors.dynamixel"
).MODEL_BAUDRATE_TABLE
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 NUM_WRITE_RETRY as num_write_retry
from lerobot.common.robot_devices.motors.dynamixel import MODEL_BAUDRATE_TABLE as model_baud_rate_table
else:
raise ValueError(
f"Currently we do not support this motor brand: {brand}. We currently support feetech and dynamixel motors."
@@ -48,7 +46,7 @@ def configure_motor(brand, model, motor_idx_des, baudrate_des):
# Initialize the MotorBus with the correct port and motor configurations
motor_bus = motor_bus_class(
port="/dev/ttyACM0", 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
@@ -67,7 +65,7 @@ def configure_motor(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()
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."
@@ -109,36 +107,42 @@ def configure_motor(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}")
motor_bus.write_with_motor_ids(motor_bus.motor_models, motor_index, "ID", motor_idx_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)
motor_bus.write("Offset", 2047)
time.sleep(4)
breakpoint()
motor_bus.read("Present_Position")
except Exception as e:
print(f"Error occurred during motor configuration: {e}")
finally:
# Disconnect the motor bus
motor_bus.disconnect()
print("Disconnected from motor bus.")
if __name__ == "__main__":
# Set up the argument parser
parser = argparse.ArgumentParser(
description="This script is used to configure a single motor at a time to the ID and baudrate you desire."
)
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)")
parser.add_argument("--ID", type=int, required=True, help="Desired ID of the current motor (e.g., 1)")
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)")
parser.add_argument("--ID", type=int, required=True, help="Desired ID of the current motor (e.g. 1,2,3)")
parser.add_argument(
"--baudrate", type=int, default=1000000, help="Desired baudrate for the motor (default: 1000000)"
)
# Parse arguments
args = parser.parse_args()
# Call the configure_motor function with the parsed arguments
configure_motor(args.brand, args.model, args.ID, args.baudrate)
configure_motor(args.port, args.brand, args.model, args.ID, args.baudrate)

2
lerobot/scripts/control_robot.py
View File

@@ -445,6 +445,8 @@ def record(
# Using `with` to exist smoothly if an execption is raised.
futures = []
num_image_writers = num_image_writers_per_camera * len(robot.cameras)
if num_image_writers == 0:
num_image_writers = 1
with concurrent.futures.ThreadPoolExecutor(max_workers=num_image_writers) as executor:
# Start recording all episodes
while episode_index < num_episodes: