refactor/lekiwi robot

examples/robots/lekiwi_client_app.py

@@ -0,0 +1,98 @@
+# Copyright 2024 The HuggingFace Inc. team. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import logging
+import time
+
+from lerobot.common.datasets.lerobot_dataset import LeRobotDataset
+from lerobot.common.robots.lekiwi.config_lekiwi import LeKiwiClientConfig
+from lerobot.common.robots.lekiwi.lekiwi_client import OBS_STATE, LeKiwiClient
+from lerobot.common.teleoperators.keyboard import KeyboardTeleop, KeyboardTeleopConfig
+from lerobot.common.teleoperators.so100 import SO100Leader, SO100LeaderConfig
+
+NB_CYCLES_CLIENT_CONNECTION = 250
+
+
+def main():
+    logging.info("Configuring Teleop Devices")
+    leader_arm_config = SO100LeaderConfig(port="/dev/tty.usbmodem58760434171")
+    leader_arm = SO100Leader(leader_arm_config)
+
+    keyboard_config = KeyboardTeleopConfig()
+    keyboard = KeyboardTeleop(keyboard_config)
+
+    logging.info("Configuring LeKiwi Client")
+    robot_config = LeKiwiClientConfig(remote_ip="192.0.2.42", id="lekiwi")
+    robot = LeKiwiClient(robot_config)
+
+    logging.info("Creating LeRobot Dataset")
+
+    # The observations that we get are expected to be in body frame (x,y,theta)
+    obs_dict = {f"{OBS_STATE}." + key: value for key, value in robot.state_feature.items()}
+    # The actions that we send are expected to be in wheel frame (motor encoders)
+    act_dict = {"action." + key: value for key, value in robot.action_feature.items()}
+
+    features_dict = {
+        **act_dict,
+        **obs_dict,
+        **robot.camera_features,
+    }
+    dataset = LeRobotDataset.create(
+        repo_id="user/lekiwi" + str(int(time.time())),
+        fps=10,
+        features=features_dict,
+    )
+
+    logging.info("Connecting Teleop Devices")
+    leader_arm.connect()
+    keyboard.connect()
+
+    logging.info("Connecting remote LeKiwi")
+    robot.connect()
+
+    if not robot.is_connected or not leader_arm.is_connected or not keyboard.is_connected:
+        logging.error("Failed to connect to all devices")
+        return
+
+    logging.info("Starting LeKiwi teleoperation")
+    i = 0
+    while i < NB_CYCLES_CLIENT_CONNECTION:
+        arm_action = leader_arm.get_action()
+        base_action = keyboard.get_action()
+        action = {**arm_action, **base_action} if len(base_action) > 0 else arm_action
+
+        action_sent = robot.send_action(action)
+        observation = robot.get_observation()
+
+        frame = {**action_sent, **observation}
+        frame.update({"task": "Dummy Example Task Dataset"})
+
+        logging.info("Saved a frame into the dataset")
+        dataset.add_frame(frame)
+        i += 1
+
+    logging.info("Disconnecting Teleop Devices and LeKiwi Client")
+    robot.disconnect()
+    leader_arm.disconnect()
+    keyboard.disconnect()
+
+    logging.info("Uploading dataset to the hub")
+    dataset.save_episode()
+    dataset.push_to_hub()
+
+    logging.info("Finished LeKiwi cleanly")
+
+
+if __name__ == "__main__":
+    main()

lerobot/common/teleoperators/keyboard/teleop_keyboard_app.py → examples/teleoperators/teleop_keyboard_app.py

@@ -21,7 +21,7 @@ def main():
         i += 1
 
     keyboard.disconnect()
-    logging.info("Finished LeKiwiRobot cleanly")
+    logging.info("Finished LeKiwi cleanly")
 
 
 if __name__ == "__main__":

lerobot/common/constants.py

@@ -48,5 +48,5 @@
 HF_LEROBOT_HOME = Path(os.getenv("HF_LEROBOT_HOME", default_cache_path)).expanduser()
 
 # calibration dir
-default_calibration_path = HF_LEROBOT_HOME / ".calibration"
+default_calibration_path = HF_LEROBOT_HOME / "calibration"
 HF_LEROBOT_CALIBRATION = Path(os.getenv("HF_LEROBOT_CALIBRATION", default_calibration_path)).expanduser()

lerobot/common/errors.py

@@ -15,3 +15,14 @@ def __init__(
     ):
         self.message = message
         super().__init__(self.message)
+
+
+class InvalidActionError(ValueError):
+    """Exception raised when an action is already invalid."""
+
+    def __init__(
+        self,
+        message="The action is invalid. Check the value follows what it is expected from the action space.",
+    ):
+        self.message = message
+        super().__init__(self.message)

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_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

@@ -570,7 +570,7 @@ def set_half_turn_homings(self, motors: NameOrID | list[NameOrID] | None = None)
             motors = list(self.motors)
         elif isinstance(motors, (str, int)):
             motors = [motors]
-        else:
+        elif not isinstance(motors, list):
             raise TypeError(motors)
 
         self.reset_calibration(motors)
