From a0657ee274c0c9fb196c24803b50d96f80533710 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Thu, 17 Apr 2025 14:21:12 +0200 Subject: [PATCH] refactor(robots): multiple changes from feedback --- examples/robots/lekiwi_client_app.py | 2 ++ lerobot/common/motors/dynamixel/tables.py | 21 +++++++++++++ lerobot/common/motors/feetech/tables.py | 20 +++++++++++++ lerobot/common/motors/motors_bus.py | 3 +- lerobot/common/robots/lekiwi/config_lekiwi.py | 6 ++++ lerobot/common/robots/lekiwi/lekiwi.py | 19 +++++++----- lerobot/common/robots/lekiwi/lekiwi_client.py | 30 +++++++++---------- lerobot/common/robots/lekiwi/lekiwi_host.py | 13 +++----- lerobot/common/robots/robot.py | 3 +- lerobot/common/robots/utils.py | 7 +++-- .../keyboard/configuration_keyboard.py | 2 +- .../teleoperators/keyboard/teleop_keyboard.py | 14 ++++----- lerobot/common/teleoperators/teleoperator.py | 3 -- lerobot/scripts/control_robot.py | 9 +++--- 14 files changed, 97 insertions(+), 55 deletions(-) diff --git a/examples/robots/lekiwi_client_app.py b/examples/robots/lekiwi_client_app.py index 620e4998..c87b4227 100755 --- a/examples/robots/lekiwi_client_app.py +++ b/examples/robots/lekiwi_client_app.py @@ -19,6 +19,7 @@ from lerobot.common.robots.lekiwi.lekiwi_client import LeKiwiClient from lerobot.common.teleoperators.keyboard import KeyboardTeleop, KeyboardTeleopConfig from lerobot.common.teleoperators.so100 import SO100Leader, SO100LeaderConfig +# TODO(Steven): Check validity of these features DUMMY_FEATURES = { "observation.state": { "dtype": "float64", @@ -118,6 +119,7 @@ def main(): # robot.set_mode(RobotMode.AUTO) # policy_action = policy.get_action() # This might be in body frame, key space or smt else # robot.send_action(policy_action) + action_sent = robot.send_action(action) observation = robot.get_observation() diff --git a/lerobot/common/motors/dynamixel/tables.py b/lerobot/common/motors/dynamixel/tables.py index 7cc179f6..e5194d94 100644 --- a/lerobot/common/motors/dynamixel/tables.py +++ b/lerobot/common/motors/dynamixel/tables.py @@ -1,3 +1,24 @@ +# TODO(Steven): Consider doing the following: +# from enum import Enum +# class MyControlTableKey(Enum): +# ID = "ID" +# GOAL_SPEED = "Goal_Speed" +# ... +# +# MY_CONTROL_TABLE ={ +# MyControlTableKey.ID.value: (5,1) +# MyControlTableKey.GOAL_SPEED.value: (46, 2) +# ... +# } +# This allows me do to: +# bus.write(MyControlTableKey.GOAL_SPEED, ...) +# Instead of: +# bus.write("Goal_Speed", ...) +# This is important for two reasons: +# 1. The linter will tell me if I'm trying to use an invalid key, instead of me realizing when I get the RunTimeError +# 2. We can change the value of the MyControlTableKey enums without impacting the client code + + # {data_name: (address, size_byte)} # https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#control-table X_SERIES_CONTROL_TABLE = { diff --git a/lerobot/common/motors/feetech/tables.py b/lerobot/common/motors/feetech/tables.py index 7946fdaf..9b794ba4 100644 --- a/lerobot/common/motors/feetech/tables.py +++ b/lerobot/common/motors/feetech/tables.py @@ -2,6 +2,26 @@ FIRMWARE_MAJOR_VERSION = (0, 1) FIRMWARE_MINOR_VERSION = (1, 1) MODEL_NUMBER = (3, 2) +# TODO(Steven): Consider doing the following: +# from enum import Enum +# class MyControlTableKey(Enum): +# ID = "ID" +# GOAL_SPEED = "Goal_Speed" +# ... +# +# MY_CONTROL_TABLE ={ +# MyControlTableKey.ID.value: (5,1) +# MyControlTableKey.GOAL_SPEED.value: (46, 2) +# ... +# } +# This allows me do to: +# bus.write(MyControlTableKey.GOAL_SPEED, ...) +# Instead of: +# bus.write("Goal_Speed", ...) +# This is important for two reasons: +# 1. The linter will tell me if I'm trying to use an invalid key, instead of me realizing when I get the RunTimeError +# 2. We can change the value of the MyControlTableKey enums without impacting the client code + # data_name: (address, size_byte) # http://doc.feetech.cn/#/prodinfodownload?srcType=FT-SMS-STS-emanual-229f4476422d4059abfb1cb0 STS_SMS_SERIES_CONTROL_TABLE = { diff --git a/lerobot/common/motors/motors_bus.py b/lerobot/common/motors/motors_bus.py index 47ea8c51..ff847b31 100644 --- a/lerobot/common/motors/motors_bus.py +++ b/lerobot/common/motors/motors_bus.py @@ -632,7 +632,8 @@ class MotorsBus(abc.ABC): min_ = self.calibration[name].range_min max_ = self.calibration[name].range_max bounded_val = min(max_, max(min_, val)) - # TODO(Steven): normalization can go boom if max_ == min_, we should add a check probably in record_ranges_of_motions (which probably indicates the user forgot to move a motor) + # TODO(Steven): normalization can go boom if max_ == min_, we should add a check probably in record_ranges_of_motions + # (which probably indicates the user forgot to move a motor, most likely a gripper-like one) if self.motors[name].norm_mode is MotorNormMode.RANGE_M100_100: normalized_values[id_] = (((bounded_val - min_) / (max_ - min_)) * 200) - 100 elif self.motors[name].norm_mode is MotorNormMode.RANGE_0_100: diff --git a/lerobot/common/robots/lekiwi/config_lekiwi.py b/lerobot/common/robots/lekiwi/config_lekiwi.py index bed92ef0..f2df0daa 100644 --- a/lerobot/common/robots/lekiwi/config_lekiwi.py +++ b/lerobot/common/robots/lekiwi/config_lekiwi.py @@ -27,6 +27,8 @@ class RobotMode(enum.Enum): # TODO(Steven): Consider sending config at initial step over a socket +# However, this isn't practical because anyways we have to configure the +# socket ports to begin with @RobotConfig.register_subclass("lekiwi") @dataclass class LeKiwiConfig(RobotConfig): @@ -50,6 +52,10 @@ class LeKiwiConfig(RobotConfig): } ) + # Network Configuration + port_zmq_cmd: int = 5555 + port_zmq_observations: int = 5556 + @RobotConfig.register_subclass("lekiwi_client") @dataclass diff --git a/lerobot/common/robots/lekiwi/lekiwi.py b/lerobot/common/robots/lekiwi/lekiwi.py index 7ca0fc07..beece191 100644 --- a/lerobot/common/robots/lekiwi/lekiwi.py +++ b/lerobot/common/robots/lekiwi/lekiwi.py @@ -116,9 +116,6 @@ class LeKiwi(Robot): def is_calibrated(self) -> bool: return self.bus.is_calibrated - # TODO(Steven): I think we should extend this to give the user the option of re-calibrate - # calibrate(recalibrate: bool = False) -> None: - # If true, then we overwrite the previous calibration file with new values def calibrate(self) -> None: logger.info(f"\nRunning calibration of {self}") @@ -131,9 +128,15 @@ class LeKiwi(Robot): input("Move robot to the middle of its range of motion and press ENTER....") homing_offsets = self.bus.set_half_turn_homings(motors) + # TODO(Steven): Previously homig_offsets was called only on self.arm_motors + # After a discussion, we said it was better to keep it like this and then + # just populate with the rest of motors. However, I don't know which value + # should we use for this + # homing_offsets.update({k,None???} for k in self.base_motors) + # TODO(Steven): Might be worth to do this also in other robots but it should be added in the docs full_turn_motor = [ - motor for motor in motors if any(keyword in motor for keyword in ["wheel", "gripper"]) + motor for motor in motors if any(keyword in motor for keyword in ["wheel", "wrist"]) ] unknown_range_motors = [motor for motor in motors if motor not in full_turn_motor] @@ -180,7 +183,7 @@ class LeKiwi(Robot): for name in self.base_motors: self.bus.write("Operating_Mode", name, OperatingMode.VELOCITY.value) - self.bus.enable_torque() # TODO(Steven): Operation has failed with: ConnectionError: Failed to write 'Lock' on id_=6 with '1' after 1 tries. [TxRxResult] Incorrect status packet! + self.bus.enable_torque() def get_observation(self) -> dict[str, Any]: if not self.is_connected: @@ -191,7 +194,7 @@ class LeKiwi(Robot): # Read actuators position for arm and vel for base start = time.perf_counter() arm_pos = self.bus.sync_read("Present_Position", self.arm_motors) - base_vel = self.bus.sync_read("Present_Speed", self.base_motors) + base_vel = self.bus.sync_read("Present_Velocity", self.base_motors) obs_dict[OBS_STATE] = {**arm_pos, **base_vel} dt_ms = (time.perf_counter() - start) * 1e3 logger.debug(f"{self} read state: {dt_ms:.1f}ms") @@ -238,12 +241,12 @@ class LeKiwi(Robot): # Send goal position to the actuators self.bus.sync_write("Goal_Position", arm_goal_pos) - self.bus.sync_write("Goal_Speed", base_goal_vel) + self.bus.sync_write("Goal_Velocity", base_goal_vel) return {**arm_goal_pos, **base_goal_vel} def stop_base(self): - self.bus.sync_write("Goal_Speed", dict.fromkeys(self.base_motors, 0), num_retry=5) + self.bus.sync_write("Goal_Velocity", dict.fromkeys(self.base_motors, 0), num_retry=5) logger.info("Base motors stopped") def disconnect(self): diff --git a/lerobot/common/robots/lekiwi/lekiwi_client.py b/lerobot/common/robots/lekiwi/lekiwi_client.py index d63434f8..d72c322b 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_client.py +++ b/lerobot/common/robots/lekiwi/lekiwi_client.py @@ -24,10 +24,9 @@ import zmq from lerobot.common.constants import OBS_IMAGES, OBS_STATE from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError, InvalidActionError -from lerobot.common.robots.config import RobotMode from ..robot import Robot -from .config_lekiwi import LeKiwiClientConfig +from .config_lekiwi import LeKiwiClientConfig, RobotMode # TODO(Steven): This doesn't need to inherit from Robot @@ -81,9 +80,8 @@ class LeKiwiClient(Robot): @property def state_feature(self) -> dict: - # TODO(Steven): Get this from the data fetched? - # TODO(Steven): Motor names are unknown for the Daemon - # Or assume its size/metadata? + # TODO(Steven): Get this from the data fetched? Motor names are unknown for the Daemon + # For now we assume its size/metadata is known return { "dtype": "float64", "shape": (9,), @@ -108,9 +106,8 @@ class LeKiwiClient(Robot): @property def camera_features(self) -> dict[str, dict]: - # TODO(Steven): Get this from the data fetched? - # TODO(Steven): camera names are unknown for the Daemon - # Or assume its size/metadata? + # TODO(Steven): Get this from the data fetched? Motor names are unknown for the Daemon + # For now we assume its size/metadata is known # TODO(Steven): Check consistency of image sizes cam_ft = { "front": { @@ -128,7 +125,8 @@ class LeKiwiClient(Robot): @property def is_connected(self) -> bool: - # TODO(Steven): Check instead the status of the sockets + # TODO(Steven): Ideally we could check instead the status of the sockets + # I didn't find any API that allows us to do that easily return self._is_connected @property @@ -138,6 +136,7 @@ class LeKiwiClient(Robot): def connect(self) -> None: """Establishes ZMQ sockets with the remote mobile robot""" + # TODO(Steven): Consider instead returning a bool + warn if self._is_connected: raise DeviceAlreadyConnectedError( "LeKiwi Daemon is already connected. Do not run `robot.connect()` twice." @@ -174,7 +173,7 @@ class LeKiwiClient(Robot): speed_int = -0x8000 # -32768 -> minimum negative value return speed_int - # Copied from robot_lekiwi MobileManipulator class + # Copied from robot_lekiwi MobileManipulator class* (before the refactor) @staticmethod def _raw_to_degps(raw_speed: int) -> float: steps_per_deg = 4096.0 / 360.0 @@ -182,7 +181,7 @@ class LeKiwiClient(Robot): degps = magnitude / steps_per_deg return degps - # Copied from robot_lekiwi MobileManipulator class + # Copied from robot_lekiwi MobileManipulator class* (before the refactor) def _body_to_wheel_raw( self, x_cmd: float, @@ -285,6 +284,7 @@ class LeKiwiClient(Robot): # TODO(Steven): This is flaky, for example, if we received a state but failed decoding the image, we will not update any value # TODO(Steven): All this function needs to be refactored + # Copied from robot_lekiwi MobileManipulator class* (before the refactor) def _get_data(self): # Copied from robot_lekiwi.py """Polls the video socket for up to 15 ms. If data arrives, decode only @@ -369,7 +369,7 @@ class LeKiwiClient(Robot): if not self._is_connected: raise DeviceNotConnectedError("LeKiwiClient is not connected. You need to run `robot.connect()`.") - # TODO(Steven): remove hard-coded cam name + # TODO(Steven): remove hard-coded cam names & dims # This is needed at init for when there's no comms obs_dict = { OBS_IMAGES: {"wrist": np.zeros(shape=(480, 640, 3)), "front": np.zeros(shape=(640, 480, 3))} @@ -377,7 +377,7 @@ class LeKiwiClient(Robot): frames, present_speed, remote_arm_state_tensor = self._get_data() body_state = self._wheel_raw_to_body(present_speed) - # TODO(Steven): output isdict[str,Any] and we multiply by 1000.0. This should be more explicit and specify the expected type instead of Any + # TODO(Steven): output is dict[str,Any] and we multiply by 1000.0. This should be more explicit and specify the expected type instead of Any body_state_mm = {k: v * 1000.0 for k, v in body_state.items()} # Convert x,y to mm/s obs_dict[OBS_STATE] = {**remote_arm_state_tensor, **body_state_mm} @@ -423,7 +423,7 @@ class LeKiwiClient(Robot): def configure(self): pass - # TODO(Steven): This assumes this call is always called from a keyboard teleop command + # TODO(Steven): This assumes this call is always called with a keyboard as a teleop device. It breaks if we teleop with other device # TODO(Steven): Doing this mapping in here adds latecy between send_action and movement from the user perspective. # t0: get teleop_cmd # t1: send_action(teleop_cmd) @@ -454,7 +454,7 @@ class LeKiwiClient(Robot): if self.robot_mode is RobotMode.AUTO: # TODO(Steven): Not yet implemented. The policy outputs might need a different conversion - raise Exception + raise InvalidActionError("Policy output as action input is not yet well defined") goal_pos = {} # TODO(Steven): This assumes teleop mode is always used with keyboard. Tomorrow we could teleop with another device ... ? diff --git a/lerobot/common/robots/lekiwi/lekiwi_host.py b/lerobot/common/robots/lekiwi/lekiwi_host.py index 5fefc41b..f1afb38f 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_host.py +++ b/lerobot/common/robots/lekiwi/lekiwi_host.py @@ -27,22 +27,17 @@ from lerobot.common.constants import OBS_IMAGES from .config_lekiwi import LeKiwiConfig from .lekiwi import LeKiwi -# TODO(Steven): Move this to the config file -# Network Configuration -PORT_ZMQ_CMD: int = 5555 -PORT_ZMQ_OBSERVATIONS: int = 5556 - class HostAgent: - def __init__(self): + def __init__(self, port_zmq_cmd, port_zmq_observations): self.zmq_context = zmq.Context() self.zmq_cmd_socket = self.zmq_context.socket(zmq.PULL) self.zmq_cmd_socket.setsockopt(zmq.CONFLATE, 1) - self.zmq_cmd_socket.bind(f"tcp://*:{PORT_ZMQ_CMD}") + self.zmq_cmd_socket.bind(f"tcp://*:{port_zmq_cmd}") self.zmq_observation_socket = self.zmq_context.socket(zmq.PUSH) self.zmq_observation_socket.setsockopt(zmq.CONFLATE, 1) - self.zmq_observation_socket.bind(f"tcp://*:{PORT_ZMQ_OBSERVATIONS}") + self.zmq_observation_socket.bind(f"tcp://*:{port_zmq_observations}") def disconnect(self): self.zmq_observation_socket.close() @@ -59,7 +54,7 @@ def main(): robot.connect() logging.info("Starting HostAgent") - remote_agent = HostAgent() + remote_agent = HostAgent(robot_config.port_zmq_cmd, robot_config.port_zmq_observations) last_cmd_time = time.time() logging.info("Waiting for commands...") diff --git a/lerobot/common/robots/robot.py b/lerobot/common/robots/robot.py index d63026fc..151af786 100644 --- a/lerobot/common/robots/robot.py +++ b/lerobot/common/robots/robot.py @@ -22,11 +22,10 @@ class Robot(abc.ABC): def __init__(self, config: RobotConfig): self.robot_type = self.name self.id = config.id - self.robot_mode = config.robot_mode self.calibration_dir = ( Path(config.calibration_dir) if config.calibration_dir - else HF_LEROBOT_CALIBRATION / ROBOTS / self.name + else Path(HF_LEROBOT_CALIBRATION / ROBOTS / self.name) ) self.calibration_dir.mkdir(parents=True, exist_ok=True) self.calibration_fpath = self.calibration_dir / f"{self.id}.json" diff --git a/lerobot/common/robots/utils.py b/lerobot/common/robots/utils.py index 86f67882..ea646279 100644 --- a/lerobot/common/robots/utils.py +++ b/lerobot/common/robots/utils.py @@ -65,9 +65,10 @@ def make_robot_from_config(config: RobotConfig): return ManipulatorRobot(config) elif isinstance(config, LeKiwiConfig): - from lerobot.common.robots.mobile_manipulator import MobileManipulator - - return MobileManipulator(config) + # TODO(Steven): Change when we decide what to do with these scripts + # from lerobot.common.robots.mobile_manipulator import MobileManipulator + # return MobileManipulator(config) + ... else: from lerobot.common.robots.stretch3.robot_stretch3 import Stretch3Robot diff --git a/lerobot/common/teleoperators/keyboard/configuration_keyboard.py b/lerobot/common/teleoperators/keyboard/configuration_keyboard.py index a1cfbbe7..ce6c9206 100644 --- a/lerobot/common/teleoperators/keyboard/configuration_keyboard.py +++ b/lerobot/common/teleoperators/keyboard/configuration_keyboard.py @@ -22,5 +22,5 @@ from ..config import TeleoperatorConfig @TeleoperatorConfig.register_subclass("keyboard") @dataclass class KeyboardTeleopConfig(TeleoperatorConfig): - # TODO(Steven): Maybe set in here the keys that we want to capture/listen + # TODO(Steven): Consider setting in here the keys that we want to capture/listen mock: bool = False diff --git a/lerobot/common/teleoperators/keyboard/teleop_keyboard.py b/lerobot/common/teleoperators/keyboard/teleop_keyboard.py index ccb00252..64a6ce38 100644 --- a/lerobot/common/teleoperators/keyboard/teleop_keyboard.py +++ b/lerobot/common/teleoperators/keyboard/teleop_keyboard.py @@ -63,12 +63,8 @@ class KeyboardTeleop(Teleoperator): @property def action_feature(self) -> dict: - # TODO(Steven): Verify this is correct - return { - "dtype": "float32", - "shape": (len(self.arm),), - "names": {"motors": list(self.arm.motors)}, - } + # TODO(Steven): Change this when we agree what should this return + ... @property def feedback_feature(self) -> dict: @@ -83,15 +79,15 @@ class KeyboardTeleop(Teleoperator): pass def connect(self) -> None: - # TODO(Steven): Consider instead of raising a warning and then returning the status + # TODO(Steven): Consider early return instead of raising a warning # if self._is_connected: # logging.warning( - # "ManipulatorRobot is already connected. Do not run `robot.connect()` twice." + # "Keyboard is already connected. Do not run `robot.connect()` twice." # ) # return self._is_connected if self._is_connected: raise DeviceAlreadyConnectedError( - "ManipulatorRobot is already connected. Do not run `robot.connect()` twice." + "Keyboard is already connected. Do not run `robot.connect()` twice." ) if PYNPUT_AVAILABLE: diff --git a/lerobot/common/teleoperators/teleoperator.py b/lerobot/common/teleoperators/teleoperator.py index 5da1aeb5..d6285f5c 100644 --- a/lerobot/common/teleoperators/teleoperator.py +++ b/lerobot/common/teleoperators/teleoperator.py @@ -45,9 +45,6 @@ class Teleoperator(abc.ABC): def is_connected(self) -> bool: pass - # TODO(Steven): I think connect() should return a bool, such that the client/application code can check if the device connected successfully - # if not device.connect(): - # raise DeviceNotConnectedError(f"{device} failed to connect") @abc.abstractmethod def connect(self) -> None: """Connects to the teleoperator.""" diff --git a/lerobot/scripts/control_robot.py b/lerobot/scripts/control_robot.py index b744c167..7e66449e 100644 --- a/lerobot/scripts/control_robot.py +++ b/lerobot/scripts/control_robot.py @@ -422,10 +422,11 @@ def control_robot(cfg: ControlPipelineConfig): elif isinstance(cfg.control, ReplayControlConfig): replay(robot, cfg.control) elif isinstance(cfg.control, RemoteRobotConfig): - from lerobot.common.robots.lekiwi.old_lekiwi_remote import run_lekiwi - - _init_rerun(control_config=cfg.control, session_name="lerobot_control_loop_remote") - run_lekiwi(cfg.robot) + ... + # TODO(Steven): Change this when we decide what to do with the control_robot script + # from lerobot.common.robots.lekiwi.old_lekiwi_remote import run_lekiwi + # _init_rerun(control_config=cfg.control, session_name="lerobot_control_loop_remote") + # run_lekiwi(cfg.robot) if robot.is_connected: # Disconnect manually to avoid a "Core dump" during process