refactor(robots): multiple changes from feedback

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()
 

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 = {

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 = {

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:

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

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):

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): Get this from the data fetched? Motor names are unknown for the Daemon
-        # TODO(Steven): Motor names are unknown for the Daemon
+        # For now we assume its size/metadata is known
-        # Or assume its size/metadata?
         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): Get this from the data fetched? Motor names are unknown for the Daemon
-        # TODO(Steven): camera names are unknown for the Daemon
+        # For now we assume its size/metadata is known
-        # Or assume its size/metadata?
         # 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 ... ?

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...")

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"

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
+        # TODO(Steven): Change when we decide what to do with these scripts
-
+        # from lerobot.common.robots.mobile_manipulator import MobileManipulator
-        return MobileManipulator(config)
+        # return MobileManipulator(config)
+        ...
     else:
         from lerobot.common.robots.stretch3.robot_stretch3 import Stretch3Robot
 

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

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
+        # TODO(Steven): Change this when we agree what should this return
-        return {
+        ...
-            "dtype": "float32",
-            "shape": (len(self.arm),),
-            "names": {"motors": list(self.arm.motors)},
-        }
 
     @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:

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."""

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
+        ...
-
+        # TODO(Steven): Change this when we decide what to do with the control_robot script
-        _init_rerun(control_config=cfg.control, session_name="lerobot_control_loop_remote")
+        # from lerobot.common.robots.lekiwi.old_lekiwi_remote import run_lekiwi
-        run_lekiwi(cfg.robot)
+        # _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