Update paths

lerobot/common/cameras/utils.py

@@ -19,20 +19,3 @@ def make_cameras_from_configs(camera_configs: dict[str, CameraConfig]) -> dict[s
             raise ValueError(f"The motor type '{cfg.type}' is not valid.")
 
     return cameras
-
-
-def make_camera(camera_type, **kwargs) -> Camera:
-    if camera_type == "opencv":
-        from .opencv import OpenCVCamera, OpenCVCameraConfig
-
-        config = OpenCVCameraConfig(**kwargs)
-        return OpenCVCamera(config)
-
-    elif camera_type == "intelrealsense":
-        from .intel import RealSenseCamera, RealSenseCameraConfig
-
-        config = RealSenseCameraConfig(**kwargs)
-        return RealSenseCamera(config)
-
-    else:
-        raise ValueError(f"The camera type '{camera_type}' is not valid.")

lerobot/common/robots/__init__.py

@@ -0,0 +1,4 @@
+from .config import RobotConfig
+from .robot import Robot
+
+__all__ = ["RobotConfig", "Robot"]

lerobot/common/robots/utils.py

@@ -1,15 +1,9 @@
+import logging
 from typing import Protocol
 
-from lerobot.common.robots.aloha.configuration_aloha import AlohaRobotConfig
+import numpy as np
-from lerobot.common.robots.config_abc import (
+
-    ManipulatorRobotConfig,
+from lerobot.common.robots import RobotConfig
-    RobotConfig,
-)
-from lerobot.common.robots.koch.configuration_koch import KochBimanualRobotConfig, KochRobotConfig
-from lerobot.common.robots.lekiwi.configuration_lekiwi import LeKiwiRobotConfig
-from lerobot.common.robots.moss.configuration_moss import MossRobotConfig
-from lerobot.common.robots.so_100.configuration_so_100 import So100RobotConfig
-from lerobot.common.robots.stretch3.configuration_stretch3 import StretchRobotConfig
 
 
 def get_arm_id(name, arm_type):
@@ -19,8 +13,8 @@ def get_arm_id(name, arm_type):
     return f"{name}_{arm_type}"
 
 
+# TODO(aliberts): Remove and point to lerobot.common.robots.Robot
 class Robot(Protocol):
-    # TODO(rcadene, aliberts): Add unit test checking the protocol is implemented in the corresponding classes
     robot_type: str
     features: dict
 
@@ -34,24 +28,39 @@ class Robot(Protocol):
 
 def make_robot_config(robot_type: str, **kwargs) -> RobotConfig:
     if robot_type == "aloha":
+        from .aloha.configuration_aloha import AlohaRobotConfig
+
         return AlohaRobotConfig(**kwargs)
     elif robot_type == "koch":
+        from .koch.configuration_koch import KochRobotConfig
+
         return KochRobotConfig(**kwargs)
-    elif robot_type == "koch_bimanual":
+    # elif robot_type == "koch_bimanual":
-        return KochBimanualRobotConfig(**kwargs)
+    #     return KochBimanualRobotConfig(**kwargs)
     elif robot_type == "moss":
+        from .moss.configuration_moss import MossRobotConfig
+
         return MossRobotConfig(**kwargs)
     elif robot_type == "so100":
+        from .so100.configuration_so100 import So100RobotConfig
+
         return So100RobotConfig(**kwargs)
     elif robot_type == "stretch":
+        from .stretch3.configuration_stretch3 import StretchRobotConfig
+
         return StretchRobotConfig(**kwargs)
     elif robot_type == "lekiwi":
+        from .lekiwi.configuration_lekiwi import LeKiwiRobotConfig
+
         return LeKiwiRobotConfig(**kwargs)
     else:
         raise ValueError(f"Robot type '{robot_type}' is not available.")
 
 
 def make_robot_from_config(config: RobotConfig):
+    from .lekiwi.configuration_lekiwi import LeKiwiRobotConfig
+    from .manipulator import ManipulatorRobotConfig
+
     if isinstance(config, ManipulatorRobotConfig):
         from lerobot.common.robots.manipulator import ManipulatorRobot
 
@@ -61,11 +70,31 @@ def make_robot_from_config(config: RobotConfig):
 
         return MobileManipulator(config)
     else:
-        from lerobot.common.robots.stretch3.robot_stretch3 import StretchRobot
+        from lerobot.common.robots.stretch3.robot_stretch3 import Stretch3Robot
 
-        return StretchRobot(config)
+        return Stretch3Robot(config)
 
 
 def make_robot(robot_type: str, **kwargs) -> Robot:
     config = make_robot_config(robot_type, **kwargs)
     return make_robot_from_config(config)
+
+
+def ensure_safe_goal_position(
+    goal_pos: np.ndarray, present_pos: np.ndarray, max_relative_target: float | list[float]
+):
+    # Cap relative action target magnitude for safety.
+    diff = goal_pos - present_pos
+    max_relative_target = np.array(max_relative_target)
+    safe_diff = np.min(diff, max_relative_target)
+    safe_diff = np.max(safe_diff, -max_relative_target)
+    safe_goal_pos = present_pos + safe_diff
+
+    if not np.allclose(goal_pos, safe_goal_pos):
+        logging.warning(
+            "Relative goal position magnitude had to be clamped to be safe.\n"
+            f"  requested relative goal position target: {diff}\n"
+            f"    clamped relative goal position target: {safe_diff}"
+        )
+
+    return safe_goal_pos

lerobot/configs/control.py

@@ -4,7 +4,7 @@ from pathlib import Path
 
 import draccus
 
-from lerobot.common.robots.config_abc import RobotConfig
+from lerobot.common.robots import RobotConfig
 from lerobot.common.utils.utils import auto_select_torch_device, is_amp_available, is_torch_device_available
 from lerobot.configs import parser
 from lerobot.configs.policies import PreTrainedConfig

lerobot/scripts/configure_motor.py

@@ -19,7 +19,7 @@ import time
 def get_motor_bus_cls(brand: str) -> tuple:
     if brand == "feetech":
         from lerobot.common.motors.configs import FeetechMotorsBusConfig
-        from lerobot.common.motors.feetech import (
+        from lerobot.common.motors.feetech.feetech import (
             MODEL_BAUDRATE_TABLE,
             SCS_SERIES_BAUDRATE_TABLE,
             FeetechMotorsBus,
@@ -29,7 +29,7 @@ def get_motor_bus_cls(brand: str) -> tuple:
 
     elif brand == "dynamixel":
         from lerobot.common.motors.configs import DynamixelMotorsBusConfig
-        from lerobot.common.motors.dynamixel import (
+        from lerobot.common.motors.dynamixel.dynamixel import (
             MODEL_BAUDRATE_TABLE,
             X_SERIES_BAUDRATE_TABLE,
             DynamixelMotorsBus,

tests/test_cameras.py

@@ -24,7 +24,7 @@ pytest -sx 'tests/test_cameras.py::test_camera[intelrealsense-True]'
 import numpy as np
 import pytest
 
-from lerobot.common.utils.robot_utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError
+from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
 from tests.utils import TEST_CAMERA_TYPES, make_camera, require_camera
 
 # Maximum absolute difference between two consecutive images recorded by a camera.
@@ -57,11 +57,11 @@ def test_camera(request, camera_type, mock):
     camera = make_camera(**camera_kwargs)
 
     # Test reading, async reading, disconnecting before connecting raises an error
-    with pytest.raises(RobotDeviceNotConnectedError):
+    with pytest.raises(DeviceNotConnectedError):
         camera.read()
-    with pytest.raises(RobotDeviceNotConnectedError):
+    with pytest.raises(DeviceNotConnectedError):
         camera.async_read()
-    with pytest.raises(RobotDeviceNotConnectedError):
+    with pytest.raises(DeviceNotConnectedError):
         camera.disconnect()
 
     # Test deleting the object without connecting first
@@ -76,7 +76,7 @@ def test_camera(request, camera_type, mock):
     assert camera.height is not None
 
     # Test connecting twice raises an error
-    with pytest.raises(RobotDeviceAlreadyConnectedError):
+    with pytest.raises(DeviceAlreadyConnectedError):
         camera.connect()
 
     # Test reading from the camera
@@ -185,9 +185,9 @@ def test_camera(request, camera_type, mock):
 def test_save_images_from_cameras(tmp_path, request, camera_type, mock):
     # TODO(rcadene): refactor
     if camera_type == "opencv":
-        from lerobot.common.cameras.opencv import save_images_from_cameras
+        from lerobot.common.cameras.opencv.camera_opencv import save_images_from_cameras
     elif camera_type == "intelrealsense":
-        from lerobot.common.cameras.intelrealsense import save_images_from_cameras
+        from lerobot.common.cameras.intel.camera_realsense import save_images_from_cameras
 
     # Small `record_time_s` to speedup unit tests
     save_images_from_cameras(tmp_path, record_time_s=0.02, mock=mock)

tests/test_motors.py

@@ -30,7 +30,7 @@ import time
 import numpy as np
 import pytest
 
-from lerobot.common.utils.robot_utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError
+from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
 from lerobot.scripts.find_motors_bus_port import find_port
 from tests.utils import TEST_MOTOR_TYPES, make_motors_bus, require_motor
 
@@ -89,11 +89,11 @@ def test_motors_bus(request, motor_type, mock):
     motors_bus = make_motors_bus(motor_type, mock=mock)
 
     # Test reading and writing before connecting raises an error
-    with pytest.raises(RobotDeviceNotConnectedError):
+    with pytest.raises(DeviceNotConnectedError):
         motors_bus.read("Torque_Enable")
-    with pytest.raises(RobotDeviceNotConnectedError):
+    with pytest.raises(DeviceNotConnectedError):
         motors_bus.write("Torque_Enable", 1)
-    with pytest.raises(RobotDeviceNotConnectedError):
+    with pytest.raises(DeviceNotConnectedError):
         motors_bus.disconnect()
 
     # Test deleting the object without connecting first
@@ -104,7 +104,7 @@ def test_motors_bus(request, motor_type, mock):
     motors_bus.connect()
 
     # Test connecting twice raises an error
-    with pytest.raises(RobotDeviceAlreadyConnectedError):
+    with pytest.raises(DeviceAlreadyConnectedError):
         motors_bus.connect()
 
     # Test disabling torque and reading torque on all motors

tests/test_robots.py

@@ -26,8 +26,8 @@ pytest -sx 'tests/test_robots.py::test_robot[aloha-True]'
 import pytest
 import torch
 
+from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
 from lerobot.common.robots.utils import make_robot
-from lerobot.common.utils.robot_utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError
 from tests.utils import TEST_ROBOT_TYPES, mock_calibration_dir, require_robot
 
 
@@ -54,15 +54,15 @@ def test_robot(tmp_path, request, robot_type, mock):
 
     # Test using robot before connecting raises an error
     robot = make_robot(**robot_kwargs)
-    with pytest.raises(RobotDeviceNotConnectedError):
+    with pytest.raises(DeviceNotConnectedError):
         robot.teleop_step()
-    with pytest.raises(RobotDeviceNotConnectedError):
+    with pytest.raises(DeviceNotConnectedError):
         robot.teleop_step(record_data=True)
-    with pytest.raises(RobotDeviceNotConnectedError):
+    with pytest.raises(DeviceNotConnectedError):
         robot.capture_observation()
-    with pytest.raises(RobotDeviceNotConnectedError):
+    with pytest.raises(DeviceNotConnectedError):
         robot.send_action(None)
-    with pytest.raises(RobotDeviceNotConnectedError):
+    with pytest.raises(DeviceNotConnectedError):
         robot.disconnect()
 
     # Test deleting the object without connecting first
@@ -74,7 +74,7 @@ def test_robot(tmp_path, request, robot_type, mock):
     assert robot.is_connected
 
     # Test connecting twice raises an error
-    with pytest.raises(RobotDeviceAlreadyConnectedError):
+    with pytest.raises(DeviceAlreadyConnectedError):
         robot.connect()
 
     # TODO(rcadene, aliberts): Test disconnecting with `__del__` instead of `disconnect`

tests/utils.py

@@ -23,9 +23,8 @@ import pytest
 import torch
 
 from lerobot import available_cameras, available_motors, available_robots
-from lerobot.common.cameras.utils import Camera
+from lerobot.common.cameras import Camera
-from lerobot.common.cameras.utils import make_camera as make_camera_device
+from lerobot.common.motors.motors_bus import MotorsBus
-from lerobot.common.motors.utils import MotorsBus
 from lerobot.common.motors.utils import make_motors_bus as make_motors_bus_device
 from lerobot.common.utils.import_utils import is_package_available
 
@@ -306,11 +305,19 @@ def mock_calibration_dir(calibration_dir):
 def make_camera(camera_type: str, **kwargs) -> Camera:
     if camera_type == "opencv":
         camera_index = kwargs.pop("camera_index", OPENCV_CAMERA_INDEX)
-        return make_camera_device(camera_type, camera_index=camera_index, **kwargs)
+        kwargs["camera_index"] = camera_index
+        from lerobot.common.cameras.opencv import OpenCVCamera, OpenCVCameraConfig
+
+        config = OpenCVCameraConfig(**kwargs)
+        return OpenCVCamera(config)
 
     elif camera_type == "intelrealsense":
         serial_number = kwargs.pop("serial_number", INTELREALSENSE_SERIAL_NUMBER)
-        return make_camera_device(camera_type, serial_number=serial_number, **kwargs)
+        kwargs["serial_number"] = serial_number
+        from lerobot.common.cameras.intel import RealSenseCamera, RealSenseCameraConfig
+
+        config = RealSenseCameraConfig(**kwargs)
+        return RealSenseCamera(config)
     else:
         raise ValueError(f"The camera type '{camera_type}' is not valid.")