From 5130cba57a94b76f494037a500e9c54bb4f70d9d Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 14 Mar 2025 14:01:07 +0000 Subject: [PATCH] fix(lekiwi): HW fixes v0.1 --- .../lekiwi/configuration_daemon_lekiwi.py | 4 ++- .../robots/lekiwi/configuration_lekiwi.py | 2 ++ .../common/robots/lekiwi/daemon_lekiwi_app.py | 9 +++-- lerobot/common/robots/lekiwi/lekiwi_app.py | 4 +-- lerobot/common/robots/lekiwi/lekiwi_robot.py | 36 ++++++++++--------- lerobot/common/robots/robot.py | 5 ++- 6 files changed, 34 insertions(+), 26 deletions(-) diff --git a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py index f95dc8d4..b9c8e549 100644 --- a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py +++ b/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py @@ -7,10 +7,12 @@ from lerobot.common.robots.config import RobotConfig @dataclass class DaemonLeKiwiRobotConfig(RobotConfig): # Network Configuration - remote_ip: str = "192.168.0.193" + remote_ip: str = "172.18.133.90" port_zmq_cmd: int = 5555 port_zmq_observations: int = 5556 + id = "daemonlekiwi" + teleop_keys: dict[str, str] = field( default_factory=lambda: { # Movement diff --git a/lerobot/common/robots/lekiwi/configuration_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_lekiwi.py index c72b427d..160c17d0 100644 --- a/lerobot/common/robots/lekiwi/configuration_lekiwi.py +++ b/lerobot/common/robots/lekiwi/configuration_lekiwi.py @@ -11,6 +11,8 @@ class LeKiwiRobotConfig(RobotConfig): port_zmq_cmd: int = 5555 port_zmq_observations: int = 5556 + id = "lekiwi" + cameras: dict[str, CameraConfig] = field( default_factory=lambda: { "front": OpenCVCameraConfig( diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py b/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py index 52da223f..b459f21c 100644 --- a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py +++ b/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py @@ -3,10 +3,10 @@ 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 +from lerobot.common.robots.lekiwi.configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig +from lerobot.common.robots.lekiwi.daemon_lekiwi import DaemonLeKiwiRobot, RobotMode +from lerobot.common.teleoperators.keyboard import KeyboardTeleop, KeyboardTeleopConfig +from lerobot.common.teleoperators.so100 import SO100Teleop, SO100TeleopConfig def main(): @@ -45,7 +45,6 @@ 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) - duration = time.perf_counter() - start logging.info("Disconnecting Teleop Devices and LeKiwiRobot Daemon") diff --git a/lerobot/common/robots/lekiwi/lekiwi_app.py b/lerobot/common/robots/lekiwi/lekiwi_app.py index f8638651..6df462fe 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_app.py +++ b/lerobot/common/robots/lekiwi/lekiwi_app.py @@ -1,7 +1,7 @@ import logging -from .configuration_lekiwi import LeKiwiRobotConfig -from .lekiwi_robot import LeKiwiRobot +from lerobot.common.robots.lekiwi.configuration_lekiwi import LeKiwiRobotConfig +from lerobot.common.robots.lekiwi.lekiwi_robot import LeKiwiRobot def main(): diff --git a/lerobot/common/robots/lekiwi/lekiwi_robot.py b/lerobot/common/robots/lekiwi/lekiwi_robot.py index 9096abde..f5204ffd 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_robot.py +++ b/lerobot/common/robots/lekiwi/lekiwi_robot.py @@ -134,7 +134,7 @@ class LeKiwiRobot(Robot): # We assume that at connection time, arm is in a rest position, # and torque can be safely disabled to run calibration. self.actuators_bus.write("Torque_Enable", TorqueMode.DISABLED.value, self.arm_actuators) - self.calibrate() # TODO(Steven): This should be only for the arm + # self.calibrate() # TODO(Steven): This should be only for the arm # Mode=0 for Position Control self.actuators_bus.write("Mode", 0, self.arm_actuators) @@ -190,17 +190,15 @@ class LeKiwiRobot(Robot): actuators_calib_path = self.calibration_dir / f"{self.config.id}.json" if actuators_calib_path.exists(): - with open(actuators_calib_path,encoding="utf-8") as f: + with open(actuators_calib_path, encoding="utf-8") as f: calibration = json.load(f) else: - logging.info("Missing calibration file '%s'",actuators_calib_path) - calibration = run_full_arm_calibration( - self.actuators_bus, self.robot_type, self.name, "follower" - ) + logging.info("Missing calibration file '%s'", actuators_calib_path) + calibration = run_full_arm_calibration(self.actuators_bus, self.robot_type, self.name, "follower") - logging.info("Calibration is done! Saving calibration file '%s'",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",encoding="utf-8") as f: + with open(actuators_calib_path, "w", encoding="utf-8") as f: json.dump(calibration, f) self.actuators_bus.set_calibration(calibration) @@ -214,9 +212,12 @@ class LeKiwiRobot(Robot): # 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] = np.concatenate( + ( + 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 @@ -290,7 +291,7 @@ class LeKiwiRobot(Robot): stop_event = threading.Event() observation_thread = threading.Thread( - target=self.update_last_observation, args=(stop_event), daemon=True + target=self.update_last_observation, args=[stop_event], daemon=True ) observation_thread.start() @@ -302,14 +303,14 @@ class LeKiwiRobot(Robot): loop_start_time = time.time() try: - msg = self.cmd_socket.recv_string(zmq.NOBLOCK) + msg = self.zmq_cmd_socket.recv_string(zmq.NOBLOCK) data = json.loads(msg) self.send_action(data) last_cmd_time = time.time() - # except zmq.Again: - # logging.warning("ZMQ again") + except zmq.Again: + logging.warning("ZMQ again") except Exception as e: - logging.error("Message fetching failed: %s",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() @@ -317,6 +318,7 @@ class LeKiwiRobot(Robot): self.stop() with self.observation_lock: + # TODO(Steven): This operation is blocking if no listener is available self.zmq_observation_socket.send_string(json.dumps(self.last_observation)) # Ensure a short sleep to avoid overloading the CPU. @@ -346,7 +348,7 @@ class LeKiwiRobot(Robot): for cam in self.cameras.values(): cam.disconnect() self.observation_socket.close() - self.cmd_socket.close() + self.zmq_cmd_socket.close() self.context.term() self.is_connected = False diff --git a/lerobot/common/robots/robot.py b/lerobot/common/robots/robot.py index e3cca3de..57ff41a4 100644 --- a/lerobot/common/robots/robot.py +++ b/lerobot/common/robots/robot.py @@ -1,6 +1,7 @@ import abc from pathlib import Path import enum +from pathlib import Path from typing import Any import draccus @@ -30,7 +31,9 @@ class Robot(abc.ABC): self.robot_mode: RobotMode | None = None self.id = config.id self.calibration_dir = ( - config.calibration_dir if config.calibration_dir else HF_LEROBOT_CALIBRATION / ROBOTS / self.name + Path(config.calibration_dir) + if config.calibration_dir + else 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"