refactor(robots): lekiwi v0.5

lerobot/common/errors.py

@@ -16,8 +16,10 @@ class DeviceAlreadyConnectedError(ConnectionError):
         self.message = message
         super().__init__(self.message)
 
+
 class InvalidActionError(ConnectionError):
     """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.",

lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py

@@ -2,10 +2,10 @@ from dataclasses import dataclass, field
 
 from lerobot.common.robots.config import RobotConfig
 
+
 @RobotConfig.register_subclass("daemon_lekiwi")
 @dataclass
 class DaemonLeKiwiRobotConfig(RobotConfig):
-
     # Network Configuration
     remote_ip: str = "192.168.0.193"
     port_zmq_cmd: int = 5555

lerobot/common/robots/lekiwi/configuration_lekiwi.py

@@ -8,7 +8,6 @@ from lerobot.common.robots.config import RobotConfig
 @RobotConfig.register_subclass("lekiwi")
 @dataclass
 class LeKiwiRobotConfig(RobotConfig):
-
     port_zmq_cmd: int = 5555
     port_zmq_observations: int = 5556
 

lerobot/common/robots/lekiwi/daemon_lekiwi.py

@@ -14,18 +14,21 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import base64
 import json
 import logging
-import numpy as np
+
-import base64
 import cv2
+import numpy as np
 import torch
+import zmq
 
 from lerobot.common.constants import OBS_IMAGES, OBS_STATE
 from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError, InvalidActionError
+
 from ..robot import Robot, RobotMode
 from .configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig
-import zmq
+
 
 # TODO(Steven): This doesn't need to inherit from Robot
 # But we do it for now to offer a familiar API
@@ -39,7 +42,6 @@ import zmq
 # (meaning the policy might be needed to be train
 # over the teleop action space)
 class DaemonLeKiwiRobot(Robot):
-
     config_class = DaemonLeKiwiRobotConfig
     name = "daemonlekiwi"
 
@@ -63,7 +65,6 @@ class DaemonLeKiwiRobot(Robot):
         self.last_present_speed = {}
         self.last_remote_arm_state = torch.zeros(6, dtype=torch.float32)
 
-
         # Define three speed levels and a current index
         self.speed_levels = [
             {"xy": 0.1, "theta": 30},  # slow
@@ -128,7 +129,7 @@ class DaemonLeKiwiRobot(Robot):
     def calibrate(self) -> None:
         # TODO(Steven): Nothing to calibrate.
         # Consider triggering calibrate() on the remote mobile robot?
-        # Althought this would require a more complex comms schema
+        # Although this would require a more complex comms schema
         logging.warning("DaemonLeKiwiRobot has nothing to calibrate.")
         return
 
@@ -367,7 +368,6 @@ class DaemonLeKiwiRobot(Robot):
         return obs_dict
 
     def from_keyboard_to_wheel_action(self, pressed_keys: np.ndarray):
-        
         # Speed control
         if self.teleop_keys["speed_up"] in pressed_keys:
             self.speed_index = min(self.speed_index + 1, 2)

lerobot/common/robots/lekiwi/daemon_lekiwi_app.py

@@ -1,12 +1,15 @@
-from ...teleoperators.so100 import SO100Teleop, SO100TeleopConfig
+import logging
+import time
+
+import numpy as np
+
 from ...teleoperators.keyboard import KeyboardTeleop, KeyboardTeleopConfig
+from ...teleoperators.so100 import SO100Teleop, SO100TeleopConfig
 from .configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig
 from .daemon_lekiwi import DaemonLeKiwiRobot, RobotMode
-import time
+
-import logging
 
 def main():
-
     logging.info("Configuring Teleop Devices")
     leader_arm_config = SO100TeleopConfig(port="/dev/tty.usbmodem585A0085511")
     leader_arm = SO100Teleop(leader_arm_config)
@@ -30,13 +33,9 @@ def main():
     start = time.perf_counter()
     duration = 0
     while duration < 20:
-        
         arm_action = leader_arm.get_action()
         base_action = keyboard.get_action()
-        action = {
+        action = np.concatenate((arm_action, base_action))
-            **arm_action,
-            **base_action
-        }
         _action_sent = robot.send_action(action)  # Translates to motor space + sends over ZMQ
         _observation = robot.get_observation()  # Receives over ZMQ, translate to body-frame vel
 
@@ -56,5 +55,6 @@ def main():
 
     logging.info("Finished LeKiwiRobot cleanly")
 
+
 if __name__ == "__main__":
     main()

lerobot/common/robots/lekiwi/lekiwi_app.py

@@ -1,9 +1,10 @@
 import logging
+
 from .configuration_lekiwi import LeKiwiRobotConfig
 from .lekiwi_robot import LeKiwiRobot
 
-def main():
 
+def main():
     logging.info("Configuring LeKiwiRobot")
     robot_config = LeKiwiRobotConfig()
     robot = LeKiwiRobot(robot_config)

lerobot/common/robots/lekiwi/lekiwi_robot.py

@@ -14,27 +14,29 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import base64
 import json
 import logging
-import time
 import threading
-import numpy as np
 import time
-import base64
+
 import cv2
+import numpy as np
+import zmq
 
 from lerobot.common.cameras.utils import make_cameras_from_configs
 from lerobot.common.constants import OBS_IMAGES, OBS_STATE
 from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
-from ..robot import Robot
-from ..utils import ensure_safe_goal_position
-from .configuration_lekiwi import LeKiwiRobotConfig
 from lerobot.common.motors.feetech import (
     FeetechMotorsBus,
     TorqueMode,
-    run_arm_manual_calibration,
+    run_full_arm_calibration,
 )
-import zmq
+
+from ..robot import Robot
+from ..utils import ensure_safe_goal_position
+from .configuration_lekiwi import LeKiwiRobotConfig
+
 
 class LeKiwiRobot(Robot):
     """
