feat: is connect checks decorators (#2813)

This commit is contained in:
Steven Palma
2026-01-16 18:52:06 +01:00
committed by GitHub
parent 77dc49b3a3
commit 46e19ae579
22 changed files with 144 additions and 261 deletions
+7 -25
View File
@@ -32,7 +32,7 @@ import serial
from deepdiff import DeepDiff from deepdiff import DeepDiff
from tqdm import tqdm from tqdm import tqdm
from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
from lerobot.utils.utils import enter_pressed, move_cursor_up from lerobot.utils.utils import enter_pressed, move_cursor_up
NameOrID: TypeAlias = str | int NameOrID: TypeAlias = str | int
@@ -411,6 +411,7 @@ class MotorsBus(abc.ABC):
"""bool: `True` if the underlying serial port is open.""" """bool: `True` if the underlying serial port is open."""
return self.port_handler.is_open return self.port_handler.is_open
@check_if_already_connected
def connect(self, handshake: bool = True) -> None: def connect(self, handshake: bool = True) -> None:
"""Open the serial port and initialise communication. """Open the serial port and initialise communication.
@@ -422,10 +423,6 @@ class MotorsBus(abc.ABC):
DeviceAlreadyConnectedError: The port is already open. DeviceAlreadyConnectedError: The port is already open.
ConnectionError: The underlying SDK failed to open the port or the handshake did not succeed. ConnectionError: The underlying SDK failed to open the port or the handshake did not succeed.
""" """
if self.is_connected:
raise DeviceAlreadyConnectedError(
f"{self.__class__.__name__}('{self.port}') is already connected. Do not call `{self.__class__.__name__}.connect()` twice."
)
self._connect(handshake) self._connect(handshake)
self.set_timeout() self.set_timeout()
@@ -447,6 +444,7 @@ class MotorsBus(abc.ABC):
def _handshake(self) -> None: def _handshake(self) -> None:
pass pass
@check_if_not_connected
def disconnect(self, disable_torque: bool = True) -> None: def disconnect(self, disable_torque: bool = True) -> None:
"""Close the serial port (optionally disabling torque first). """Close the serial port (optionally disabling torque first).
@@ -455,10 +453,6 @@ class MotorsBus(abc.ABC):
closing the port. This can prevent damaging motors if they are left applying resisting torque closing the port. This can prevent damaging motors if they are left applying resisting torque
after disconnect. after disconnect.
""" """
if not self.is_connected:
raise DeviceNotConnectedError(
f"{self.__class__.__name__}('{self.port}') is not connected. Try running `{self.__class__.__name__}.connect()` first."
)
if disable_torque: if disable_torque:
self.port_handler.clearPort() self.port_handler.clearPort()
@@ -907,6 +901,7 @@ class MotorsBus(abc.ABC):
""" """
pass pass
@check_if_not_connected
def read( def read(
self, self,
data_name: str, data_name: str,
@@ -927,10 +922,6 @@ class MotorsBus(abc.ABC):
Returns: Returns:
Value: Raw or normalised value depending on *normalize*. Value: Raw or normalised value depending on *normalize*.
""" """
if not self.is_connected:
raise DeviceNotConnectedError(
f"{self.__class__.__name__}('{self.port}') is not connected. You need to run `{self.__class__.__name__}.connect()`."
)
id_ = self.motors[motor].id id_ = self.motors[motor].id
model = self.motors[motor].model model = self.motors[motor].model
@@ -981,6 +972,7 @@ class MotorsBus(abc.ABC):
return value, comm, error return value, comm, error
@check_if_not_connected
def write( def write(
self, data_name: str, motor: str, value: Value, *, normalize: bool = True, num_retry: int = 0 self, data_name: str, motor: str, value: Value, *, normalize: bool = True, num_retry: int = 0
) -> None: ) -> None:
@@ -999,10 +991,6 @@ class MotorsBus(abc.ABC):
normalize (bool, optional): Enable or disable normalisation. Defaults to `True`. normalize (bool, optional): Enable or disable normalisation. Defaults to `True`.
num_retry (int, optional): Retry attempts. Defaults to `0`. num_retry (int, optional): Retry attempts. Defaults to `0`.
""" """
if not self.is_connected:
raise DeviceNotConnectedError(
f"{self.__class__.__name__}('{self.port}') is not connected. You need to run `{self.__class__.__name__}.connect()`."
)
id_ = self.motors[motor].id id_ = self.motors[motor].id
model = self.motors[motor].model model = self.motors[motor].model
@@ -1044,6 +1032,7 @@ class MotorsBus(abc.ABC):
return comm, error return comm, error
@check_if_not_connected
def sync_read( def sync_read(
self, self,
data_name: str, data_name: str,
@@ -1063,10 +1052,6 @@ class MotorsBus(abc.ABC):
Returns: Returns:
dict[str, Value]: Mapping *motor name → value*. dict[str, Value]: Mapping *motor name → value*.
""" """
if not self.is_connected:
raise DeviceNotConnectedError(
f"{self.__class__.__name__}('{self.port}') is not connected. You need to run `{self.__class__.__name__}.connect()`."
)
self._assert_protocol_is_compatible("sync_read") self._assert_protocol_is_compatible("sync_read")
@@ -1139,6 +1124,7 @@ class MotorsBus(abc.ABC):
# for id_ in motor_ids: # for id_ in motor_ids:
# value = self.sync_reader.getData(id_, address, length) # value = self.sync_reader.getData(id_, address, length)
@check_if_not_connected
def sync_write( def sync_write(
self, self,
data_name: str, data_name: str,
@@ -1160,10 +1146,6 @@ class MotorsBus(abc.ABC):
normalize (bool, optional): If `True` (default) convert values from the user range to raw units. normalize (bool, optional): If `True` (default) convert values from the user range to raw units.
num_retry (int, optional): Retry attempts. Defaults to `0`. num_retry (int, optional): Retry attempts. Defaults to `0`.
""" """
if not self.is_connected:
raise DeviceNotConnectedError(
f"{self.__class__.__name__}('{self.port}') is not connected. You need to run `{self.__class__.__name__}.connect()`."
)
ids_values = self._get_ids_values_dict(values) ids_values = self._get_ids_values_dict(values)
models = [self._id_to_model(id_) for id_ in ids_values] models = [self._id_to_model(id_) for id_ in ids_values]
@@ -24,7 +24,8 @@ import numpy as np
import requests import requests
from lerobot.processor import RobotAction, RobotObservation from lerobot.processor import RobotAction, RobotObservation
from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
from lerobot.utils.errors import DeviceNotConnectedError
from ..robot import Robot from ..robot import Robot
from .config_earthrover_mini_plus import EarthRoverMiniPlusConfig from .config_earthrover_mini_plus import EarthRoverMiniPlusConfig
@@ -99,6 +100,7 @@ class EarthRoverMiniPlus(Robot):
"""Check if robot is connected to SDK.""" """Check if robot is connected to SDK."""
return self._is_connected return self._is_connected
@check_if_already_connected
def connect(self, calibrate: bool = True) -> None: def connect(self, calibrate: bool = True) -> None:
"""Connect to robot via Frodobots SDK. """Connect to robot via Frodobots SDK.
@@ -109,8 +111,6 @@ class EarthRoverMiniPlus(Robot):
DeviceAlreadyConnectedError: If robot is already connected DeviceAlreadyConnectedError: If robot is already connected
DeviceNotConnectedError: If cannot connect to SDK server DeviceNotConnectedError: If cannot connect to SDK server
""" """
if self._is_connected:
raise DeviceAlreadyConnectedError(f"{self.name} is already connected")
# Verify SDK is running and accessible # Verify SDK is running and accessible
try: try:
@@ -197,6 +197,7 @@ class EarthRoverMiniPlus(Robot):
ACTION_ANGULAR_VEL: float, ACTION_ANGULAR_VEL: float,
} }
@check_if_not_connected
def get_observation(self) -> RobotObservation: def get_observation(self) -> RobotObservation:
"""Get current robot observation from SDK. """Get current robot observation from SDK.
@@ -223,8 +224,6 @@ class EarthRoverMiniPlus(Robot):
Robot telemetry is retrieved from /data endpoint. Robot telemetry is retrieved from /data endpoint.
All SDK values are normalized to appropriate ranges for dataset recording. All SDK values are normalized to appropriate ranges for dataset recording.
""" """
if not self._is_connected:
raise DeviceNotConnectedError(f"{self.name} is not connected")
observation = {} observation = {}
@@ -255,6 +254,7 @@ class EarthRoverMiniPlus(Robot):
return observation return observation
@check_if_not_connected
def send_action(self, action: RobotAction) -> RobotAction: def send_action(self, action: RobotAction) -> RobotAction:
"""Send action to robot via SDK. """Send action to robot via SDK.
@@ -272,8 +272,6 @@ class EarthRoverMiniPlus(Robot):
Actions are sent to SDK via POST /control endpoint. Actions are sent to SDK via POST /control endpoint.
SDK expects commands in range [-1, 1]. SDK expects commands in range [-1, 1].
""" """
if not self._is_connected:
raise DeviceNotConnectedError(f"{self.name} is not connected")
# Extract action values and convert to float # Extract action values and convert to float
linear = float(action.get(ACTION_LINEAR_VEL, 0.0)) linear = float(action.get(ACTION_LINEAR_VEL, 0.0))
@@ -291,6 +289,7 @@ class EarthRoverMiniPlus(Robot):
ACTION_ANGULAR_VEL: angular, ACTION_ANGULAR_VEL: angular,
} }
@check_if_not_connected
def disconnect(self) -> None: def disconnect(self) -> None:
"""Disconnect from robot. """Disconnect from robot.
@@ -299,8 +298,6 @@ class EarthRoverMiniPlus(Robot):
Raises: Raises:
DeviceNotConnectedError: If robot is not connected DeviceNotConnectedError: If robot is not connected
""" """
if not self._is_connected:
raise DeviceNotConnectedError(f"{self.name} is not connected")
# Stop the robot before disconnecting # Stop the robot before disconnecting
try: try:
+5 -12
View File
@@ -25,7 +25,7 @@ from lerobot.motors.feetech import (
FeetechMotorsBus, FeetechMotorsBus,
) )
from lerobot.processor import RobotAction, RobotObservation from lerobot.processor import RobotAction, RobotObservation
from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
from ..robot import Robot from ..robot import Robot
from ..utils import ensure_safe_goal_position from ..utils import ensure_safe_goal_position
@@ -82,13 +82,12 @@ class HopeJrArm(Robot):
def is_connected(self) -> bool: def is_connected(self) -> bool:
return self.bus.is_connected and all(cam.is_connected for cam in self.cameras.values()) return self.bus.is_connected and all(cam.is_connected for cam in self.cameras.values())
@check_if_already_connected
def connect(self, calibrate: bool = True) -> None: def connect(self, calibrate: bool = True) -> None:
""" """
We assume that at connection time, arm is in a rest position, We assume that at connection time, arm is in a rest position,
and torque can be safely disabled to run calibration. and torque can be safely disabled to run calibration.
""" """
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} already connected")
self.bus.connect(handshake=False) self.bus.connect(handshake=False)
if not self.is_calibrated and calibrate: if not self.is_calibrated and calibrate:
@@ -128,10 +127,8 @@ class HopeJrArm(Robot):
self.bus.setup_motor(motor) self.bus.setup_motor(motor)
print(f"'{motor}' motor id set to {self.bus.motors[motor].id}") print(f"'{motor}' motor id set to {self.bus.motors[motor].id}")
@check_if_not_connected
def get_observation(self) -> RobotObservation: def get_observation(self) -> RobotObservation:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
# Read arm position # Read arm position
start = time.perf_counter() start = time.perf_counter()
obs_dict = self.bus.sync_read("Present_Position", self.other_motors) obs_dict = self.bus.sync_read("Present_Position", self.other_motors)
@@ -149,10 +146,8 @@ class HopeJrArm(Robot):
return obs_dict return obs_dict
@check_if_not_connected
def send_action(self, action: RobotAction) -> RobotAction: def send_action(self, action: RobotAction) -> RobotAction:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
goal_pos = {key.removesuffix(".pos"): val for key, val in action.items() if key.endswith(".pos")} goal_pos = {key.removesuffix(".pos"): val for key, val in action.items() if key.endswith(".pos")}
# Cap goal position when too far away from present position. # Cap goal position when too far away from present position.
@@ -165,10 +160,8 @@ class HopeJrArm(Robot):
self.bus.sync_write("Goal_Position", goal_pos) self.bus.sync_write("Goal_Position", goal_pos)
return {f"{motor}.pos": val for motor, val in goal_pos.items()} return {f"{motor}.pos": val for motor, val in goal_pos.items()}
@check_if_not_connected
def disconnect(self): def disconnect(self):
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
self.bus.disconnect(self.config.disable_torque_on_disconnect) self.bus.disconnect(self.config.disable_torque_on_disconnect)
for cam in self.cameras.values(): for cam in self.cameras.values():
cam.disconnect() cam.disconnect()
+5 -13
View File
@@ -25,7 +25,7 @@ from lerobot.motors.feetech import (
FeetechMotorsBus, FeetechMotorsBus,
) )
from lerobot.processor import RobotAction, RobotObservation from lerobot.processor import RobotAction, RobotObservation
from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
from ..robot import Robot from ..robot import Robot
from .config_hope_jr import HopeJrHandConfig from .config_hope_jr import HopeJrHandConfig
@@ -118,10 +118,8 @@ class HopeJrHand(Robot):
def is_connected(self) -> bool: def is_connected(self) -> bool:
return self.bus.is_connected and all(cam.is_connected for cam in self.cameras.values()) return self.bus.is_connected and all(cam.is_connected for cam in self.cameras.values())
@check_if_already_connected
def connect(self, calibrate: bool = True) -> None: def connect(self, calibrate: bool = True) -> None:
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} already connected")
self.bus.connect() self.bus.connect()
if not self.is_calibrated and calibrate: if not self.is_calibrated and calibrate:
self.calibrate() self.calibrate()
@@ -159,10 +157,8 @@ class HopeJrHand(Robot):
self.bus.setup_motor(motor) self.bus.setup_motor(motor)
print(f"'{motor}' motor id set to {self.bus.motors[motor].id}") print(f"'{motor}' motor id set to {self.bus.motors[motor].id}")
@check_if_not_connected
def get_observation(self) -> RobotObservation: def get_observation(self) -> RobotObservation:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
obs_dict = {} obs_dict = {}
# Read hand position # Read hand position
@@ -181,18 +177,14 @@ class HopeJrHand(Robot):
return obs_dict return obs_dict
@check_if_not_connected
def send_action(self, action: RobotAction) -> RobotAction: def send_action(self, action: RobotAction) -> RobotAction:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
goal_pos = {key.removesuffix(".pos"): val for key, val in action.items() if key.endswith(".pos")} goal_pos = {key.removesuffix(".pos"): val for key, val in action.items() if key.endswith(".pos")}
self.bus.sync_write("Goal_Position", goal_pos) self.bus.sync_write("Goal_Position", goal_pos)
return action return action
@check_if_not_connected
def disconnect(self): def disconnect(self):
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
self.bus.disconnect(self.config.disable_torque_on_disconnect) self.bus.disconnect(self.config.disable_torque_on_disconnect)
for cam in self.cameras.values(): for cam in self.cameras.values():
cam.disconnect() cam.disconnect()
@@ -25,7 +25,7 @@ from lerobot.motors.dynamixel import (
OperatingMode, OperatingMode,
) )
from lerobot.processor import RobotAction, RobotObservation from lerobot.processor import RobotAction, RobotObservation
from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
from ..robot import Robot from ..robot import Robot
from ..utils import ensure_safe_goal_position from ..utils import ensure_safe_goal_position
@@ -84,13 +84,12 @@ class KochFollower(Robot):
def is_connected(self) -> bool: def is_connected(self) -> bool:
return self.bus.is_connected and all(cam.is_connected for cam in self.cameras.values()) return self.bus.is_connected and all(cam.is_connected for cam in self.cameras.values())
@check_if_already_connected
def connect(self, calibrate: bool = True) -> None: def connect(self, calibrate: bool = True) -> None:
""" """
We assume that at connection time, arm is in a rest position, We assume that at connection time, arm is in a rest position,
and torque can be safely disabled to run calibration. and torque can be safely disabled to run calibration.
""" """
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} already connected")
self.bus.connect() self.bus.connect()
if not self.is_calibrated and calibrate: if not self.is_calibrated and calibrate:
@@ -182,10 +181,8 @@ class KochFollower(Robot):
self.bus.setup_motor(motor) self.bus.setup_motor(motor)
print(f"'{motor}' motor id set to {self.bus.motors[motor].id}") print(f"'{motor}' motor id set to {self.bus.motors[motor].id}")
@check_if_not_connected
def get_observation(self) -> RobotObservation: def get_observation(self) -> RobotObservation:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
# Read arm position # Read arm position
start = time.perf_counter() start = time.perf_counter()
obs_dict = self.bus.sync_read("Present_Position") obs_dict = self.bus.sync_read("Present_Position")
@@ -202,6 +199,7 @@ class KochFollower(Robot):
return obs_dict return obs_dict
@check_if_not_connected
def send_action(self, action: RobotAction) -> RobotAction: def send_action(self, action: RobotAction) -> RobotAction:
"""Command arm to move to a target joint configuration. """Command arm to move to a target joint configuration.
@@ -215,8 +213,6 @@ class KochFollower(Robot):
Returns: Returns:
RobotAction: The action sent to the motors, potentially clipped. RobotAction: The action sent to the motors, potentially clipped.
""" """
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
goal_pos = {key.removesuffix(".pos"): val for key, val in action.items() if key.endswith(".pos")} goal_pos = {key.removesuffix(".pos"): val for key, val in action.items() if key.endswith(".pos")}
@@ -231,10 +227,8 @@ class KochFollower(Robot):
self.bus.sync_write("Goal_Position", goal_pos) self.bus.sync_write("Goal_Position", goal_pos)
return {f"{motor}.pos": val for motor, val in goal_pos.items()} return {f"{motor}.pos": val for motor, val in goal_pos.items()}
@check_if_not_connected
def disconnect(self): def disconnect(self):
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
self.bus.disconnect(self.config.disable_torque_on_disconnect) self.bus.disconnect(self.config.disable_torque_on_disconnect)
for cam in self.cameras.values(): for cam in self.cameras.values():
cam.disconnect() cam.disconnect()
+5 -12
View File
@@ -29,7 +29,7 @@ from lerobot.motors.feetech import (
OperatingMode, OperatingMode,
) )
from lerobot.processor import RobotAction, RobotObservation from lerobot.processor import RobotAction, RobotObservation
from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
from ..robot import Robot from ..robot import Robot
from ..utils import ensure_safe_goal_position from ..utils import ensure_safe_goal_position
@@ -109,10 +109,8 @@ class LeKiwi(Robot):
def is_connected(self) -> bool: def is_connected(self) -> bool:
return self.bus.is_connected and all(cam.is_connected for cam in self.cameras.values()) return self.bus.is_connected and all(cam.is_connected for cam in self.cameras.values())
@check_if_already_connected
def connect(self, calibrate: bool = True) -> None: def connect(self, calibrate: bool = True) -> None:
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} already connected")
self.bus.connect() self.bus.connect()
if not self.is_calibrated and calibrate: if not self.is_calibrated and calibrate:
logger.info( logger.info(
@@ -339,10 +337,8 @@ class LeKiwi(Robot):
"theta.vel": theta, "theta.vel": theta,
} # m/s and deg/s } # m/s and deg/s
@check_if_not_connected
def get_observation(self) -> RobotObservation: def get_observation(self) -> RobotObservation:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
# Read actuators position for arm and vel for base # Read actuators position for arm and vel for base
start = time.perf_counter() start = time.perf_counter()
arm_pos = self.bus.sync_read("Present_Position", self.arm_motors) arm_pos = self.bus.sync_read("Present_Position", self.arm_motors)
@@ -370,6 +366,7 @@ class LeKiwi(Robot):
return obs_dict return obs_dict
@check_if_not_connected
def send_action(self, action: RobotAction) -> RobotAction: def send_action(self, action: RobotAction) -> RobotAction:
"""Command lekiwi to move to a target joint configuration. """Command lekiwi to move to a target joint configuration.
@@ -383,8 +380,6 @@ class LeKiwi(Robot):
Returns: Returns:
RobotAction: the action sent to the motors, potentially clipped. RobotAction: the action sent to the motors, potentially clipped.
""" """
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
arm_goal_pos = {k: v for k, v in action.items() if k.endswith(".pos")} arm_goal_pos = {k: v for k, v in action.items() if k.endswith(".pos")}
base_goal_vel = {k: v for k, v in action.items() if k.endswith(".vel")} base_goal_vel = {k: v for k, v in action.items() if k.endswith(".vel")}
@@ -412,10 +407,8 @@ class LeKiwi(Robot):
self.bus.sync_write("Goal_Velocity", dict.fromkeys(self.base_motors, 0), num_retry=5) self.bus.sync_write("Goal_Velocity", dict.fromkeys(self.base_motors, 0), num_retry=5)
logger.info("Base motors stopped") logger.info("Base motors stopped")
@check_if_not_connected
def disconnect(self): def disconnect(self):
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
self.stop_base() self.stop_base()
self.bus.disconnect(self.config.disable_torque_on_disconnect) self.bus.disconnect(self.config.disable_torque_on_disconnect)
for cam in self.cameras.values(): for cam in self.cameras.values():
+6 -16
View File
@@ -24,7 +24,8 @@ import numpy as np
from lerobot.processor import RobotAction, RobotObservation from lerobot.processor import RobotAction, RobotObservation
from lerobot.utils.constants import ACTION, OBS_STATE from lerobot.utils.constants import ACTION, OBS_STATE
from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
from lerobot.utils.errors import DeviceNotConnectedError
from ..robot import Robot from ..robot import Robot
from .config_lekiwi import LeKiwiClientConfig from .config_lekiwi import LeKiwiClientConfig
@@ -112,14 +113,10 @@ class LeKiwiClient(Robot):
def is_calibrated(self) -> bool: def is_calibrated(self) -> bool:
pass pass
@check_if_already_connected
def connect(self) -> None: def connect(self) -> None:
"""Establishes ZMQ sockets with the remote mobile robot""" """Establishes ZMQ sockets with the remote mobile robot"""
if self._is_connected:
raise DeviceAlreadyConnectedError(
"LeKiwi Daemon is already connected. Do not run `robot.connect()` twice."
)
zmq = self._zmq zmq = self._zmq
self.zmq_context = zmq.Context() self.zmq_context = zmq.Context()
self.zmq_cmd_socket = self.zmq_context.socket(zmq.PUSH) self.zmq_cmd_socket = self.zmq_context.socket(zmq.PUSH)
@@ -252,14 +249,13 @@ class LeKiwiClient(Robot):
return new_frames, new_state return new_frames, new_state
@check_if_not_connected
def get_observation(self) -> RobotObservation: def get_observation(self) -> RobotObservation:
""" """
Capture observations from the remote robot: current follower arm positions, Capture observations from the remote robot: current follower arm positions,
present wheel speeds (converted to body-frame velocities: x, y, theta), present wheel speeds (converted to body-frame velocities: x, y, theta),
and a camera frame. Receives over ZMQ, translate to body-frame vel and a camera frame. Receives over ZMQ, translate to body-frame vel
""" """
if not self._is_connected:
raise DeviceNotConnectedError("LeKiwiClient is not connected. You need to run `robot.connect()`.")
frames, obs_dict = self._get_data() frames, obs_dict = self._get_data()
@@ -307,6 +303,7 @@ class LeKiwiClient(Robot):
def configure(self): def configure(self):
pass pass
@check_if_not_connected
def send_action(self, action: RobotAction) -> RobotAction: def send_action(self, action: RobotAction) -> RobotAction:
"""Command lekiwi to move to a target joint configuration. Translates to motor space + sends over ZMQ """Command lekiwi to move to a target joint configuration. Translates to motor space + sends over ZMQ
@@ -318,10 +315,6 @@ class LeKiwiClient(Robot):
Returns: Returns:
np.ndarray: the action sent to the motors, potentially clipped. np.ndarray: the action sent to the motors, potentially clipped.
""" """
if not self._is_connected:
raise DeviceNotConnectedError(
"ManipulatorRobot is not connected. You need to run `robot.connect()`."
)
self.zmq_cmd_socket.send_string(json.dumps(action)) # action is in motor space self.zmq_cmd_socket.send_string(json.dumps(action)) # action is in motor space
@@ -332,13 +325,10 @@ class LeKiwiClient(Robot):
action_sent[ACTION] = actions action_sent[ACTION] = actions
return action_sent return action_sent
@check_if_not_connected
def disconnect(self): def disconnect(self):
"""Cleans ZMQ comms""" """Cleans ZMQ comms"""
if not self._is_connected:
raise DeviceNotConnectedError(
"LeKiwi is not connected. You need to run `robot.connect()` before disconnecting."
)
self.zmq_observation_socket.close() self.zmq_observation_socket.close()
self.zmq_cmd_socket.close() self.zmq_cmd_socket.close()
self.zmq_context.term() self.zmq_context.term()
@@ -26,7 +26,7 @@ from lerobot.motors.dynamixel import (
OperatingMode, OperatingMode,
) )
from lerobot.processor import RobotAction, RobotObservation from lerobot.processor import RobotAction, RobotObservation
from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
from ..robot import Robot from ..robot import Robot
from ..utils import ensure_safe_goal_position from ..utils import ensure_safe_goal_position
@@ -84,6 +84,7 @@ class OmxFollower(Robot):
def is_connected(self) -> bool: def is_connected(self) -> bool:
return self.bus.is_connected and all(cam.is_connected for cam in self.cameras.values()) return self.bus.is_connected and all(cam.is_connected for cam in self.cameras.values())
@check_if_already_connected
def connect(self, calibrate: bool = True) -> None: def connect(self, calibrate: bool = True) -> None:
""" """
For OMX robots that come pre-calibrated: For OMX robots that come pre-calibrated:
@@ -91,8 +92,6 @@ class OmxFollower(Robot):
- This allows using pre-calibrated robots without manual calibration - This allows using pre-calibrated robots without manual calibration
- If no calibration file exists, use factory default values (homing_offset=0, range_min=0, range_max=4095) - If no calibration file exists, use factory default values (homing_offset=0, range_min=0, range_max=4095)
""" """
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} already connected")
self.bus.connect() self.bus.connect()
if not self.is_calibrated and calibrate: if not self.is_calibrated and calibrate:
@@ -165,10 +164,8 @@ class OmxFollower(Robot):
self.bus.setup_motor(motor) self.bus.setup_motor(motor)
print(f"'{motor}' motor id set to {self.bus.motors[motor].id}") print(f"'{motor}' motor id set to {self.bus.motors[motor].id}")
@check_if_not_connected
def get_observation(self) -> RobotObservation: def get_observation(self) -> RobotObservation:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
# Read arm position # Read arm position
start = time.perf_counter() start = time.perf_counter()
obs_dict = self.bus.sync_read("Present_Position") obs_dict = self.bus.sync_read("Present_Position")
@@ -185,6 +182,7 @@ class OmxFollower(Robot):
return obs_dict return obs_dict
@check_if_not_connected
def send_action(self, action: RobotAction) -> RobotAction: def send_action(self, action: RobotAction) -> RobotAction:
"""Command arm to move to a target joint configuration. """Command arm to move to a target joint configuration.
@@ -198,8 +196,6 @@ class OmxFollower(Robot):
Returns: Returns:
RobotAction: The action sent to the motors, potentially clipped. RobotAction: The action sent to the motors, potentially clipped.
""" """
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
goal_pos = {key.removesuffix(".pos"): val for key, val in action.items() if key.endswith(".pos")} goal_pos = {key.removesuffix(".pos"): val for key, val in action.items() if key.endswith(".pos")}
@@ -214,10 +210,8 @@ class OmxFollower(Robot):
self.bus.sync_write("Goal_Position", goal_pos) self.bus.sync_write("Goal_Position", goal_pos)
return {f"{motor}.pos": val for motor, val in goal_pos.items()} return {f"{motor}.pos": val for motor, val in goal_pos.items()}
@check_if_not_connected
def disconnect(self): def disconnect(self):
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
self.bus.disconnect(self.config.disable_torque_on_disconnect) self.bus.disconnect(self.config.disable_torque_on_disconnect)
for cam in self.cameras.values(): for cam in self.cameras.values():
cam.disconnect() cam.disconnect()
+5 -11
View File
@@ -26,7 +26,7 @@ from lerobot.motors.feetech import (
OperatingMode, OperatingMode,
) )
from lerobot.processor import RobotAction, RobotObservation from lerobot.processor import RobotAction, RobotObservation
from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
from ..robot import Robot from ..robot import Robot
from ..utils import ensure_safe_goal_position from ..utils import ensure_safe_goal_position
@@ -85,13 +85,12 @@ class SOFollower(Robot):
def is_connected(self) -> bool: def is_connected(self) -> bool:
return self.bus.is_connected and all(cam.is_connected for cam in self.cameras.values()) return self.bus.is_connected and all(cam.is_connected for cam in self.cameras.values())
@check_if_already_connected
def connect(self, calibrate: bool = True) -> None: def connect(self, calibrate: bool = True) -> None:
""" """
We assume that at connection time, arm is in a rest position, We assume that at connection time, arm is in a rest position,
and torque can be safely disabled to run calibration. and torque can be safely disabled to run calibration.
""" """
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} already connected")
self.bus.connect() self.bus.connect()
if not self.is_calibrated and calibrate: if not self.is_calibrated and calibrate:
@@ -176,10 +175,8 @@ class SOFollower(Robot):
self.bus.setup_motor(motor) self.bus.setup_motor(motor)
print(f"'{motor}' motor id set to {self.bus.motors[motor].id}") print(f"'{motor}' motor id set to {self.bus.motors[motor].id}")
@check_if_not_connected
def get_observation(self) -> RobotObservation: def get_observation(self) -> RobotObservation:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
# Read arm position # Read arm position
start = time.perf_counter() start = time.perf_counter()
obs_dict = self.bus.sync_read("Present_Position") obs_dict = self.bus.sync_read("Present_Position")
@@ -196,6 +193,7 @@ class SOFollower(Robot):
return obs_dict return obs_dict
@check_if_not_connected
def send_action(self, action: RobotAction) -> RobotAction: def send_action(self, action: RobotAction) -> RobotAction:
"""Command arm to move to a target joint configuration. """Command arm to move to a target joint configuration.
@@ -209,8 +207,6 @@ class SOFollower(Robot):
Returns: Returns:
RobotAction: the action sent to the motors, potentially clipped. RobotAction: the action sent to the motors, potentially clipped.
""" """
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
goal_pos = {key.removesuffix(".pos"): val for key, val in action.items() if key.endswith(".pos")} goal_pos = {key.removesuffix(".pos"): val for key, val in action.items() if key.endswith(".pos")}
@@ -225,10 +221,8 @@ class SOFollower(Robot):
self.bus.sync_write("Goal_Position", goal_pos) self.bus.sync_write("Goal_Position", goal_pos)
return {f"{motor}.pos": val for motor, val in goal_pos.items()} return {f"{motor}.pos": val for motor, val in goal_pos.items()}
@check_if_not_connected
def disconnect(self): def disconnect(self):
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
self.bus.disconnect(self.config.disable_torque_on_disconnect) self.bus.disconnect(self.config.disable_torque_on_disconnect)
for cam in self.cameras.values(): for cam in self.cameras.values():
cam.disconnect() cam.disconnect()
@@ -18,7 +18,7 @@ import logging
from functools import cached_property from functools import cached_property
from lerobot.teleoperators.so_leader import SOLeaderTeleopConfig from lerobot.teleoperators.so_leader import SOLeaderTeleopConfig
from lerobot.utils.errors import DeviceNotConnectedError from lerobot.utils.decorators import check_if_not_connected
from ..so_leader import SOLeader from ..so_leader import SOLeader
from ..teleoperator import Teleoperator from ..teleoperator import Teleoperator
@@ -92,10 +92,8 @@ class BiSOLeader(Teleoperator):
self.left_arm.setup_motors() self.left_arm.setup_motors()
self.right_arm.setup_motors() self.right_arm.setup_motors()
@check_if_not_connected
def get_action(self) -> dict[str, float]: def get_action(self) -> dict[str, float]:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
action_dict = {} action_dict = {}
# Add "left_" prefix # Add "left_" prefix
@@ -21,7 +21,7 @@ from typing import Any
import numpy as np import numpy as np
from lerobot.processor import RobotAction from lerobot.processor import RobotAction
from lerobot.utils.errors import DeviceNotConnectedError from lerobot.utils.decorators import check_if_not_connected
from ..teleoperator import Teleoperator from ..teleoperator import Teleoperator
from ..utils import TeleopEvents from ..utils import TeleopEvents
@@ -86,10 +86,8 @@ class GamepadTeleop(Teleoperator):
self.gamepad = Gamepad() self.gamepad = Gamepad()
self.gamepad.start() self.gamepad.start()
@check_if_not_connected
def get_action(self) -> RobotAction: def get_action(self) -> RobotAction:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
# Update the controller to get fresh inputs # Update the controller to get fresh inputs
self.gamepad.update() self.gamepad.update()
@@ -22,7 +22,7 @@ from pprint import pformat
import serial import serial
from lerobot.motors.motors_bus import MotorCalibration, MotorNormMode from lerobot.motors.motors_bus import MotorCalibration, MotorNormMode
from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
from lerobot.utils.utils import enter_pressed, move_cursor_up from lerobot.utils.utils import enter_pressed, move_cursor_up
from ..teleoperator import Teleoperator from ..teleoperator import Teleoperator
@@ -93,10 +93,8 @@ class HomunculusArm(Teleoperator):
with self.serial_lock: with self.serial_lock:
return self.serial.is_open and self.thread.is_alive() return self.serial.is_open and self.thread.is_alive()
@check_if_already_connected
def connect(self, calibrate: bool = True) -> None: def connect(self, calibrate: bool = True) -> None:
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} already connected")
if not self.serial.is_open: if not self.serial.is_open:
self.serial.open() self.serial.open()
self.thread.start() self.thread.start()
@@ -299,20 +297,16 @@ class HomunculusArm(Teleoperator):
except Exception as e: except Exception as e:
logger.debug(f"Error reading frame in background thread for {self}: {e}") logger.debug(f"Error reading frame in background thread for {self}: {e}")
@check_if_not_connected
def get_action(self) -> dict[str, float]: def get_action(self) -> dict[str, float]:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
joint_positions = self._read() joint_positions = self._read()
return {f"{joint}.pos": pos for joint, pos in joint_positions.items()} return {f"{joint}.pos": pos for joint, pos in joint_positions.items()}
def send_feedback(self, feedback: dict[str, float]) -> None: def send_feedback(self, feedback: dict[str, float]) -> None:
raise NotImplementedError raise NotImplementedError
@check_if_not_connected
def disconnect(self) -> None: def disconnect(self) -> None:
if not self.is_connected:
DeviceNotConnectedError(f"{self} is not connected.")
self.stop_event.set() self.stop_event.set()
self.thread.join(timeout=1) self.thread.join(timeout=1)
self.serial.close() self.serial.close()
@@ -24,7 +24,7 @@ import serial
from lerobot.motors import MotorCalibration from lerobot.motors import MotorCalibration
from lerobot.motors.motors_bus import MotorNormMode from lerobot.motors.motors_bus import MotorNormMode
from lerobot.teleoperators.homunculus.joints_translation import homunculus_glove_to_hope_jr_hand from lerobot.teleoperators.homunculus.joints_translation import homunculus_glove_to_hope_jr_hand
from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
from lerobot.utils.utils import enter_pressed, move_cursor_up from lerobot.utils.utils import enter_pressed, move_cursor_up
from ..teleoperator import Teleoperator from ..teleoperator import Teleoperator
@@ -119,10 +119,8 @@ class HomunculusGlove(Teleoperator):
with self.serial_lock: with self.serial_lock:
return self.serial.is_open and self.thread.is_alive() return self.serial.is_open and self.thread.is_alive()
@check_if_already_connected
def connect(self, calibrate: bool = True) -> None: def connect(self, calibrate: bool = True) -> None:
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} already connected")
if not self.serial.is_open: if not self.serial.is_open:
self.serial.open() self.serial.open()
self.thread.start() self.thread.start()
@@ -325,10 +323,8 @@ class HomunculusGlove(Teleoperator):
except Exception as e: except Exception as e:
logger.debug(f"Error reading frame in background thread for {self}: {e}") logger.debug(f"Error reading frame in background thread for {self}: {e}")
@check_if_not_connected
def get_action(self) -> dict[str, float]: def get_action(self) -> dict[str, float]:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
joint_positions = self._read() joint_positions = self._read()
return homunculus_glove_to_hope_jr_hand( return homunculus_glove_to_hope_jr_hand(
{f"{joint}.pos": pos for joint, pos in joint_positions.items()} {f"{joint}.pos": pos for joint, pos in joint_positions.items()}
@@ -337,10 +333,8 @@ class HomunculusGlove(Teleoperator):
def send_feedback(self, feedback: dict[str, float]) -> None: def send_feedback(self, feedback: dict[str, float]) -> None:
raise NotImplementedError raise NotImplementedError
@check_if_not_connected
def disconnect(self) -> None: def disconnect(self) -> None:
if not self.is_connected:
DeviceNotConnectedError(f"{self} is not connected.")
self.stop_event.set() self.stop_event.set()
self.thread.join(timeout=1) self.thread.join(timeout=1)
self.serial.close() self.serial.close()
@@ -22,7 +22,7 @@ from queue import Queue
from typing import Any from typing import Any
from lerobot.processor import RobotAction from lerobot.processor import RobotAction
from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
from ..teleoperator import Teleoperator from ..teleoperator import Teleoperator
from ..utils import TeleopEvents from ..utils import TeleopEvents
@@ -86,12 +86,8 @@ class KeyboardTeleop(Teleoperator):
def is_calibrated(self) -> bool: def is_calibrated(self) -> bool:
pass pass
@check_if_already_connected
def connect(self) -> None: def connect(self) -> None:
if self.is_connected:
raise DeviceAlreadyConnectedError(
"Keyboard is already connected. Do not run `robot.connect()` twice."
)
if PYNPUT_AVAILABLE: if PYNPUT_AVAILABLE:
logging.info("pynput is available - enabling local keyboard listener.") logging.info("pynput is available - enabling local keyboard listener.")
self.listener = keyboard.Listener( self.listener = keyboard.Listener(
@@ -125,14 +121,10 @@ class KeyboardTeleop(Teleoperator):
def configure(self): def configure(self):
pass pass
@check_if_not_connected
def get_action(self) -> RobotAction: def get_action(self) -> RobotAction:
before_read_t = time.perf_counter() before_read_t = time.perf_counter()
if not self.is_connected:
raise DeviceNotConnectedError(
"KeyboardTeleop is not connected. You need to run `connect()` before `get_action()`."
)
self._drain_pressed_keys() self._drain_pressed_keys()
# Generate action based on current key states # Generate action based on current key states
@@ -144,11 +136,8 @@ class KeyboardTeleop(Teleoperator):
def send_feedback(self, feedback: dict[str, Any]) -> None: def send_feedback(self, feedback: dict[str, Any]) -> None:
pass pass
@check_if_not_connected
def disconnect(self) -> None: def disconnect(self) -> None:
if not self.is_connected:
raise DeviceNotConnectedError(
"KeyboardTeleop is not connected. You need to run `robot.connect()` before `disconnect()`."
)
if self.listener is not None: if self.listener is not None:
self.listener.stop() self.listener.stop()
@@ -182,12 +171,8 @@ class KeyboardEndEffectorTeleop(KeyboardTeleop):
"names": {"delta_x": 0, "delta_y": 1, "delta_z": 2}, "names": {"delta_x": 0, "delta_y": 1, "delta_z": 2},
} }
@check_if_not_connected
def get_action(self) -> RobotAction: def get_action(self) -> RobotAction:
if not self.is_connected:
raise DeviceNotConnectedError(
"KeyboardTeleop is not connected. You need to run `connect()` before `get_action()`."
)
self._drain_pressed_keys() self._drain_pressed_keys()
delta_x = 0.0 delta_x = 0.0
delta_y = 0.0 delta_y = 0.0
@@ -375,6 +360,7 @@ class KeyboardRoverTeleop(KeyboardTeleop):
# Only remove key if it's being released # Only remove key if it's being released
self.current_pressed.pop(key_char, None) self.current_pressed.pop(key_char, None)
@check_if_not_connected
def get_action(self) -> RobotAction: def get_action(self) -> RobotAction:
""" """
Get the current action based on pressed keys. Get the current action based on pressed keys.
@@ -384,11 +370,6 @@ class KeyboardRoverTeleop(KeyboardTeleop):
""" """
before_read_t = time.perf_counter() before_read_t = time.perf_counter()
if not self.is_connected:
raise DeviceNotConnectedError(
"KeyboardRoverTeleop is not connected. You need to run `connect()` before `get_action()`."
)
self._drain_pressed_keys() self._drain_pressed_keys()
linear_velocity = 0.0 linear_velocity = 0.0
@@ -23,7 +23,7 @@ from lerobot.motors.dynamixel import (
DynamixelMotorsBus, DynamixelMotorsBus,
OperatingMode, OperatingMode,
) )
from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
from ..teleoperator import Teleoperator from ..teleoperator import Teleoperator
from .config_koch_leader import KochLeaderConfig from .config_koch_leader import KochLeaderConfig
@@ -69,10 +69,8 @@ class KochLeader(Teleoperator):
def is_connected(self) -> bool: def is_connected(self) -> bool:
return self.bus.is_connected return self.bus.is_connected
@check_if_already_connected
def connect(self, calibrate: bool = True) -> None: def connect(self, calibrate: bool = True) -> None:
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} already connected")
self.bus.connect() self.bus.connect()
if not self.is_calibrated and calibrate: if not self.is_calibrated and calibrate:
logger.info( logger.info(
@@ -161,10 +159,8 @@ class KochLeader(Teleoperator):
self.bus.setup_motor(motor) self.bus.setup_motor(motor)
print(f"'{motor}' motor id set to {self.bus.motors[motor].id}") print(f"'{motor}' motor id set to {self.bus.motors[motor].id}")
@check_if_not_connected
def get_action(self) -> dict[str, float]: def get_action(self) -> dict[str, float]:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
start = time.perf_counter() start = time.perf_counter()
action = self.bus.sync_read("Present_Position") action = self.bus.sync_read("Present_Position")
action = {f"{motor}.pos": val for motor, val in action.items()} action = {f"{motor}.pos": val for motor, val in action.items()}
@@ -176,9 +172,7 @@ class KochLeader(Teleoperator):
# TODO(rcadene, aliberts): Implement force feedback # TODO(rcadene, aliberts): Implement force feedback
raise NotImplementedError raise NotImplementedError
@check_if_not_connected
def disconnect(self) -> None: def disconnect(self) -> None:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
self.bus.disconnect() self.bus.disconnect()
logger.info(f"{self} disconnected.") logger.info(f"{self} disconnected.")
@@ -23,7 +23,7 @@ from lerobot.motors.dynamixel import (
DynamixelMotorsBus, DynamixelMotorsBus,
OperatingMode, OperatingMode,
) )
from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
from ..teleoperator import Teleoperator from ..teleoperator import Teleoperator
from .config_omx_leader import OmxLeaderConfig from .config_omx_leader import OmxLeaderConfig
@@ -68,10 +68,8 @@ class OmxLeader(Teleoperator):
def is_connected(self) -> bool: def is_connected(self) -> bool:
return self.bus.is_connected return self.bus.is_connected
@check_if_already_connected
def connect(self, calibrate: bool = True) -> None: def connect(self, calibrate: bool = True) -> None:
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} already connected")
self.bus.connect() self.bus.connect()
if not self.is_calibrated and calibrate: if not self.is_calibrated and calibrate:
logger.info( logger.info(
@@ -142,10 +140,8 @@ class OmxLeader(Teleoperator):
self.bus.setup_motor(motor) self.bus.setup_motor(motor)
print(f"'{motor}' motor id set to {self.bus.motors[motor].id}") print(f"'{motor}' motor id set to {self.bus.motors[motor].id}")
@check_if_not_connected
def get_action(self) -> dict[str, float]: def get_action(self) -> dict[str, float]:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
start = time.perf_counter() start = time.perf_counter()
action = self.bus.sync_read("Present_Position") action = self.bus.sync_read("Present_Position")
action = {f"{motor}.pos": val for motor, val in action.items()} action = {f"{motor}.pos": val for motor, val in action.items()}
@@ -157,9 +153,7 @@ class OmxLeader(Teleoperator):
# TODO(rcadene, aliberts): Implement force feedback # TODO(rcadene, aliberts): Implement force feedback
raise NotImplementedError raise NotImplementedError
@check_if_not_connected
def disconnect(self) -> None: def disconnect(self) -> None:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
self.bus.disconnect() self.bus.disconnect()
logger.info(f"{self} disconnected.") logger.info(f"{self} disconnected.")
@@ -28,7 +28,7 @@ from teleop import Teleop
from lerobot.teleoperators.phone.config_phone import PhoneConfig, PhoneOS from lerobot.teleoperators.phone.config_phone import PhoneConfig, PhoneOS
from lerobot.teleoperators.teleoperator import Teleoperator from lerobot.teleoperators.teleoperator import Teleoperator
from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
from lerobot.utils.rotation import Rotation from lerobot.utils.rotation import Rotation
logger = logging.getLogger(__name__) logger = logging.getLogger(__name__)
@@ -81,10 +81,8 @@ class IOSPhone(BasePhone, Teleoperator):
def is_connected(self) -> bool: def is_connected(self) -> bool:
return self._group is not None return self._group is not None
@check_if_already_connected
def connect(self) -> None: def connect(self) -> None:
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} already connected")
logger.info("Connecting to IPhone, make sure to open the HEBI Mobile I/O app.") logger.info("Connecting to IPhone, make sure to open the HEBI Mobile I/O app.")
lookup = hebi.Lookup() lookup = hebi.Lookup()
time.sleep(2.0) time.sleep(2.0)
@@ -164,10 +162,8 @@ class IOSPhone(BasePhone, Teleoperator):
pos = ar_pos - rot.apply(self.config.camera_offset) pos = ar_pos - rot.apply(self.config.camera_offset)
return True, pos, rot, pose return True, pos, rot, pose
@check_if_not_connected
def get_action(self) -> dict: def get_action(self) -> dict:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
has_pose, raw_position, raw_rotation, fb_pose = self._read_current_pose() has_pose, raw_position, raw_rotation, fb_pose = self._read_current_pose()
if not has_pose or not self.is_calibrated: if not has_pose or not self.is_calibrated:
return {} return {}
@@ -207,10 +203,8 @@ class IOSPhone(BasePhone, Teleoperator):
"phone.enabled": self._enabled, "phone.enabled": self._enabled,
} }
@check_if_not_connected
def disconnect(self) -> None: def disconnect(self) -> None:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
self._group = None self._group = None
@@ -230,10 +224,8 @@ class AndroidPhone(BasePhone, Teleoperator):
def is_connected(self) -> bool: def is_connected(self) -> bool:
return self._teleop is not None return self._teleop is not None
@check_if_already_connected
def connect(self) -> None: def connect(self) -> None:
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} already connected")
logger.info("Starting teleop stream for Android...") logger.info("Starting teleop stream for Android...")
self._teleop = Teleop() self._teleop = Teleop()
self._teleop.subscribe(self._android_callback) self._teleop.subscribe(self._android_callback)
@@ -321,10 +313,8 @@ class AndroidPhone(BasePhone, Teleoperator):
self._latest_pose = pose self._latest_pose = pose
self._latest_message = message self._latest_message = message
@check_if_not_connected
def get_action(self) -> dict: def get_action(self) -> dict:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
ok, raw_pos, raw_rot, pose = self._read_current_pose() ok, raw_pos, raw_rot, pose = self._read_current_pose()
if not ok or not self.is_calibrated: if not ok or not self.is_calibrated:
return {} return {}
@@ -356,10 +346,8 @@ class AndroidPhone(BasePhone, Teleoperator):
"phone.enabled": self._enabled, "phone.enabled": self._enabled,
} }
@check_if_not_connected
def disconnect(self) -> None: def disconnect(self) -> None:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
self._teleop = None self._teleop = None
if self._teleop_thread and self._teleop_thread.is_alive(): if self._teleop_thread and self._teleop_thread.is_alive():
self._teleop_thread.join(timeout=1.0) self._teleop_thread.join(timeout=1.0)
@@ -26,7 +26,8 @@ if TYPE_CHECKING or _reachy2_sdk_available:
else: else:
ReachySDK = None ReachySDK = None
from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
from lerobot.utils.errors import DeviceNotConnectedError
from ..teleoperator import Teleoperator from ..teleoperator import Teleoperator
from .config_reachy2_teleoperator import Reachy2TeleoperatorConfig from .config_reachy2_teleoperator import Reachy2TeleoperatorConfig
@@ -126,10 +127,8 @@ class Reachy2Teleoperator(Teleoperator):
def is_connected(self) -> bool: def is_connected(self) -> bool:
return self.reachy.is_connected() if self.reachy is not None else False return self.reachy.is_connected() if self.reachy is not None else False
@check_if_already_connected
def connect(self, calibrate: bool = True) -> None: def connect(self, calibrate: bool = True) -> None:
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} already connected")
self.reachy = ReachySDK(self.config.ip_address) self.reachy = ReachySDK(self.config.ip_address)
if not self.is_connected: if not self.is_connected:
@@ -146,12 +145,10 @@ class Reachy2Teleoperator(Teleoperator):
def configure(self) -> None: def configure(self) -> None:
pass pass
@check_if_not_connected
def get_action(self) -> dict[str, float]: def get_action(self) -> dict[str, float]:
start = time.perf_counter() start = time.perf_counter()
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
joint_action: dict[str, float] = {} joint_action: dict[str, float] = {}
vel_action: dict[str, float] = {} vel_action: dict[str, float] = {}
@@ -23,7 +23,7 @@ from lerobot.motors.feetech import (
FeetechMotorsBus, FeetechMotorsBus,
OperatingMode, OperatingMode,
) )
from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
from ..teleoperator import Teleoperator from ..teleoperator import Teleoperator
from .config_so_leader import SOLeaderTeleopConfig from .config_so_leader import SOLeaderTeleopConfig
@@ -66,10 +66,8 @@ class SOLeader(Teleoperator):
def is_connected(self) -> bool: def is_connected(self) -> bool:
return self.bus.is_connected return self.bus.is_connected
@check_if_already_connected
def connect(self, calibrate: bool = True) -> None: def connect(self, calibrate: bool = True) -> None:
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} already connected")
self.bus.connect() self.bus.connect()
if not self.is_calibrated and calibrate: if not self.is_calibrated and calibrate:
logger.info( logger.info(
@@ -139,10 +137,8 @@ class SOLeader(Teleoperator):
self.bus.setup_motor(motor) self.bus.setup_motor(motor)
print(f"'{motor}' motor id set to {self.bus.motors[motor].id}") print(f"'{motor}' motor id set to {self.bus.motors[motor].id}")
@check_if_not_connected
def get_action(self) -> dict[str, float]: def get_action(self) -> dict[str, float]:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
start = time.perf_counter() start = time.perf_counter()
action = self.bus.sync_read("Present_Position") action = self.bus.sync_read("Present_Position")
action = {f"{motor}.pos": val for motor, val in action.items()} action = {f"{motor}.pos": val for motor, val in action.items()}
@@ -154,10 +150,8 @@ class SOLeader(Teleoperator):
# TODO: Implement force feedback # TODO: Implement force feedback
raise NotImplementedError raise NotImplementedError
@check_if_not_connected
def disconnect(self) -> None: def disconnect(self) -> None:
if not self.is_connected:
DeviceNotConnectedError(f"{self} is not connected.")
self.bus.disconnect() self.bus.disconnect()
logger.info(f"{self} disconnected.") logger.info(f"{self} disconnected.")
+41
View File
@@ -0,0 +1,41 @@
#!/usr/bin/env python
# Copyright 2026 The HuggingFace Inc. team. All rights reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from functools import wraps
from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
def check_if_not_connected(func):
@wraps(func)
def wrapper(self, *args, **kwargs):
if not self.is_connected:
raise DeviceNotConnectedError(
f"{self.__class__.__name__} is not connected. Run `.connect()` first."
)
return func(self, *args, **kwargs)
return wrapper
def check_if_already_connected(func):
@wraps(func)
def wrapper(self, *args, **kwargs):
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self.__class__.__name__} is already connected.")
return func(self, *args, **kwargs)
return wrapper
+6 -16
View File
@@ -22,7 +22,7 @@ from lerobot.cameras import CameraConfig, make_cameras_from_configs
from lerobot.motors.motors_bus import Motor, MotorNormMode from lerobot.motors.motors_bus import Motor, MotorNormMode
from lerobot.processor import RobotAction, RobotObservation from lerobot.processor import RobotAction, RobotObservation
from lerobot.robots import Robot, RobotConfig from lerobot.robots import Robot, RobotConfig
from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
from tests.mocks.mock_motors_bus import MockMotorsBus from tests.mocks.mock_motors_bus import MockMotorsBus
@@ -98,10 +98,8 @@ class MockRobot(Robot):
def is_connected(self) -> bool: def is_connected(self) -> bool:
return self._is_connected return self._is_connected
@check_if_already_connected
def connect(self, calibrate: bool = True) -> None: def connect(self, calibrate: bool = True) -> None:
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} already connected")
self._is_connected = True self._is_connected = True
if calibrate: if calibrate:
self.calibrate() self.calibrate()
@@ -110,19 +108,15 @@ class MockRobot(Robot):
def is_calibrated(self) -> bool: def is_calibrated(self) -> bool:
return self._is_calibrated return self._is_calibrated
@check_if_not_connected
def calibrate(self) -> None: def calibrate(self) -> None:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
self._is_calibrated = True self._is_calibrated = True
def configure(self) -> None: def configure(self) -> None:
pass pass
@check_if_not_connected
def get_observation(self) -> RobotObservation: def get_observation(self) -> RobotObservation:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
if self.config.random_values: if self.config.random_values:
return {f"{motor}.pos": random.uniform(-100, 100) for motor in self.motors} return {f"{motor}.pos": random.uniform(-100, 100) for motor in self.motors}
else: else:
@@ -130,14 +124,10 @@ class MockRobot(Robot):
f"{motor}.pos": val for motor, val in zip(self.motors, self.config.static_values, strict=True) f"{motor}.pos": val for motor, val in zip(self.motors, self.config.static_values, strict=True)
} }
@check_if_not_connected
def send_action(self, action: RobotAction) -> RobotAction: def send_action(self, action: RobotAction) -> RobotAction:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
return action return action
@check_if_not_connected
def disconnect(self) -> None: def disconnect(self) -> None:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
self._is_connected = False self._is_connected = False
+7 -16
View File
@@ -21,7 +21,7 @@ from typing import Any
from lerobot.processor import RobotAction from lerobot.processor import RobotAction
from lerobot.teleoperators import Teleoperator, TeleoperatorConfig from lerobot.teleoperators import Teleoperator, TeleoperatorConfig
from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
@TeleoperatorConfig.register_subclass("mock_teleop") @TeleoperatorConfig.register_subclass("mock_teleop")
@@ -68,10 +68,8 @@ class MockTeleop(Teleoperator):
def is_connected(self) -> bool: def is_connected(self) -> bool:
return self._is_connected return self._is_connected
@check_if_already_connected
def connect(self, calibrate: bool = True) -> None: def connect(self, calibrate: bool = True) -> None:
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} already connected")
self._is_connected = True self._is_connected = True
if calibrate: if calibrate:
self.calibrate() self.calibrate()
@@ -80,19 +78,15 @@ class MockTeleop(Teleoperator):
def is_calibrated(self) -> bool: def is_calibrated(self) -> bool:
return self._is_calibrated return self._is_calibrated
@check_if_not_connected
def calibrate(self) -> None: def calibrate(self) -> None:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
self._is_calibrated = True self._is_calibrated = True
def configure(self) -> None: def configure(self) -> None:
pass pass
@check_if_not_connected
def get_action(self) -> RobotAction: def get_action(self) -> RobotAction:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
if self.config.random_values: if self.config.random_values:
return {f"{motor}.pos": random.uniform(-100, 100) for motor in self.motors} return {f"{motor}.pos": random.uniform(-100, 100) for motor in self.motors}
else: else:
@@ -100,12 +94,9 @@ class MockTeleop(Teleoperator):
f"{motor}.pos": val for motor, val in zip(self.motors, self.config.static_values, strict=True) f"{motor}.pos": val for motor, val in zip(self.motors, self.config.static_values, strict=True)
} }
def send_feedback(self, feedback: dict[str, Any]) -> None: @check_if_not_connected
if not self.is_connected: def send_feedback(self, feedback: dict[str, Any]) -> None: ...
raise DeviceNotConnectedError(f"{self} is not connected.")
@check_if_not_connected
def disconnect(self) -> None: def disconnect(self) -> None:
if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.")
self._is_connected = False self._is_connected = False