diff --git a/lerobot/common/errors.py b/lerobot/common/errors.py index f37ac3e3..b064a295 100644 --- a/lerobot/common/errors.py +++ b/lerobot/common/errors.py @@ -16,4 +16,11 @@ class DeviceAlreadyConnectedError(ConnectionError): self.message = message super().__init__(self.message) -# TODO(Steven): Consider adding an InvalidActionError \ No newline at end of file +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 diff --git a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py index 6bd4f0d8..fe7e175b 100644 --- a/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py +++ b/lerobot/common/robots/lekiwi/configuration_daemon_lekiwi.py @@ -1,18 +1,10 @@ from dataclasses import dataclass, field -from lerobot.common.cameras.configs import CameraConfig -from lerobot.common.cameras.opencv.configuration_opencv import OpenCVCameraConfig -from lerobot.common.motors.configs import FeetechMotorsBusConfig, MotorsBusConfig from lerobot.common.robots.config import RobotConfig - @RobotConfig.register_subclass("daemon_lekiwi") @dataclass class DaemonLeKiwiRobotConfig(RobotConfig): - # `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes. - # Set this to a positive scalar to have the same value for all motors, or a list that is the same length as - # the number of motors in your follower arms. - max_relative_target: int | None = None # Network Configuration remote_ip: str = "192.168.0.193" diff --git a/lerobot/common/robots/lekiwi/configuration_lekiwi.py b/lerobot/common/robots/lekiwi/configuration_lekiwi.py index 37a88ded..3fd34d11 100644 --- a/lerobot/common/robots/lekiwi/configuration_lekiwi.py +++ b/lerobot/common/robots/lekiwi/configuration_lekiwi.py @@ -28,6 +28,8 @@ class LeKiwiRobotConfig(RobotConfig): port_motor_bus = "/dev/ttyACM0" # TODO(Steven): consider split this into arm and base + # TODO(Steven): Consider also removing this entirely as we can say that + # LeKiwiRobot will always have (and needs) such shoulder_pan: tuple = (1, "sts3215") shoulder_lift: tuple = (2, "sts3215") elbow_flex: tuple=(3, "sts3215") @@ -37,3 +39,8 @@ class LeKiwiRobotConfig(RobotConfig): 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. + # Set this to a positive scalar to have the same value for all motors, or a list that is the same length as + # the number of motors in your follower arms. + max_relative_target: int | None = None diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi.py b/lerobot/common/robots/lekiwi/daemon_lekiwi.py index 7d26c880..94c04d2f 100644 --- a/lerobot/common/robots/lekiwi/daemon_lekiwi.py +++ b/lerobot/common/robots/lekiwi/daemon_lekiwi.py @@ -16,20 +16,14 @@ import json import logging -import time -import threading import numpy as np -import time -# import torch import base64 import cv2 import torch -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 lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError, InvalidActionError +from ..robot import Robot, RobotMode from .configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig import zmq @@ -38,6 +32,12 @@ import zmq # 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: +# 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 +# over the teleop action space) class DaemonLeKiwiRobot(Robot): config_class = DaemonLeKiwiRobotConfig @@ -49,8 +49,6 @@ class DaemonLeKiwiRobot(Robot): self.id = config.id self.robot_type = config.type - self.max_relative_target = config.max_relative_target - self.remote_ip = config.remote_ip self.port_zmq_cmd = config.port_zmq_cmd self.port_zmq_observations = config.port_zmq_observations @@ -65,6 +63,7 @@ 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 @@ -73,23 +72,14 @@ class DaemonLeKiwiRobot(Robot): ] self.speed_index = 0 # Start at slow - # Keyboard state for base teleoperation. - # self.running = True - # self.pressed_keys = { - # "forward": False, - # "backward": False, - # "left": False, - # "right": False, - # "rotate_left": False, - # "rotate_right": False, - # } - self.is_connected = False self.logs = {} @property def state_feature(self) -> dict: # TODO(Steven): Get this from the data fetched? + # TODO(Steven): Motor names are unknown for the Daemon + # Or assume its size/metadata? # return { # "dtype": "float32", # "shape": (len(self.actuators),), @@ -103,7 +93,9 @@ class DaemonLeKiwiRobot(Robot): @property def camera_features(self) -> dict[str, dict]: - # TODO(Steven): Fetch this info or set it static? + # TODO(Steven): Get this from the data fetched? + # TODO(Steven): Motor names are unknown for the Daemon + # Or assume its size/metadata? # cam_ft = {} # for cam_key, cam in self.cameras.items(): # cam_ft[cam_key] = { @@ -134,8 +126,11 @@ class DaemonLeKiwiRobot(Robot): self.is_connected = True def calibrate(self) -> None: - # TODO(Steven): Nothing to calibrate - pass + # TODO(Steven): Nothing to calibrate. + # Consider triggering calibrate() on the remote mobile robot? + # Althought 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 @@ -267,12 +262,15 @@ class DaemonLeKiwiRobot(Robot): 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 the *latest* message, returning frames, speed, and arm state. If nothing arrives for any field, use the last known values.""" frames = {} present_speed = {} + + # TODO(Steven): Size is being assumed, is this safe? remote_arm_state_tensor = torch.zeros(6, dtype=torch.float32) # Poll up to 15 ms @@ -281,10 +279,13 @@ class DaemonLeKiwiRobot(Robot): socks = dict(poller.poll(15)) if self.zmq_observation_socket not in socks or socks[self.zmq_observation_socket] != zmq.POLLIN: # No new data arrived → reuse ALL old data + # TODO(Steven): This might return empty variables at init return (self.last_frames, self.last_present_speed, self.last_remote_arm_state) # Drain all messages, keep only the last last_msg = None + # TODO(Steven): There's probably a way to do this without while True + # TODO(Steven): Even consider changing to PUB/SUB while True: try: obs_string = self.zmq_observation_socket.recv_string(zmq.NOBLOCK) @@ -300,13 +301,11 @@ class DaemonLeKiwiRobot(Robot): try: observation = json.loads(last_msg) - #TODO(Steven): Check this - images_dict = observation.get("images", {}) - new_speed = observation.get("present_speed", {}) - new_arm_state = observation.get("follower_arm_state", None) + 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)} # Convert images - for cam_name, image_b64 in images_dict.items(): + for cam_name, image_b64 in image_observation.items(): if image_b64: jpg_data = base64.b64decode(image_b64) np_arr = np.frombuffer(jpg_data, dtype=np.uint8) @@ -315,19 +314,17 @@ class DaemonLeKiwiRobot(Robot): frames[cam_name] = frame_candidate # If remote_arm_state is None and frames is None there is no message then use the previous message - if new_arm_state is not None and frames is not None: + if state_observation is not None and frames is not None: self.last_frames = frames - remote_arm_state_tensor = torch.tensor(new_arm_state, dtype=torch.float32) + remote_arm_state_tensor = torch.tensor(state_observation[:6], dtype=torch.float32) self.last_remote_arm_state = remote_arm_state_tensor - present_speed = new_speed - self.last_present_speed = new_speed + present_speed = state_observation[6:] + self.last_present_speed = present_speed else: frames = self.last_frames - remote_arm_state_tensor = self.last_remote_arm_state - present_speed = self.last_present_speed except Exception as e: @@ -351,7 +348,6 @@ class DaemonLeKiwiRobot(Robot): obs_dict = {} - # TODO(Steven): Check this frames, present_speed, remote_arm_state_tensor = self.get_data() body_state = self.wheel_raw_to_body(present_speed) body_state_mm = (body_state[0] * 1000.0, body_state[1] * 1000.0, body_state[2]) # Convert x,y to mm/s @@ -361,18 +357,18 @@ class DaemonLeKiwiRobot(Robot): obs_dict = {OBS_STATE: combined_state_tensor} # Loop over each configured camera - for cam_name, cam in self.cameras.items(): - frame = frames.get(cam_name, None) + for cam_name, frame in frames.items(): if frame is None: - # Create a black image using the camera's configured width, height, and channels - frame = np.zeros((cam.height, cam.width, cam.channels), dtype=np.uint8) - obs_dict[f"{OBS_IMAGES}.{cam_name}"] = torch.from_numpy(frame) + # 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) + obs_dict[cam_name] = torch.from_numpy(frame) return obs_dict def from_keyboard_to_wheel_action(self, pressed_keys: np.ndarray): + # Speed control - # TODO(Steven): Handle the right action if self.teleop_keys["speed_up"] in pressed_keys: self.speed_index = min(self.speed_index + 1, 2) if self.teleop_keys["speed_down"] in pressed_keys: @@ -381,12 +377,10 @@ class DaemonLeKiwiRobot(Robot): 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 - # (The rest of your code for generating wheel commands remains unchanged) x_cmd = 0.0 # m/s forward/backward y_cmd = 0.0 # m/s lateral theta_cmd = 0.0 # deg/s rotation - # TODO(Steven): Handle action properly if self.teleop_keys["forward"] in pressed_keys: x_cmd += xy_speed if self.teleop_keys["backward"] in pressed_keys: @@ -402,13 +396,21 @@ class DaemonLeKiwiRobot(Robot): return self.body_to_wheel_raw(x_cmd, y_cmd, theta_cmd) + # TODO(Steven): This assumes this call is always called from a keyboard teleop command + # TODO(Steven): Doing this mapping in here adds latecy between send_action and movement from the user perspective. + # t0: get teleop_cmd + # t1: send_action(teleop_cmd) + # t2: mapping teleop_cmd -> motor_cmd + # t3: execute motor_md + # This mapping for other robots/teleop devices might be slower. Doing this in the teleop will make this explicit + # t0': get teleop_cmd + # t1': mapping teleop_cmd -> motor_cmd + # t2': send_action(motor_cmd) + # t3': execute motor_cmd + # t3'-t2' << t3-t1 def send_action(self, action: np.ndarray) -> np.ndarray: """Command lekiwi to move to a target joint configuration. - The relative action magnitude may be clipped depending on the configuration parameter - `max_relative_target`. In this case, the action sent differs from original action. - Thus, this function always returns the action actually sent. - Args: action (np.ndarray): array containing the goal positions for the motors. @@ -422,24 +424,24 @@ class DaemonLeKiwiRobot(Robot): raise DeviceNotConnectedError( "ManipulatorRobot is not connected. You need to run `robot.connect()`." ) - if self.mode is TELEOP: - # do conversion keys to motor - else: - # convert policy output - # TODO(Steven): This won't work if this is called by a policy with body vels outputs goal_pos: np.array = np.empty(9) - if action.size <6: - # TODO(Steven): Handle this properly + + if self.robot_mode is RobotMode.AUTO: + # TODO(Steven): Not yet implemented. The policy outputs might need a different conversion raise Exception - # TODO(Steven): Assumes size and order is respected - # TODO(Steven): This assumes this call is always called from a keyboard teleop command - 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)) + # TODO(Steven): This assumes teleop mode is always used with keyboard + if self.robot_mode is RobotMode.TELEOP: + 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 return goal_pos diff --git a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py b/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py index 2f907093..79c89f6b 100644 --- a/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py +++ b/lerobot/common/robots/lekiwi/daemon_lekiwi_app.py @@ -1,7 +1,7 @@ from ...teleoperators.so100 import SO100Teleop, SO100TeleopConfig from ...teleoperators.keyboard import KeyboardTeleop, KeyboardTeleopConfig from .configuration_daemon_lekiwi import DaemonLeKiwiRobotConfig -from .daemon_lekiwi import DaemonLeKiwiRobot +from .daemon_lekiwi import DaemonLeKiwiRobot, RobotMode import time import logging @@ -24,31 +24,28 @@ def main(): logging.info("Connecting remote LeKiwiRobot") 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() # 6 motors - base_action = keyboard.get_action() # n keys pressed + arm_action = leader_arm.get_action() + base_action = keyboard.get_action() action = { **arm_action, **base_action } - robot.set_mode(TELEOP) - action_sent = robot.send_action(action) # Translates to motor space + sends over ZMQ - obs = robot.get_observation() # Receives over ZMQ, translate to body-frame vel + _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) + # dataset.save(action_sent, obs) - # TODO(Steven) - robot.set_mode(AUTO) - policy_action = policy.get_action() # This might be in body frame or in key space - robot.send_action(policy_action) # This has no way to know - - teleop_step() # teleop - send_action() #policy + # TODO(Steven): Deal with policy action space + # 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 diff --git a/lerobot/common/robots/lekiwi/lekiwi_robot.py b/lerobot/common/robots/lekiwi/lekiwi_robot.py index d70196e9..b28629f1 100644 --- a/lerobot/common/robots/lekiwi/lekiwi_robot.py +++ b/lerobot/common/robots/lekiwi/lekiwi_robot.py @@ -20,7 +20,6 @@ import time import threading import numpy as np import time -# import torch import base64 import cv2 @@ -59,24 +58,29 @@ 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): We will need to have a key for arm and base for calibration - self.actuators = FeetechMotorsBus( + # TODO(Steven): Order and dimension are generaly assumed to be 6 first for arm, 3 last for base + self.actuators_bus = FeetechMotorsBus( port=self.config.port_motor_bus, motors={ - "shoulder_pan": config.shoulder_pan, - "shoulder_lift": config.shoulder_lift, - "elbow_flex": config.elbow_flex, - "wrist_flex": config.wrist_flex, - "wrist_roll": config.wrist_roll, - "gripper": config.gripper, - "left_wheel": config.left_wheel, - "right_wheel": config.right_wheel, - "back_wheel": config.back_wheel, + "arm_shoulder_pan": config.shoulder_pan, + "arm_shoulder_lift": config.shoulder_lift, + "arm_elbow_flex": config.elbow_flex, + "arm_wrist_flex": config.wrist_flex, + "arm_wrist_roll": config.wrist_roll, + "arm_gripper": config.gripper, + "base_left_wheel": config.left_wheel, + "base_right_wheel": config.right_wheel, + "base_back_wheel": config.back_wheel, }, ) + self.arm_actuators = [m for m in self.actuators_bus.motor_names if m.startswith("arm")] + self.base_actuators = [m for m in self.actuators_bus.motor_names if m.startswith("base")] + + self.max_relative_target = config.max_relative_target + #TODO(Steven): Consider removing cameras from configs - self.cameras = make_cameras_from_configs(config.cameras) + self.cameras = make_cameras_from_configs(config.cameras) self.observation_lock = threading.Lock() self.last_observation = None @@ -94,8 +98,8 @@ class LeKiwiRobot(Robot): def state_feature(self) -> dict: return { "dtype": "float32", - "shape": (len(self.actuators),), - "names": {"motors": list(self.actuators.motors)}, + "shape": (len(self.actuators_bus),), + "names": {"motors": list(self.actuators_bus.motors)}, } @property @@ -126,32 +130,39 @@ 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. - self.actuators.write("Torque_Enable", TorqueMode.DISABLED.value) - self.calibrate() + 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 - # TODO(Steven): Base robots should actually be in vel mode - self.actuators.write("Mode", 0) + self.actuators_bus.write("Mode", 0,self.arm_actuators) # Set P_Coefficient to lower value to avoid shakiness (Default is 32) - self.actuators.write("P_Coefficient", 16) + self.actuators_bus.write("P_Coefficient", 16,self.arm_actuators) # Set I_Coefficient and D_Coefficient to default value 0 and 32 - self.actuators.write("I_Coefficient", 0) - self.actuators.write("D_Coefficient", 32) + self.actuators_bus.write("I_Coefficient", 0, 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.write("Lock", 0) + 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.write("Maximum_Acceleration", 254) - self.actuators.write("Acceleration", 254) + 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.write("Torque_Enable", TorqueMode.ENABLED.value) + self.actuators_bus.write("Torque_Enable", TorqueMode.ENABLED.value,self.arm_actuators) # Check arm can be read - self.actuators.read("Present_Position") + 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) + def connect(self) -> None: if self.is_connected: @@ -160,7 +171,7 @@ class LeKiwiRobot(Robot): ) logging.info("Connecting actuators.") - self.actuators.connect() + self.actuators_bus.connect() self.setup_actuators() logging.info("Connecting cameras.") @@ -168,14 +179,10 @@ class LeKiwiRobot(Robot): cam.connect() logging.info("Connecting ZMQ sockets.") - self.zmq_context, self.zmq_cmd_socket, self.zmq_observation_socket = self.setup_zmq_sockets(self.config) + self.zmq_context, self.zmq_cmd_socket, self.zmq_observation_socket = self.setup_zmq_sockets() self.is_connected = True - # TODO(Steven): Consider using this - # def get_motor_names(self, arms: dict[str, MotorsBus]) -> list: - # return [f"{arm}_{motor}" for arm, bus in arms.items() for motor in bus.motors] - def calibrate(self) -> None: # Copied from S100 robot """After calibration all motors function in human interpretable ranges. @@ -189,14 +196,14 @@ class LeKiwiRobot(Robot): calibration = json.load(f) else: logging.info(f"Missing calibration file '{actuators_calib_path}'") - calibration = run_arm_manual_calibration(self.actuators, self.robot_type, self.name, "follower") + calibration = run_arm_manual_calibration(self.actuators_bus, self.robot_type, self.name, "follower") logging.info(f"Calibration is done! Saving calibration file '{actuators_calib_path}'") actuators_calib_path.parent.mkdir(parents=True, exist_ok=True) with open(actuators_calib_path, "w") as f: json.dump(calibration, f) - self.actuators.set_calibration(calibration) + self.actuators_bus.set_calibration(calibration) def get_observation(self) -> dict[str, np.ndarray]: """The returned observations do not have a batch dimension.""" @@ -207,10 +214,9 @@ class LeKiwiRobot(Robot): obs_dict = {} - # Read actuators position - # TODO(Steven): Base motors should return a vel instead of a pos + # Read actuators position for arm and vel for base before_read_t = time.perf_counter() - obs_dict[OBS_STATE] = self.actuators.read("Present_Position") + 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 @@ -249,17 +255,18 @@ class LeKiwiRobot(Robot): "LeKiwiRobot is not connected. You need to run `robot.connect()`." ) - goal_pos = action - + # Input action is in motor space + 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.read("Present_Position") - goal_pos = ensure_safe_goal_position(goal_pos, 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): Base motors should set a vel instead - self.actuators.write("Goal_Position", goal_pos.astype(np.int32)) + # 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) return goal_pos @@ -271,8 +278,10 @@ class LeKiwiRobot(Robot): # TODO(Steven): Consider adding a delay to not starve the CPU def stop(self): - # TODO(Steven): Base motors speed should be set to 0 - pass + # 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) + logging.info("Base motors stopped") def run(self): # Copied logic from run_lekiwi in lekiwi_remote.py @@ -297,7 +306,6 @@ class LeKiwiRobot(Robot): try: msg = self.cmd_socket.recv_string(zmq.NOBLOCK) data = json.loads(msg) - # TODO(Steven): Process data correctly self.send_action(data) last_cmd_time = time.time() # except zmq.Again: @@ -338,7 +346,7 @@ class LeKiwiRobot(Robot): ) self.stop() - self.actuators.disconnect() + self.actuators_bus.disconnect() for cam in self.cameras.values(): cam.disconnect() self.observation_socket.close() diff --git a/lerobot/common/robots/robot.py b/lerobot/common/robots/robot.py index a7ec4eda..de908426 100644 --- a/lerobot/common/robots/robot.py +++ b/lerobot/common/robots/robot.py @@ -9,6 +9,12 @@ from lerobot.common.motors import MotorCalibration from .config import RobotConfig +import enum + +class RobotMode(enum.Enum): + TELEOP = 0 + AUTO = 1 + # TODO(aliberts): action/obs typing such as Generic[ObsType, ActType] similar to gym.Env ? # https://github.com/Farama-Foundation/Gymnasium/blob/3287c869f9a48d99454306b0d4b4ec537f0f35e3/gymnasium/core.py#L23 @@ -21,6 +27,7 @@ class Robot(abc.ABC): def __init__(self, config: RobotConfig): self.robot_type = self.name + 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 diff --git a/lerobot/common/teleoperators/keyboard/configuration_keyboard.py b/lerobot/common/teleoperators/keyboard/configuration_keyboard.py index 18db787c..a1cfbbe7 100644 --- a/lerobot/common/teleoperators/keyboard/configuration_keyboard.py +++ b/lerobot/common/teleoperators/keyboard/configuration_keyboard.py @@ -22,5 +22,5 @@ from ..config import TeleoperatorConfig @TeleoperatorConfig.register_subclass("keyboard") @dataclass class KeyboardTeleopConfig(TeleoperatorConfig): - # TODO(Steven): Maybe set in here the keys that we want to capture + # TODO(Steven): Maybe set in here the keys that we want to capture/listen mock: bool = False