@@ -58,7 +60,7 @@ class LeKiwiRobot(Robot):
         # TODO(Steven): Consider in the future using S100 robot class
         # TODO(Steven): Another option is to use the motorbus factory, but in this case we assume that
         # what we consider 'lekiwi robot' always uses the FeetechMotorsBus
-        # TODO(Steven): Order and dimension are generaly assumed to be 6 first for arm, 3 last for base
+        # TODO(Steven): Order and dimension are generally assumed to be 6 first for arm, 3 last for base
         self.actuators_bus = FeetechMotorsBus(
             port=self.config.port_motor_bus,
             motors={
@@ -92,8 +94,6 @@ class LeKiwiRobot(Robot):
         self.is_connected = False
         self.logs = {}
 
-        
-
     @property
     def state_feature(self) -> dict:
         return {
@@ -130,7 +130,6 @@ class LeKiwiRobot(Robot):
         return context, cmd_socket, observation_socket
 
     def setup_actuators(self):
-
         # Set-up arm actuators (position mode)
         # We assume that at connection time, arm is in a rest position,
         # and torque can be safely disabled to run calibration.
@@ -163,7 +162,6 @@ class LeKiwiRobot(Robot):
         self.actuators_bus.write("Mode", [1, 1, 1], self.base_actuators)
         self.actuators_bus.write("Lock", 1, self.base_actuators)
 
-
     def connect(self) -> None:
         if self.is_connected:
             raise DeviceAlreadyConnectedError(
@@ -192,15 +190,17 @@ class LeKiwiRobot(Robot):
         actuators_calib_path = self.calibration_dir / f"{self.config.id}.json"
 
         if actuators_calib_path.exists():
-            with open(actuators_calib_path) as f:
+            with open(actuators_calib_path,encoding="utf-8") as f:
                 calibration = json.load(f)
         else:
-            logging.info(f"Missing calibration file '{actuators_calib_path}'")
+            logging.info("Missing calibration file '%s'",actuators_calib_path)
-            calibration = run_arm_manual_calibration(self.actuators_bus, self.robot_type, self.name, "follower")
+            calibration = run_full_arm_calibration(
+                self.actuators_bus, self.robot_type, self.name, "follower"
+            )
 
-            logging.info(f"Calibration is done! Saving calibration file '{actuators_calib_path}'")
+            logging.info("Calibration is done! Saving calibration file '%s'",actuators_calib_path)
             actuators_calib_path.parent.mkdir(parents=True, exist_ok=True)
-            with open(actuators_calib_path, "w") as f:
+            with open(actuators_calib_path, "w",encoding="utf-8") as f:
                 json.dump(calibration, f)
 
         self.actuators_bus.set_calibration(calibration)
@@ -208,15 +208,15 @@ class LeKiwiRobot(Robot):
     def get_observation(self) -> dict[str, np.ndarray]:
         """The returned observations do not have a batch dimension."""
         if not self.is_connected:
-            raise DeviceNotConnectedError(
+            raise DeviceNotConnectedError("LeKiwiRobot is not connected. You need to run `robot.connect()`.")
-                "LeKiwiRobot is not connected. You need to run `robot.connect()`."
-            )
 
         obs_dict = {}
 
         # Read actuators position for arm and vel for base
         before_read_t = time.perf_counter()
-        obs_dict[OBS_STATE] = self.actuators_bus.read("Present_Position",self.arm_actuators) + self.actuators_bus.read("Present_Speed", self.base_actuators)
+        obs_dict[OBS_STATE] = self.actuators_bus.read(
+            "Present_Position", self.arm_actuators
+        ) + self.actuators_bus.read("Present_Speed", self.base_actuators)
         self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
 
         # Capture images from cameras
@@ -251,9 +251,7 @@ class LeKiwiRobot(Robot):
             np.ndarray: the action sent to the motors, potentially clipped.
         """
         if not self.is_connected:
-            raise DeviceNotConnectedError(
+            raise DeviceNotConnectedError("LeKiwiRobot is not connected. You need to run `robot.connect()`.")
-                "LeKiwiRobot is not connected. You need to run `robot.connect()`."
-            )
 
         # Input action is in motor space
         goal_pos = action
@@ -261,7 +259,9 @@ class LeKiwiRobot(Robot):
         # /!\ Slower fps expected due to reading from the follower.
         if self.config.max_relative_target is not None:
             present_pos = self.actuators_bus.read("Present_Position", self.arm_actuators)
-            goal_pos[:6] = ensure_safe_goal_position(goal_pos[:6], present_pos, self.config.max_relative_target)
+            goal_pos[:6] = ensure_safe_goal_position(
+                goal_pos[:6], present_pos, self.config.max_relative_target
+            )
 
         # Send goal position to the actuators
         # TODO(Steven): This happens synchronously
@@ -286,9 +286,7 @@ class LeKiwiRobot(Robot):
     def run(self):
         # Copied logic from run_lekiwi in lekiwi_remote.py
         if not self.is_connected:
-            raise DeviceNotConnectedError(
+            raise DeviceNotConnectedError("LeKiwiRobot is not connected. You need to run `robot.connect()`.")
-                "LeKiwiRobot is not connected. You need to run `robot.connect()`."
-            )
 
         stop_event = threading.Event()
         observation_thread = threading.Thread(
@@ -311,13 +309,12 @@ class LeKiwiRobot(Robot):
                 # except zmq.Again:
                 #     logging.warning("ZMQ again")
                 except Exception as e:
-                    logging.warning(f"[ERROR] Message fetching failed: {e}")
+                    logging.error("Message fetching failed: %s",e)
 
                 # Watchdog: stop the robot if no command is received for over 0.5 seconds.
                 now = time.time()
                 if now - last_cmd_time > 0.5:
                     self.stop()
-                    pass
 
                 with self.observation_lock:
                     self.zmq_observation_socket.send_string(json.dumps(self.last_observation))
@@ -333,7 +330,6 @@ class LeKiwiRobot(Robot):
             stop_event.set()
             observation_thread.join()
             self.disconnect()
-            pass
 
     def print_logs(self):
         # TODO(Steven): Refactor logger

lerobot/common/robots/robot.py

@@ -1,5 +1,6 @@
 import abc
 from pathlib import Path
+import enum
 from typing import Any
 
 import draccus
@@ -9,7 +10,6 @@ from lerobot.common.motors import MotorCalibration
 
 from .config import RobotConfig
 
-import enum
 
 class RobotMode(enum.Enum):
     TELEOP = 0