From a74345bf538367294929d932fdc96d6746e47c40 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Mon, 7 Apr 2025 11:18:34 +0200 Subject: [PATCH] dbg(robots): check sent action wheels lekiwi --- examples/robots/lekiwi_client_app.py | 4 +--- lerobot/common/robots/lekiwi/lekiwi.py | 4 +++- lerobot/common/robots/lekiwi/lekiwi_client.py | 2 +- lerobot/common/robots/lekiwi/lekiwi_host.py | 2 ++ 4 files changed, 7 insertions(+), 5 deletions(-) diff --git a/examples/robots/lekiwi_client_app.py b/examples/robots/lekiwi_client_app.py index ed9f61c4..f3f9747a 100755 --- a/examples/robots/lekiwi_client_app.py +++ b/examples/robots/lekiwi_client_app.py @@ -85,9 +85,7 @@ def main(): keyboard = KeyboardTeleop(keyboard_config) logging.info("Configuring LeKiwi Client") - robot_config = LeKiwiClientConfig( - id="daemonlekiwi", calibration_dir=".cache/calibration/lekiwi", robot_mode=RobotMode.TELEOP - ) + robot_config = LeKiwiClientConfig(id="lekiwi", robot_mode=RobotMode.TELEOP) robot = LeKiwiClient(robot_config) logging.info("Creating LeRobot Dataset") diff --git a/lerobot/common/robots/lekiwi/lekiwi.py b/lerobot/common/robots/lekiwi/lekiwi.py index 52045606..411f1c51 100644 --- a/lerobot/common/robots/lekiwi/lekiwi.py +++ b/lerobot/common/robots/lekiwi/lekiwi.py @@ -186,7 +186,9 @@ class LeKiwi(Robot): if not self.is_connected: raise DeviceNotConnectedError(f"{self} is not connected.") - obs_dict = {} + # TODO(Steven): remove hard-coded cam name + # This is needed at init for when there's no comms + obs_dict = {OBS_IMAGES: {}} # Read actuators position for arm and vel for base start = time.perf_counter() diff --git a/lerobot/common/robots/lekiwi/lekiwi_client.py b/lerobot/common/robots/lekiwi/lekiwi_client.py index 34e2b40b..b6148c23 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_client.py +++ b/lerobot/common/robots/lekiwi/lekiwi_client.py @@ -246,6 +246,7 @@ class LeKiwiClient(Robot): # Convert each wheel’s angular speed (deg/s) to a raw integer. wheel_raw = [LeKiwiClient._degps_to_raw(deg) for deg in wheel_degps] + # TODO(Steven): remove hard-coded names return {"left_wheel": wheel_raw[0], "back_wheel": wheel_raw[1], "right_wheel": wheel_raw[2]} # Copied from robot_lekiwi MobileManipulator class @@ -396,7 +397,6 @@ class LeKiwiClient(Robot): frame = np.zeros((480, 640, 3), dtype=np.uint8) obs_dict[OBS_IMAGES][cam_name] = torch.from_numpy(frame) - print("obs_dict", obs_dict) return obs_dict def _from_keyboard_to_wheel_action(self, pressed_keys: np.ndarray): diff --git a/lerobot/common/robots/lekiwi/lekiwi_host.py b/lerobot/common/robots/lekiwi/lekiwi_host.py index 56228ac9..8439fcc7 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_host.py +++ b/lerobot/common/robots/lekiwi/lekiwi_host.py @@ -71,7 +71,9 @@ def main(): try: msg = remote_agent.zmq_cmd_socket.recv_string(zmq.NOBLOCK) data = dict(json.loads(msg)) + print("Received command:", data) _action_sent = robot.send_action(data) + print("Sent action:", _action_sent) last_cmd_time = time.time() except zmq.Again: logging.warning("No command available")