diff --git a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py index b9c8e549..b526bce9 100644 --- a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py +++ b/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py @@ -13,6 +13,8 @@ class DaemonLeKiwiRobotConfig(RobotConfig): id = "daemonlekiwi" + calibration_dir: str = ".cache/calibration/lekiwi" + 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 160c17d0..80c25d20 100644 --- a/lerobot/common/robots/lekiwi/configuration_lekiwi.py +++ b/lerobot/common/robots/lekiwi/configuration_lekiwi.py @@ -16,10 +16,10 @@ class LeKiwiRobotConfig(RobotConfig): cameras: dict[str, CameraConfig] = field( default_factory=lambda: { "front": OpenCVCameraConfig( - camera_index="/dev/video0", fps=30, width=640, height=480, rotation=90 + camera_index="/dev/video1", fps=30, width=640, height=480, rotation=90 ), "wrist": OpenCVCameraConfig( - camera_index="/dev/video2", fps=30, width=640, height=480, rotation=180 + camera_index="/dev/video4", fps=30, width=640, height=480, rotation=180 ), } ) diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi.py b/lerobot/common/robots/lekiwi/daemon_lekiwi.py index 3a76f809..b096ec02 100644 --- a/lerobot/common/robots/lekiwi/daemon_lekiwi.py +++ b/lerobot/common/robots/lekiwi/daemon_lekiwi.py @@ -62,7 +62,7 @@ class DaemonLeKiwiRobot(Robot): self.zmq_observation_socket = None self.last_frames = {} - self.last_present_speed = {} + self.last_present_speed = [0, 0, 0] self.last_remote_arm_state = torch.zeros(6, dtype=torch.float32) # Define three speed levels and a current index @@ -223,13 +223,13 @@ class DaemonLeKiwiRobot(Robot): # Copied from robot_lekiwi MobileManipulator class def wheel_raw_to_body( - self, wheel_raw: dict, wheel_radius: float = 0.05, base_radius: float = 0.125 + self, wheel_raw: np.array, wheel_radius: float = 0.05, base_radius: float = 0.125 ) -> tuple: """ Convert wheel raw command feedback back into body-frame velocities. Parameters: - wheel_raw : Dictionary with raw wheel commands (keys: "left_wheel", "back_wheel", "right_wheel"). + wheel_raw : Vector with raw wheel commands ("left_wheel", "back_wheel", "right_wheel"). wheel_radius: Radius of each wheel (meters). base_radius : Distance from the robot center to each wheel (meters). @@ -239,15 +239,9 @@ class DaemonLeKiwiRobot(Robot): y_cmd : Linear velocity in y (m/s). theta_cmd : Rotational velocity in deg/s. """ - # Extract the raw values in order. - raw_list = [ - int(wheel_raw.get("left_wheel", 0)), - int(wheel_raw.get("back_wheel", 0)), - int(wheel_raw.get("right_wheel", 0)), - ] # Convert each raw command back to an angular speed in deg/s. - wheel_degps = np.array([DaemonLeKiwiRobot.raw_to_degps(r) for r in raw_list]) + wheel_degps = np.array([DaemonLeKiwiRobot.raw_to_degps(int(r)) for r in wheel_raw]) # Convert from deg/s to rad/s. wheel_radps = wheel_degps * (np.pi / 180.0) # Compute each wheel’s linear speed (m/s) from its angular speed. @@ -264,6 +258,7 @@ class DaemonLeKiwiRobot(Robot): theta_cmd = theta_rad * (180.0 / np.pi) return (x_cmd, y_cmd, theta_cmd) + # TODO(Steven): This is flaky, for example, if we received a state but failed decoding the image, we will not update any value def get_data(self): # Copied from robot_lekiwi.py """Polls the video socket for up to 15 ms. If data arrives, decode only @@ -271,10 +266,10 @@ class DaemonLeKiwiRobot(Robot): nothing arrives for any field, use the last known values.""" frames = {} - present_speed = {} + present_speed = [] # TODO(Steven): Size is being assumed, is this safe? - remote_arm_state_tensor = torch.zeros(6, dtype=torch.float32) + remote_arm_state_tensor = torch.empty(6, dtype=torch.float32) # Poll up to 15 ms poller = zmq.Poller() @@ -303,7 +298,9 @@ class DaemonLeKiwiRobot(Robot): # Decode only the final message try: observation = json.loads(last_msg) + observation[OBS_STATE] = np.array(observation[OBS_STATE]) + # TODO(Steven): Consider getting directly the item with observation[OBS_STATE] state_observation = {k: v for k, v in observation.items() if k.startswith(OBS_STATE)} image_observation = {k: v for k, v in observation.items() if k.startswith(OBS_IMAGES)} @@ -320,10 +317,10 @@ class DaemonLeKiwiRobot(Robot): if state_observation is not None and frames is not None: self.last_frames = frames - remote_arm_state_tensor = torch.tensor(state_observation[:6], dtype=torch.float32) + remote_arm_state_tensor = torch.tensor(state_observation[OBS_STATE][:6], dtype=torch.float32) self.last_remote_arm_state = remote_arm_state_tensor - present_speed = state_observation[6:] + present_speed = state_observation[OBS_STATE][6:] self.last_present_speed = present_speed else: frames = self.last_frames diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py b/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py index a52f534b..3c45377d 100644 --- a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py +++ b/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py @@ -32,7 +32,7 @@ def main(): logging.info("Starting LeKiwiRobot teleoperation") start = time.perf_counter() duration = 0 - while duration < 20: + while duration < 100: arm_action = leader_arm.get_action() base_action = keyboard.get_action() action = np.append(arm_action, base_action) if base_action.size > 0 else arm_action diff --git a/lerobot/common/robots/lekiwi/lekiwi_robot.py b/lerobot/common/robots/lekiwi/lekiwi_robot.py index 79388a12..22153b5f 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_robot.py +++ b/lerobot/common/robots/lekiwi/lekiwi_robot.py @@ -203,6 +203,7 @@ class LeKiwiRobot(Robot): self.actuators_bus.set_calibration(calibration) + # TODO(Steven): Should this be dict[str, Any] ? def get_observation(self) -> dict[str, np.ndarray]: """The returned observations do not have a batch dimension.""" if not self.is_connected: @@ -274,13 +275,17 @@ class LeKiwiRobot(Robot): def update_last_observation(self, stop_event): while not stop_event.is_set(): obs = self.get_observation() + obs[OBS_STATE] = obs[OBS_STATE].tolist() # Needed for np.array be serializable with self.observation_lock: self.last_observation = obs # TODO(Steven): Consider adding a delay to not starve the CPU + # TODO(Steven): Check this value + time.sleep(0.5) def stop(self): # TODO(Steven): Assumes there's only 3 motors for base logging.info("Stopping base") + # TODO(Steven): Check if these operations are thread safe! self.actuators_bus.write("Goal_Speed", [0, 0, 0], self.base_actuators) logging.info("Base motors stopped") @@ -299,23 +304,28 @@ class LeKiwiRobot(Robot): logging.info("LeKiwi robot server started. Waiting for commands...") try: - while True: + start = time.perf_counter() + duration = 0 + while duration < 100: loop_start_time = time.time() try: msg = self.zmq_cmd_socket.recv_string(zmq.NOBLOCK) data = np.array(json.loads(msg)) - self.send_action(data) + _action_sent = self.send_action(data) last_cmd_time = time.time() except zmq.Again: logging.warning("ZMQ again") except Exception as e: logging.error("Message fetching failed: %s", e) + # TODO(Steven): Check this value # 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() + # TODO(Steven): This doesn't seem to be thread safe! + # self.stop() + pass with self.observation_lock: # TODO(Steven): This operation is blocking if no listener is available @@ -323,9 +333,12 @@ class LeKiwiRobot(Robot): # Ensure a short sleep to avoid overloading the CPU. elapsed = time.time() - loop_start_time + + # TODO(Steven): Check this value time.sleep( max(0.033 - elapsed, 0) ) # If robot jitters increase the sleep and monitor cpu load with `top` in cmd + duration = time.perf_counter() - start except KeyboardInterrupt: print("Shutting down LeKiwi server.") finally: @@ -349,7 +362,7 @@ class LeKiwiRobot(Robot): cam.disconnect() self.zmq_observation_socket.close() self.zmq_cmd_socket.close() - self.context.term() + self.zmq_context.term() self.is_connected = False def __del__(self):