@@ -633,6 +633,8 @@ def _normalize(self, data_name: str, ids_values: dict[int, int]) -> dict[int, fl
             min_ = self.calibration[motor].range_min
             max_ = self.calibration[motor].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, most likely a gripper-like one)
             if self.motors[motor].norm_mode is MotorNormMode.RANGE_M100_100:
                 normalized_values[id_] = (((bounded_val - min_) / (max_ - min_)) * 200) - 100
             elif self.motors[motor].norm_mode is MotorNormMode.RANGE_0_100:

lerobot/common/robots/lekiwi/README.md

@@ -194,11 +194,11 @@ sudo chmod 666 /dev/ttyACM1
 
 #### d. Update config file
 
-IMPORTANTLY: Now that you have your ports of leader and follower arm and ip address of the mobile-so100, update the **ip** in Network configuration, **port** in leader_arms and **port** in lekiwi. In the [`LeKiwiRobotConfig`](../lerobot/common/robot_devices/robots/configs.py) file. Where you will find something like:
+IMPORTANTLY: Now that you have your ports of leader and follower arm and ip address of the mobile-so100, update the **ip** in Network configuration, **port** in leader_arms and **port** in lekiwi. In the [`LeKiwiConfig`](../lerobot/common/robot_devices/robots/configs.py) file. Where you will find something like:
 ```python
 @RobotConfig.register_subclass("lekiwi")
 @dataclass
-class LeKiwiRobotConfig(RobotConfig):
+class LeKiwiConfig(RobotConfig):
     # `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.
@@ -281,7 +281,7 @@ For the wired LeKiwi version your configured IP address should refer to your own
 ```python
 @RobotConfig.register_subclass("lekiwi")
 @dataclass
-class LeKiwiRobotConfig(RobotConfig):
+class LeKiwiConfig(RobotConfig):
     # `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.
@@ -446,7 +446,7 @@ You should see on your laptop something like this: ```[INFO] Connected to remote
 | F   | Decrease speed |
 
 > [!TIP]
->  If you use a different keyboard you can change the keys for each command in the [`LeKiwiRobotConfig`](../lerobot/common/robot_devices/robots/configs.py).
+>  If you use a different keyboard you can change the keys for each command in the [`LeKiwiConfig`](../lerobot/common/robot_devices/robots/configs.py).
 
 ### Wired version
 If you have the **wired** LeKiwi version please run all commands including both these teleoperation commands on your laptop.

lerobot/common/robots/lekiwi/__init__.py

@@ -0,0 +1,3 @@
+from .config_lekiwi import LeKiwiClientConfig, LeKiwiConfig
+from .lekiwi import LeKiwi
+from .lekiwi_client import LeKiwiClient

lerobot/common/robots/lekiwi/config_lekiwi.py

@@ -0,0 +1,89 @@
+# Copyright 2024 The HuggingFace Inc. team. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from dataclasses import dataclass, field
+
+from lerobot.common.cameras.configs import CameraConfig
+from lerobot.common.cameras.opencv.configuration_opencv import OpenCVCameraConfig
+
+from ..config import RobotConfig
+
+
+@RobotConfig.register_subclass("lekiwi")
+@dataclass
+class LeKiwiConfig(RobotConfig):
+    port = "/dev/ttyACM0"  # port to connect to the bus
+
+    disable_torque_on_disconnect: bool = True
+
+    # `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: int | None = None
+
+    cameras: dict[str, CameraConfig] = field(
+        default_factory=lambda: {
+            "front": OpenCVCameraConfig(
+                camera_index="/dev/video0", fps=30, width=640, height=480, rotation=None
+            ),
+            "wrist": OpenCVCameraConfig(
+                camera_index="/dev/video2", fps=30, width=640, height=480, rotation=180
+            ),
+        }
+    )
+
+
+@dataclass
+class LeKiwiHostConfig:
+    # Network Configuration
+    port_zmq_cmd: int = 5555
+    port_zmq_observations: int = 5556
+
+    # Duration of the application
+    connection_time_s: int = 30
+
+    # Watchdog: stop the robot if no command is received for over 0.5 seconds.
+    watchdog_timeout_ms: int = 500
+
+    # If robot jitters decrease the frequency and monitor cpu load with `top` in cmd
+    max_loop_freq_hz: int = 30
+
+
+@RobotConfig.register_subclass("lekiwi_client")
+@dataclass
+class LeKiwiClientConfig(RobotConfig):
+    # Network Configuration
+    remote_ip: str
+    port_zmq_cmd: int = 5555
+    port_zmq_observations: int = 5556
+
+    teleop_keys: dict[str, str] = field(
+        default_factory=lambda: {
+            # Movement
+            "forward": "w",
+            "backward": "s",
+            "left": "a",
+            "right": "d",
+            "rotate_left": "z",
+            "rotate_right": "x",
+            # Speed control
+            "speed_up": "r",
+            "speed_down": "f",
+            # quit teleop
+            "quit": "q",
+        }
+    )
+
+    polling_timeout_ms: int = 15
+    connect_timeout_s: int = 5