diff --git a/lerobot/common/errors.py b/lerobot/common/errors.py index b064a295..6398bc6f 100644 --- a/lerobot/common/errors.py +++ b/lerobot/common/errors.py @@ -16,11 +16,13 @@ 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.", ): self.message = message - super().__init__(self.message) \ No newline at end of file + super().__init__(self.message) diff --git a/lerobot/common/robots/lekiwi/old_lekiwi_remote.py b/lerobot/common/robots/lekiwi/_old_lekiwi_remote.py similarity index 100% rename from lerobot/common/robots/lekiwi/old_lekiwi_remote.py rename to lerobot/common/robots/lekiwi/_old_lekiwi_remote.py diff --git a/lerobot/common/robots/lekiwi/old_robot_lekiwi.py b/lerobot/common/robots/lekiwi/_old_robot_lekiwi.py similarity index 100% rename from lerobot/common/robots/lekiwi/old_robot_lekiwi.py rename to lerobot/common/robots/lekiwi/_old_robot_lekiwi.py diff --git a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py index fe7e175b..f95dc8d4 100644 --- a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py +++ b/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 @@ -26,4 +26,4 @@ class DaemonLeKiwiRobotConfig(RobotConfig): # quit teleop "quit": "q", } - ) \ No newline at end of file + ) diff --git a/lerobot/common/robots/lekiwi/configuration_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_lekiwi.py index 3fd34d11..c72b427d 100644 --- a/lerobot/common/robots/lekiwi/configuration_lekiwi.py +++ b/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 @@ -24,7 +23,7 @@ class LeKiwiRobotConfig(RobotConfig): ) calibration_dir: str = ".cache/calibration/lekiwi" - + port_motor_bus = "/dev/ttyACM0" # TODO(Steven): consider split this into arm and base @@ -32,12 +31,12 @@ class LeKiwiRobotConfig(RobotConfig): # LeKiwiRobot will always have (and needs) such shoulder_pan: tuple = (1, "sts3215") shoulder_lift: tuple = (2, "sts3215") - elbow_flex: tuple=(3, "sts3215") - wrist_flex: tuple=(4, "sts3215") - wrist_roll: tuple=(5, "sts3215") - gripper: tuple =(6, "sts3215") - left_wheel: tuple= (7, "sts3215") - back_wheel: tuple = (8, "sts3215") + elbow_flex: tuple = (3, "sts3215") + wrist_flex: tuple = (4, "sts3215") + wrist_roll: tuple = (5, "sts3215") + gripper: tuple = (6, "sts3215") + left_wheel: tuple = (7, "sts3215") + back_wheel: tuple = (8, "sts3215") right_wheel: tuple = (9, "sts3215") # `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes. diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi.py b/lerobot/common/robots/lekiwi/daemon_lekiwi.py index 94c04d2f..3878e08e 100644 --- a/lerobot/common/robots/lekiwi/daemon_lekiwi.py +++ b/lerobot/common/robots/lekiwi/daemon_lekiwi.py @@ -14,32 +14,34 @@ # 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 # TODO(Steven): This doesn't need to take care of the # mapping from teleop to motor commands, but given that # we already have a middle-man (this class) we add it here -# Other options include: +# Other options include: # 1. Adding it to the Telop implementation for lekiwi # (meaning each robot will need a teleop imple) or -# 2. Adding it into the robot implementation -# (meaning the policy might be needed to be train +# 2. Adding it into the robot implementation +# (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 @@ -105,7 +106,7 @@ class DaemonLeKiwiRobot(Robot): # } # return cam_ft pass - + def connect(self) -> None: if self.is_connected: raise DeviceAlreadyConnectedError( @@ -121,17 +122,17 @@ class DaemonLeKiwiRobot(Robot): self.zmq_observation_socket = self.zmq_context.socket(zmq.PULL) zmq_observations_locator = f"tcp://{self.remote_ip}:{self.port_zmq_observations}" self.zmq_observation_socket.connect(zmq_observations_locator) - self.zmq_observation_socket.setsockopt(zmq.CONFLATE,1) + self.zmq_observation_socket.setsockopt(zmq.CONFLATE, 1) self.is_connected = True def calibrate(self) -> None: - # TODO(Steven): Nothing to calibrate. + # 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 - + # Consider moving these static functions out of the class # Copied from robot_lekiwi MobileManipulator class @staticmethod @@ -145,7 +146,7 @@ class DaemonLeKiwiRobot(Robot): return speed_int | 0x8000 else: return speed_int & 0x7FFF - + # Copied from robot_lekiwi MobileManipulator class @staticmethod def raw_to_degps(raw_speed: int) -> float: @@ -155,7 +156,7 @@ class DaemonLeKiwiRobot(Robot): if raw_speed & 0x8000: degps = -degps return degps - + # Copied from robot_lekiwi MobileManipulator class def body_to_wheel_raw( self, @@ -217,7 +218,7 @@ class DaemonLeKiwiRobot(Robot): wheel_raw = [DaemonLeKiwiRobot.degps_to_raw(deg) for deg in wheel_degps] return {"left_wheel": wheel_raw[0], "back_wheel": wheel_raw[1], "right_wheel": wheel_raw[2]} - + # 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 @@ -260,7 +261,7 @@ class DaemonLeKiwiRobot(Robot): x_cmd, y_cmd, theta_rad = velocity_vector theta_cmd = theta_rad * (180.0 / np.pi) return (x_cmd, y_cmd, theta_cmd) - + def get_data(self): # Copied from robot_lekiwi.py """Polls the video socket for up to 15 ms. If data arrives, decode only @@ -332,7 +333,7 @@ class DaemonLeKiwiRobot(Robot): # If decode fails, fall back to old data return (self.last_frames, self.last_present_speed, self.last_remote_arm_state) return frames, present_speed, remote_arm_state_tensor - + # TODO(Steven): The returned space is different from the get_observation of LeKiwiRobot # This returns body-frames velocities instead of wheel pos/speeds def get_observation(self) -> dict[str, np.ndarray]: @@ -353,7 +354,7 @@ class DaemonLeKiwiRobot(Robot): body_state_mm = (body_state[0] * 1000.0, body_state[1] * 1000.0, body_state[2]) # Convert x,y to mm/s wheel_state_tensor = torch.tensor(body_state_mm, dtype=torch.float32) combined_state_tensor = torch.cat((remote_arm_state_tensor, wheel_state_tensor), dim=0) - + obs_dict = {OBS_STATE: combined_state_tensor} # Loop over each configured camera @@ -361,13 +362,12 @@ class DaemonLeKiwiRobot(Robot): if frame is None: # TODO(Steven): Daemon doesn't know camera dimensions logging.warning("Frame is None") - #frame = np.zeros((cam.height, cam.width, cam.channels), dtype=np.uint8) + # frame = np.zeros((cam.height, cam.width, cam.channels), dtype=np.uint8) obs_dict[cam_name] = torch.from_numpy(frame) 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) @@ -376,7 +376,7 @@ class DaemonLeKiwiRobot(Robot): speed_setting = self.speed_levels[self.speed_index] xy_speed = speed_setting["xy"] # e.g. 0.1, 0.25, or 0.4 theta_speed = speed_setting["theta"] # e.g. 30, 60, or 90 - + x_cmd = 0.0 # m/s forward/backward y_cmd = 0.0 # m/s lateral theta_cmd = 0.0 # deg/s rotation @@ -424,27 +424,27 @@ class DaemonLeKiwiRobot(Robot): raise DeviceNotConnectedError( "ManipulatorRobot is not connected. You need to run `robot.connect()`." ) - + goal_pos: np.array = np.empty(9) if self.robot_mode is RobotMode.AUTO: # TODO(Steven): Not yet implemented. The policy outputs might need a different conversion raise Exception - + # TODO(Steven): This assumes teleop mode is always used with keyboard if self.robot_mode is RobotMode.TELEOP: - if action.size <6: + if action.size < 6: logging.error("Action should include at least the 6 states of the leader arm") raise InvalidActionError - + # TODO(Steven): Assumes size and order is respected - wheel_actions = [v for _,v in self.from_keyboard_to_wheel_action(action[6:])] - goal_pos[:6]=action[:6] - goal_pos[6:]=wheel_actions - self.zmq_cmd_socket.send_string(json.dumps(goal_pos)) #action is in motor space + wheel_actions = [v for _, v in self.from_keyboard_to_wheel_action(action[6:])] + goal_pos[:6] = action[:6] + goal_pos[6:] = wheel_actions + self.zmq_cmd_socket.send_string(json.dumps(goal_pos)) # action is in motor space return goal_pos - + def print_logs(self): # TODO(Steven): Refactor logger pass @@ -459,7 +459,7 @@ class DaemonLeKiwiRobot(Robot): self.zmq_cmd_socket.close() self.zmq_context.term() self.is_connected = False - + def __del__(self): if getattr(self, "is_connected", False): self.disconnect() diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py b/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py index 79c89f6b..52da223f 100644 --- a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py +++ b/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) @@ -21,24 +24,20 @@ def main(): logging.info("Configuring LeKiwiRobot Daemon") robot_config = DaemonLeKiwiRobotConfig() robot = DaemonLeKiwiRobot(robot_config) - + logging.info("Connecting remote LeKiwiRobot") - robot.connect() # Establishes ZMQ sockets with the remote mobile robot + robot.connect() # Establishes ZMQ sockets with the remote mobile robot robot.robot_mode = RobotMode.TELEOP logging.info("Starting LeKiwiRobot teleoperation") start = time.perf_counter() duration = 0 while duration < 20: - arm_action = leader_arm.get_action() base_action = keyboard.get_action() - 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 + action = np.concatenate((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 # dataset.save(action_sent, obs) @@ -50,11 +49,12 @@ def main(): duration = time.perf_counter() - start logging.info("Disconnecting Teleop Devices and LeKiwiRobot Daemon") - robot.disconnect() # Cleans ZMQ comms + robot.disconnect() # Cleans ZMQ comms leader_arm.disconnect() keyboard.disconnect() logging.info("Finished LeKiwiRobot cleanly") + if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/lerobot/common/robots/lekiwi/lekiwi_app.py b/lerobot/common/robots/lekiwi/lekiwi_app.py index 349c952b..f8638651 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_app.py +++ b/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) @@ -14,9 +15,9 @@ def main(): # Remotely teleoperated logging.info("Starting LeKiwiRobot teleoperation") robot.run() - + logging.info("Finished LeKiwiRobot cleanly") if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/lerobot/common/robots/lekiwi/lekiwi_robot.py b/lerobot/common/robots/lekiwi/lekiwi_robot.py index b28629f1..9096abde 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_robot.py +++ b/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={ @@ -79,7 +81,7 @@ class LeKiwiRobot(Robot): self.max_relative_target = config.max_relative_target - #TODO(Steven): Consider removing cameras from configs + # TODO(Steven): Consider removing cameras from configs self.cameras = make_cameras_from_configs(config.cameras) self.observation_lock = threading.Lock() @@ -92,8 +94,6 @@ class LeKiwiRobot(Robot): self.is_connected = False self.logs = {} - - @property def state_feature(self) -> dict: return { @@ -128,41 +128,39 @@ class LeKiwiRobot(Robot): observation_socket.bind(f"tcp://*:{self.port_zmq_observations}") return context, cmd_socket, observation_socket - - def setup_actuators(self): + 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. - self.actuators_bus.write("Torque_Enable", TorqueMode.DISABLED.value,self.arm_actuators) - self.calibrate() # TODO(Steven): This should be only for the arm + self.actuators_bus.write("Torque_Enable", TorqueMode.DISABLED.value, self.arm_actuators) + 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) + self.actuators_bus.write("Mode", 0, self.arm_actuators) # Set P_Coefficient to lower value to avoid shakiness (Default is 32) - self.actuators_bus.write("P_Coefficient", 16,self.arm_actuators) + self.actuators_bus.write("P_Coefficient", 16, self.arm_actuators) # Set I_Coefficient and D_Coefficient to default value 0 and 32 self.actuators_bus.write("I_Coefficient", 0, self.arm_actuators) - self.actuators_bus.write("D_Coefficient", 32,self.arm_actuators) + self.actuators_bus.write("D_Coefficient", 32, self.arm_actuators) # Close the write lock so that Maximum_Acceleration gets written to EPROM address, # which is mandatory for Maximum_Acceleration to take effect after rebooting. - self.actuators_bus.write("Lock", 0,self.arm_actuators) + self.actuators_bus.write("Lock", 0, self.arm_actuators) # Set Maximum_Acceleration to 254 to speedup acceleration and deceleration of # the motors. Note: this configuration is not in the official STS3215 Memory Table - self.actuators_bus.write("Maximum_Acceleration", 254,self.arm_actuators) + self.actuators_bus.write("Maximum_Acceleration", 254, self.arm_actuators) self.actuators_bus.write("Acceleration", 254, self.arm_actuators) logging.info("Activating torque.") - self.actuators_bus.write("Torque_Enable", TorqueMode.ENABLED.value,self.arm_actuators) + self.actuators_bus.write("Torque_Enable", TorqueMode.ENABLED.value, self.arm_actuators) # Check arm can be read - self.actuators_bus.read("Present_Position",self.arm_actuators) + self.actuators_bus.read("Present_Position", self.arm_actuators) # Set-up base actuators (velocity mode) - self.actuators_bus.write("Lock",0,self.base_actuators) - self.actuators_bus.write("Mode",[1,1,1],self.base_actuators) - self.actuators_bus.write("Lock",1,self.base_actuators) - + self.actuators_bus.write("Lock", 0, self.base_actuators) + 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: @@ -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}'") - calibration = run_arm_manual_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(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( - "LeKiwiRobot is not connected. You need to run `robot.connect()`." - ) + raise DeviceNotConnectedError("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,22 +251,22 @@ class LeKiwiRobot(Robot): np.ndarray: the action sent to the motors, potentially clipped. """ if not self.is_connected: - raise DeviceNotConnectedError( - "LeKiwiRobot is not connected. You need to run `robot.connect()`." - ) + raise DeviceNotConnectedError("LeKiwiRobot is not connected. You need to run `robot.connect()`.") # Input action is in motor space - goal_pos=action + goal_pos = action # Cap goal position when too far away from present position. # /!\ 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) + 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 + ) # Send goal position to the actuators # TODO(Steven): This happens synchronously - self.actuators_bus.write("Goal_Position", goal_pos[:6].astype(np.int32),self.arm_actuators) - self.actuators_bus.write("Goal_Speed",goal_pos[6:].astype(np.int32),self.base_actuators) + self.actuators_bus.write("Goal_Position", goal_pos[:6].astype(np.int32), self.arm_actuators) + self.actuators_bus.write("Goal_Speed", goal_pos[6:].astype(np.int32), self.base_actuators) return goal_pos @@ -280,16 +280,14 @@ class LeKiwiRobot(Robot): def stop(self): # TODO(Steven): Assumes there's only 3 motors for base logging.info("Stopping base") - self.actuators_bus.write("Goal_Speed",[0,0,0],self.base_actuators) + self.actuators_bus.write("Goal_Speed", [0, 0, 0], self.base_actuators) logging.info("Base motors stopped") def run(self): # Copied logic from run_lekiwi in lekiwi_remote.py if not self.is_connected: - raise DeviceNotConnectedError( - "LeKiwiRobot is not connected. You need to run `robot.connect()`." - ) - + raise DeviceNotConnectedError("LeKiwiRobot is not connected. You need to run `robot.connect()`.") + stop_event = threading.Event() observation_thread = threading.Thread( target=self.update_last_observation, args=(stop_event), daemon=True @@ -311,17 +309,16 @@ 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 - + self.stop() + with self.observation_lock: self.zmq_observation_socket.send_string(json.dumps(self.last_observation)) - + # Ensure a short sleep to avoid overloading the CPU. elapsed = time.time() - loop_start_time time.sleep( @@ -333,8 +330,7 @@ class LeKiwiRobot(Robot): stop_event.set() observation_thread.join() self.disconnect() - pass - + def print_logs(self): # TODO(Steven): Refactor logger pass @@ -344,7 +340,7 @@ class LeKiwiRobot(Robot): raise DeviceNotConnectedError( "LeKiwi is not connected. You need to run `robot.connect()` before disconnecting." ) - + self.stop() self.actuators_bus.disconnect() for cam in self.cameras.values(): @@ -353,7 +349,7 @@ class LeKiwiRobot(Robot): self.cmd_socket.close() self.context.term() self.is_connected = False - + def __del__(self): if getattr(self, "is_connected", False): self.disconnect() diff --git a/lerobot/common/robots/robot.py b/lerobot/common/robots/robot.py index de908426..e3cca3de 100644 --- a/lerobot/common/robots/robot.py +++ b/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 diff --git a/lerobot/common/teleoperators/keyboard/teleop_keyboard.py b/lerobot/common/teleoperators/keyboard/teleop_keyboard.py index 1a679b18..1a8e0e96 100644 --- a/lerobot/common/teleoperators/keyboard/teleop_keyboard.py +++ b/lerobot/common/teleoperators/keyboard/teleop_keyboard.py @@ -64,7 +64,7 @@ class KeyboardTeleop(Teleoperator): @property def action_feature(self) -> dict: - #TODO(Steven): Verify this is correct + # TODO(Steven): Verify this is correct return { "dtype": "float32", "shape": (len(self.arm),), @@ -76,7 +76,7 @@ class KeyboardTeleop(Teleoperator): return {} def connect(self) -> None: - #TODO(Steven): Consider instead of raising a warning and then returning the status + # TODO(Steven): Consider instead of raising a warning and then returning the status # if self.is_connected: # logging.warning( # "ManipulatorRobot is already connected. Do not run `robot.connect()` twice."