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

Fix setup_motor & add it to robots

Browse Source
This commit is contained in:
Simon Alibert
2025-04-17 16:56:23 +02:00
parent bf1c737858
commit 702749b7d3
8 changed files with 51 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
lerobot/common/motors/dynamixel/dynamixel.py
View File

@@ -24,7 +24,7 @@ from enum import Enum
from lerobot.common.utils.encoding_utils import decode_twos_complement, encode_twos_complement
from ..motors_bus import Motor, MotorCalibration, MotorsBus, NameOrID, Value
from ..motors_bus import Motor, MotorCalibration, MotorsBus, NameOrID, Value, get_address
from .tables import (
AVAILABLE_BAUDRATES,
MODEL_BAUDRATE_TABLE,
@@ -198,6 +198,10 @@ class DynamixelMotorsBus(MotorsBus):
for name in self._get_motors_list(motors):
self.write("Torque_Enable", name, TorqueMode.DISABLED.value, num_retry=num_retry)
def _disable_torque(self, motor_id: int, model: str, num_retry: int = 0) -> None:
addr, length = get_address(self.model_ctrl_table, model, "Torque_Enable")
self._write(addr, length, motor_id, TorqueMode.DISABLED.value, num_retry=num_retry)
def enable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None:
for name in self._get_motors_list(motors):
self.write("Torque_Enable", name, TorqueMode.ENABLED.value, num_retry=num_retry)

8
lerobot/common/motors/feetech/feetech.py
View File

@@ -19,7 +19,7 @@ from pprint import pformat
from lerobot.common.utils.encoding_utils import decode_sign_magnitude, encode_sign_magnitude
from ..motors_bus import Motor, MotorCalibration, MotorsBus, NameOrID, Value
from ..motors_bus import Motor, MotorCalibration, MotorsBus, NameOrID, Value, get_address
from .tables import (
FIRMWARE_MAJOR_VERSION,
FIRMWARE_MINOR_VERSION,
@@ -280,6 +280,12 @@ class FeetechMotorsBus(MotorsBus):
self.write("Torque_Enable", name, TorqueMode.DISABLED.value, num_retry=num_retry)
self.write("Lock", name, 0, num_retry=num_retry)
def _disable_torque(self, motor_id: int, model: str, num_retry: int = 0) -> None:
addr, length = get_address(self.model_ctrl_table, model, "Torque_Enable")
self._write(addr, length, motor_id, TorqueMode.DISABLED.value, num_retry=num_retry)
addr, length = get_address(self.model_ctrl_table, model, "Lock")
self._write(addr, length, motor_id, 0, num_retry=num_retry)
def enable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None:
for name in self._get_motors_list(motors):
self.write("Torque_Enable", name, TorqueMode.ENABLED.value, num_retry=num_retry)

11
lerobot/common/motors/motors_bus.py
View File

@@ -464,13 +464,14 @@ class MotorsBus(abc.ABC):
model = self.motors[motor].model
target_id = self.motors[motor].id
self.set_baudrate(initial_baudrate)
self._disable_torque(initial_id, model)
# Set ID
addr, length = get_address(self.model_ctrl_table, "ID", model)
addr, length = get_address(self.model_ctrl_table, model, "ID")
self._write(addr, length, initial_id, target_id)
# Set Baudrate
addr, length = get_address(self.model_ctrl_table, "Baud_Rate", model)
addr, length = get_address(self.model_ctrl_table, model, "Baud_Rate")
baudrate_value = self.model_baudrate_table[model][self.default_baudrate]
self._write(addr, length, target_id, baudrate_value)
@@ -485,7 +486,11 @@ class MotorsBus(abc.ABC):
pass
@abc.abstractmethod
def disable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None:
def disable_torque(self, motors: int | str | list[str] | None = None, num_retry: int = 0) -> None:
pass
@abc.abstractmethod
def _disable_torque(self, motor: int, model: str, num_retry: int = 0) -> None:
pass
@abc.abstractmethod

8
lerobot/common/robots/koch/koch_follower.py
View File

@@ -117,7 +117,7 @@ class KochFollower(Robot):
for name in self.arm.names:
self.arm.write("Operating_Mode", name, OperatingMode.EXTENDED_POSITION.value)
input("Move robot to the middle of its range of motion and press ENTER....")
input(f"Move {self} to the middle of its range of motion and press ENTER....")
homing_offsets = self.arm.set_half_turn_homings()
full_turn_motors = ["shoulder_pan", "wrist_roll"]
@@ -169,6 +169,12 @@ class KochFollower(Robot):
self.arm.write("Position_I_Gain", "elbow_flex", 0)
self.arm.write("Position_D_Gain", "elbow_flex", 600)
def setup_motors(self) -> None:
for motor in reversed(self.arm.motors):
input(f"Connect the controller board to the '{motor}' motor only and press enter.")
self.arm.setup_motor(motor)
print(f"'{motor}' motor id set to {self.arm.motors[motor].id}")
def get_observation(self) -> dict[str, Any]:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")

8
lerobot/common/robots/so100/so100_follower.py
View File

@@ -116,7 +116,7 @@ class SO100Follower(Robot):
for name in self.arm.names:
self.arm.write("Operating_Mode", name, OperatingMode.POSITION.value)
input("Move robot to the middle of its range of motion and press ENTER....")
input(f"Move {self} to the middle of its range of motion and press ENTER....")
homing_offsets = self.arm.set_half_turn_homings()
full_turn_motor = "wrist_roll"
@@ -154,6 +154,12 @@ class SO100Follower(Robot):
self.arm.write("I_Coefficient", name, 0)
self.arm.write("D_Coefficient", name, 32)
def setup_motors(self) -> None:
for motor in reversed(self.arm.motors):
input(f"Connect the controller board to the '{motor}' motor only and press enter.")
self.arm.setup_motor(motor)
print(f"'{motor}' motor id set to {self.arm.motors[motor].id}")
def get_observation(self) -> dict[str, Any]:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")

8
lerobot/common/teleoperators/koch/koch_leader.py
View File

@@ -97,7 +97,7 @@ class KochLeader(Teleoperator):
self.arm.write("Drive_Mode", "elbow_flex", DriveMode.INVERTED.value)
drive_modes = {name: 1 if name == "elbow_flex" else 0 for name in self.arm.names}
input("Move robot to the middle of its range of motion and press ENTER....")
input(f"Move {self} to the middle of its range of motion and press ENTER....")
homing_offsets = self.arm.set_half_turn_homings()
full_turn_motors = ["shoulder_pan", "wrist_roll"]
@@ -146,6 +146,12 @@ class KochLeader(Teleoperator):
self.arm.enable_torque("gripper")
self.arm.write("Goal_Position", "gripper", self.config.gripper_open_pos)
def setup_motors(self) -> None:
for motor in reversed(self.arm.motors):
input(f"Connect the controller board to the '{motor}' motor only and press enter.")
self.arm.setup_motor(motor)
print(f"'{motor}' motor id set to {self.arm.motors[motor].id}")
def get_action(self) -> dict[str, float]:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")

8
lerobot/common/teleoperators/so100/so100_leader.py
View File

@@ -91,7 +91,7 @@ class SO100Leader(Teleoperator):
for name in self.arm.names:
self.arm.write("Operating_Mode", name, OperatingMode.POSITION.value)
input("Move robot to the middle of its range of motion and press ENTER....")
input(f"Move {self} to the middle of its range of motion and press ENTER....")
homing_offsets = self.arm.set_half_turn_homings()
full_turn_motor = "wrist_roll"
@@ -124,6 +124,12 @@ class SO100Leader(Teleoperator):
for name in self.arm.names:
self.arm.write("Operating_Mode", name, OperatingMode.POSITION.value)
def setup_motors(self) -> None:
for motor in reversed(self.arm.motors):
input(f"Connect the controller board to the '{motor}' motor only and press enter.")
self.arm.setup_motor(motor)
print(f"'{motor}' motor id set to {self.arm.motors[motor].id}")
def get_action(self) -> dict[str, float]:
start = time.perf_counter()
action = self.arm.sync_read("Present_Position")

5
tests/motors/test_motors_bus.py
View File

@@ -136,8 +136,9 @@ class MockMotorsBus(MotorsBus):
def configure_motors(self): ...
def read_calibration(self): ...
def write_calibration(self, calibration_dict): ...
def disable_torque(self, motors): ...
def enable_torque(self, motors): ...
def disable_torque(self, motors, num_retry): ...
def _disable_torque(self, motor, model, num_retry): ...
def enable_torque(self, motors, num_retry): ...
def _get_half_turn_homings(self, positions): ...
def _encode_sign(self, data_name, ids_values): ...
def _decode_sign(self, data_name, ids_values): ...