mirror of
https://github.com/huggingface/lerobot.git
synced 2026-07-24 10:16:09 +00:00
feat: is connect checks decorators (#2813)
This commit is contained in:
@@ -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:
|
||||||
|
|||||||
@@ -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()
|
||||||
|
|||||||
@@ -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()
|
||||||
|
|||||||
@@ -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():
|
||||||
|
|||||||
@@ -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()
|
||||||
|
|||||||
@@ -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.")
|
||||||
|
|
||||||
|
|||||||
@@ -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
|
||||||
@@ -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
|
||||||
|
|||||||
@@ -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
|
||||||
|
|||||||
Reference in New Issue
Block a user