diff --git a/.github/workflows/build_wheels.yaml b/.github/workflows/build_wheels.yaml index 393c31e8..b58bc250 100644 --- a/.github/workflows/build_wheels.yaml +++ b/.github/workflows/build_wheels.yaml @@ -82,8 +82,10 @@ jobs: fail-fast: false matrix: extension: + - rcs_digit - rcs_realsense - rcs_robotiq2f85 + - rcs_tilburg_hand - rcs_ur5e - rcs_usb_cam - rcs_xarm7 diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index eb6bea24..1b6014f5 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -87,6 +87,8 @@ jobs: extension: # Python extensions - rcs_xarm7 + - rcs_digit + - rcs_tilburg_hand - rcs_realsense - rcs_robotiq2f85 - rcs_tacto diff --git a/Makefile b/Makefile index 655fe690..d1f9c381 100644 --- a/Makefile +++ b/Makefile @@ -5,7 +5,7 @@ WHEELHOUSE = wheelhouse CORE_WHEELHOUSE = ${WHEELHOUSE}/core PY_EXT_WHEELHOUSE = ${WHEELHOUSE}/py_extensions CPP_EXT_WHEELHOUSE = ${WHEELHOUSE}/cpp_extensions -PY_EXTENSIONS ?= rcs_realsense rcs_robotiq2f85 rcs_ur5e rcs_usb_cam rcs_xarm7 rcs_zed +PY_EXTENSIONS ?= rcs_digit rcs_realsense rcs_robotiq2f85 rcs_tilburg_hand rcs_ur5e rcs_usb_cam rcs_xarm7 rcs_zed CPP_EXTENSIONS ?= rcs_fr3 rcs_panda rcs_so101 # set to pypi to publish to the real index PYPI_REPOSITORY ?= testpypi diff --git a/docs/development/python_extension.md b/docs/development/python_extension.md index 13b5b859..0955689b 100644 --- a/docs/development/python_extension.md +++ b/docs/development/python_extension.md @@ -36,7 +36,22 @@ rcs_myext/ 2. **Implement the Interface**: Create your device class in `src/rcs_myext/my_device.py`. You should inherit from the appropriate RCS base class (e.g., `Camera`, `Gripper`) if applicable, or implement the required methods. -3. **Register the Extension**: If your extension needs to be discoverable by RCS (e.g., for CLI tools or automatic loading), ensure it's installed in the same environment. +3. **Register the Backend**: If your extension provides a camera, gripper or hand, declare a factory + as an entry point so the robot extensions can create it from a config. The entry point name is + the type id the config carries (`camera_type_id`, `GripperType.id` or `HandType.id`), and the + group is one of `rcs.cameras`, `rcs.grippers` or `rcs.hands`: + + ```toml + [project.entry-points."rcs.cameras"] + mycam = "rcs_myext.creators:create_camera_set" + ``` + + The factory takes the config and returns the device, for a camera + `(HardwareCameraCreatorConfig) -> HardwareCamera`. Nothing else has to know about your + extension: `rcs.registry` lists installed backends from package metadata without importing + them, and imports yours only when its id is requested. Reinstall the extension after changing + entry points, they are read from the installed metadata. For code that is not installed as a + package, `rcs.registry.CAMERAS.register("mycam", create_camera_set)` does the same at runtime. ## Example: USB Camera diff --git a/docs/extensions/index.md b/docs/extensions/index.md index a576bdad..5277c24f 100644 --- a/docs/extensions/index.md +++ b/docs/extensions/index.md @@ -12,8 +12,10 @@ rcs_ur5e rcs_so101 rcs_yam rcs_realsense +rcs_digit rcs_usb_cam rcs_tacto rcs_robotiq2f85 +rcs_tilburg_hand rcs_zed ``` diff --git a/docs/extensions/overview.md b/docs/extensions/overview.md index 5b286ecf..fef47680 100644 --- a/docs/extensions/overview.md +++ b/docs/extensions/overview.md @@ -42,6 +42,8 @@ RCS comes with several supported extensions: - **rcs_yam**: Support for the I2RT YAM arm. - **rcs_realsense**: Support for Intel RealSense cameras. - **rcs_usb_cam**: Support for generic USB webcams. +- **rcs_digit**: Support for DIGIT tactile sensors. +- **rcs_tilburg_hand**: Support for the Tilburg Hand (hardware; the simulated hand is in `rcs-core`). - **rcs_tacto**: Integration with the Tacto tactile sensor simulator. - **rcs_robotiq2f85**: Integration with the Robotiq 2F-85 Gripper. diff --git a/docs/extensions/rcs_digit.md b/docs/extensions/rcs_digit.md new file mode 100644 index 00000000..86e76b18 --- /dev/null +++ b/docs/extensions/rcs_digit.md @@ -0,0 +1,20 @@ +# RCS DIGIT Extension + +This extension provides support for [DIGIT](https://digit.ml) tactile sensors in RCS. + +The sensor is exposed as an RCS `HardwareCamera`, so it is configured and polled like any other +camera in a camera set. The robot extensions register a `digit` camera type that resolves once +this extension is installed. + +## Installation + +```shell +pip install rcs-digit +``` + +For local development: + +```shell +pip install -ve . --no-build-isolation +pip install -ve extensions/rcs_digit +``` diff --git a/docs/extensions/rcs_tilburg_hand.md b/docs/extensions/rcs_tilburg_hand.md new file mode 100644 index 00000000..5c82b6cc --- /dev/null +++ b/docs/extensions/rcs_tilburg_hand.md @@ -0,0 +1,22 @@ +# RCS Tilburg Hand Extension + +This extension provides support for the Tilburg Hand in RCS. + +It covers the **hardware** hand only. The simulated hand, `rcs.sim.SimTilburgHand`, is implemented +in C++ inside `rcs-core` and needs nothing from this extension. + +The arm extensions accept a `THConfig` wherever they take a gripper config, and attach the hand +through a `HandWrapper` once this extension is installed. + +## Installation + +```shell +pip install rcs-tilburg-hand +``` + +For local development: + +```shell +pip install -ve . --no-build-isolation +pip install -ve extensions/rcs_tilburg_hand +``` diff --git a/docs/extensions/rcs_zed.md b/docs/extensions/rcs_zed.md index 6f82cfc1..3b9759a4 100644 --- a/docs/extensions/rcs_zed.md +++ b/docs/extensions/rcs_zed.md @@ -22,7 +22,7 @@ pip install -ve extensions/rcs_zed ## Calibration The `default_zed(...)` helper uses the identity -`DummyCalibrationStrategy` unless a `calibration_strategy` mapping is supplied. +`IdentityCalibrationStrategy` unless a `calibration_strategy` mapping is supplied. Mapping keys must match the logical camera names, and values must implement `rcs.camera.hw.CalibrationStrategy`. Calibration is injected explicitly so the ZED extension remains independent of other hardware-camera extensions. diff --git a/extensions/rcs_digit/README.md b/extensions/rcs_digit/README.md new file mode 100644 index 00000000..9c699b4c --- /dev/null +++ b/extensions/rcs_digit/README.md @@ -0,0 +1,53 @@ +# RCS DIGIT Extension + +Support for the [DIGIT](https://digit.ml) tactile sensor in RCS, built on the +[`digit-interface`](https://pypi.org/project/digit-interface/) driver. + +This extension depends on [`rcs-core`](https://pypi.org/project/rcs-core/). +Documentation: + +## Installation + +Install from PyPI: + +```shell +pip install rcs-digit +``` + +Warning: plain `pip install rcs-digit` will install the published `rcs-core` dependency from PyPI. + +Install from a local checkout: + +```shell +pip install -ve . +``` + +If you want this extension to use your local RCS checkout instead of the published `rcs-core` package, first install the main package from the repository root: + +```shell +pip install -ve . --no-build-isolation +pip install -ve extensions/rcs_digit +``` + +## Usage + +The robot extensions register a `digit` camera type that resolves to `DigitCam` once this +extension is installed, so a DIGIT is configured like any other hardware camera: + +```python +from rcs.envs.base import HardwareCameraCreatorConfig + +camera_cfgs["digit"] = HardwareCameraCreatorConfig( + camera_type_id="digit", + camera_cfgs={"digit": BaseCameraConfig(identifier="D00001")}, +) +``` + +`default_digit` builds those configs from a name-to-serial mapping, using the resolution and +frame rate of a named `digit-interface` stream: + +```python +from rcs_digit.camera import default_digit + +cameras = default_digit({"thumb": "D00001", "index": "D00002"}, stream_name="QVGA") +``` diff --git a/extensions/rcs_digit/pyproject.toml b/extensions/rcs_digit/pyproject.toml new file mode 100644 index 00000000..858db4bb --- /dev/null +++ b/extensions/rcs_digit/pyproject.toml @@ -0,0 +1,26 @@ +[build-system] +requires = ["setuptools"] +build-backend = "setuptools.build_meta" + +[project] +name = "rcs_digit" +version = "0.7.3" +description = "RCS DIGIT tactile sensor module" +readme = "README.md" +license = "AGPL-3.0-or-later" +dependencies = ["rcs-core>=0.7.3", "digit-interface"] +maintainers = [ + { name = "Tobias Juelg", email = "tobias.juelg@utn.de" }, +] +authors = [{ name = "Tobias Juelg", email = "tobias.juelg@utn.de" }] +requires-python = ">=3.11" + +[project.entry-points."rcs.cameras"] +digit = "rcs_digit.creators:create_camera_set" + +[tool.black] +line-length = 120 +target-version = ["py310"] + +[tool.isort] +profile = "black" diff --git a/extensions/rcs_digit/src/rcs_digit/__init__.py b/extensions/rcs_digit/src/rcs_digit/__init__.py new file mode 100644 index 00000000..4910b9ec --- /dev/null +++ b/extensions/rcs_digit/src/rcs_digit/__init__.py @@ -0,0 +1 @@ +__version__ = "0.7.3" diff --git a/python/rcs/camera/digit_cam.py b/extensions/rcs_digit/src/rcs_digit/camera.py similarity index 65% rename from python/rcs/camera/digit_cam.py rename to extensions/rcs_digit/src/rcs_digit/camera.py index bb7113fb..1e128a74 100644 --- a/python/rcs/camera/digit_cam.py +++ b/extensions/rcs_digit/src/rcs_digit/camera.py @@ -1,4 +1,10 @@ -from digit_interface.digit import Digit +"""DIGIT tactile sensor support, built on the `digit-interface` driver. + +The sensor is exposed as an RCS `HardwareCamera`, so it is configured and polled like any other +camera in a camera set. `default_digit` builds the per-sensor configs from a name-to-serial map. +""" + +from digit_interface import Digit from rcs._core.common import BaseCameraConfig from rcs.camera.hw import HardwareCamera from rcs.camera.interface import CameraFrame, DataFrame, Frame @@ -55,3 +61,20 @@ def config(self, camera_name) -> BaseCameraConfig: def calibrate(self) -> bool: """No calibration needed for DIGIT cameras.""" return True + + +def default_digit(name2id: dict[str, str] | None, stream_name: str = "QVGA") -> DigitCam | None: + """Build a `DigitCam` for `name -> serial`, sized from the named `digit-interface` stream.""" + if name2id is None: + return None + stream_dict = Digit.STREAMS[stream_name] + cameras = { + name: BaseCameraConfig( + identifier=identifier, + resolution_width=stream_dict["resolution"]["width"], + resolution_height=stream_dict["resolution"]["height"], + frame_rate=stream_dict["fps"]["30fps"], + ) + for name, identifier in name2id.items() + } + return DigitCam(cameras=cameras) diff --git a/extensions/rcs_digit/src/rcs_digit/creators.py b/extensions/rcs_digit/src/rcs_digit/creators.py new file mode 100644 index 00000000..3e47f35f --- /dev/null +++ b/extensions/rcs_digit/src/rcs_digit/creators.py @@ -0,0 +1,13 @@ +"""Factory for the `digit` camera backend, registered as an `rcs.cameras` entry point.""" + +import typing + +from rcs.camera.hw import HardwareCamera, HardwareCameraCreatorConfig +from rcs_digit.camera import DigitCam + + +def create_camera_set(cfg: HardwareCameraCreatorConfig) -> HardwareCamera: + if cfg.calibration is not None: + msg = "DIGIT sensors are not calibrated, `calibration` must be None" + raise ValueError(msg) + return typing.cast(HardwareCamera, DigitCam(cameras=cfg.camera_cfgs)) diff --git a/extensions/rcs_fr3/pyproject.toml b/extensions/rcs_fr3/pyproject.toml index a53e9bf1..fd267f0c 100644 --- a/extensions/rcs_fr3/pyproject.toml +++ b/extensions/rcs_fr3/pyproject.toml @@ -24,6 +24,9 @@ authors = [{ name = "Tobias Juelg", email = "tobias.juelg@utn.de" }] requires-python = ">=3.11" +[project.entry-points."rcs.grippers"] +FrankaHand = "rcs_fr3.creators:create_franka_gripper" + [dependency-groups] build_deps = [ "pin==3.7.0", diff --git a/extensions/rcs_fr3/src/rcs_fr3/configs.py b/extensions/rcs_fr3/src/rcs_fr3/configs.py index c989bc2c..4727c37e 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/configs.py +++ b/extensions/rcs_fr3/src/rcs_fr3/configs.py @@ -118,7 +118,7 @@ def config(self) -> FR3MultiHardwareEnvCreatorConfig: cfg = base.config() cfg.robot_cfg.async_control = True cfg.robot_cfg.ip = self.robot_ip - cfg.robot_cfg.tcp_offset = rcs.GRIPPER_TCP_OFFSETS[common.GripperType("Robotiq2F85")] + cfg.robot_cfg.tcp_offset = rcs.GRIPPER_TCP_OFFSETS[common.GripperType.Robotiq2F85] cfg.robot_cfg.q_home = rcs.HOME_POSITIONS["FR3_DROID"] return FR3MultiHardwareEnvCreatorConfig( @@ -208,8 +208,8 @@ def config(self) -> FR3MultiHardwareEnvCreatorConfig: cfg = super().config() cfg.camera_cfgs = None - cfg.robot_cfgs["left"].tcp_offset = rcs.GRIPPER_TCP_OFFSETS[common.GripperType("Robotiq2F85")] - cfg.robot_cfgs["right"].tcp_offset = rcs.GRIPPER_TCP_OFFSETS[common.GripperType("Robotiq2F85")] + cfg.robot_cfgs["left"].tcp_offset = rcs.GRIPPER_TCP_OFFSETS[common.GripperType.Robotiq2F85] + cfg.robot_cfgs["right"].tcp_offset = rcs.GRIPPER_TCP_OFFSETS[common.GripperType.Robotiq2F85] cfg.robot_cfgs["left"].q_home = rcs.HOME_POSITIONS["FR3_DUO_LEFT"] cfg.robot_cfgs["right"].q_home = rcs.HOME_POSITIONS["FR3_DUO_RIGHT"] cfg.gripper_cfgs = { diff --git a/extensions/rcs_fr3/src/rcs_fr3/creators.py b/extensions/rcs_fr3/src/rcs_fr3/creators.py index 36e4f3f9..28fbd317 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/creators.py +++ b/extensions/rcs_fr3/src/rcs_fr3/creators.py @@ -4,15 +4,8 @@ import gymnasium as gym import numpy as np -import rcs.hand.tilburg_hand -from frankik import FrankaKinematics -from rcs._core.common import BaseCameraConfig, Gripper, GripperConfig, Kinematics, Pose -from rcs.camera.hw import ( - CalibrationStrategy, - DummyCalibrationStrategy, - HardwareCamera, - HardwareCameraSet, -) +from rcs._core.common import Gripper, GripperConfig, HandConfig, Kinematics, Pose +from rcs.camera.hw import HardwareCameraCreatorConfig, create_hardware_camera_set from rcs.envs.base import ( CameraSetWrapper, ControlMode, @@ -27,11 +20,11 @@ RobotWrapper, ) from rcs.envs.scenes import RCSEnvCreator, WrapperConfig -from rcs.hand.tilburg_hand import TilburgHand from rcs_fr3._core import hw from rcs_fr3.envs import FR3HW import rcs +from frankik import FrankaKinematics logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -58,108 +51,19 @@ def inverse( # type: ignore FastIK = FrankIK() -@dataclass(kw_only=True) -class HardwareCameraCreatorConfig: - camera_type_id: str - camera_cfgs: dict[str, BaseCameraConfig] - kwargs: dict[str, typing.Any] = field(default_factory=dict) - - -def _create_realsense_camera(cfg: HardwareCameraCreatorConfig) -> HardwareCamera: - try: - # from rcs_realsense.calibration import FR3BaseArucoCalibration - from rcs_realsense.camera import RealSenseCameraSet - except ImportError as e: - msg = "RealSense camera support requires the `rcs_realsense` extension to be installed." - raise ImportError(msg) from e - - calibration_strategy = { - name: typing.cast(CalibrationStrategy, DummyCalibrationStrategy()) for name in cfg.camera_cfgs - } - return typing.cast( - HardwareCamera, - RealSenseCameraSet(cameras=cfg.camera_cfgs, calibration_strategy=calibration_strategy, **cfg.kwargs), - ) - - -def _create_zed_camera(cfg: HardwareCameraCreatorConfig) -> HardwareCamera: - try: - from rcs_zed.camera import ZEDCameraSet - except ImportError as e: - msg = "ZED camera support requires the `rcs_zed` extension to be installed." - raise ImportError(msg) from e - - calibration_strategy = { - name: typing.cast(CalibrationStrategy, DummyCalibrationStrategy()) for name in cfg.camera_cfgs - } - return typing.cast( - HardwareCamera, - ZEDCameraSet(cameras=cfg.camera_cfgs, calibration_strategy=calibration_strategy, **cfg.kwargs), - ) - - -def _create_digit_camera(cfg: HardwareCameraCreatorConfig) -> HardwareCamera: - try: - from rcs.camera.digit_cam import DigitCam - except ImportError as e: - msg = "DIGIT camera support requires the `digit_interface` package to be installed." - raise ImportError(msg) from e - - return typing.cast(HardwareCamera, DigitCam(cameras=cfg.camera_cfgs)) - - -HARDWARE_CAMERA_CREATORS: dict[str, typing.Callable[[HardwareCameraCreatorConfig], HardwareCamera]] = { - "realsense": _create_realsense_camera, - "zed": _create_zed_camera, - "digit": _create_digit_camera, -} - - -def _create_hardware_camera_set( - camera_cfgs: dict[str, HardwareCameraCreatorConfig] | None, -) -> HardwareCameraSet | None: - if camera_cfgs is None: - return None - cameras: list[HardwareCamera] = [] - for cfg in camera_cfgs.values(): - if cfg.camera_type_id not in HARDWARE_CAMERA_CREATORS: - msg = f"Unknown hardware camera type id: {cfg.camera_type_id}" - raise ValueError(msg) - cameras.append(HARDWARE_CAMERA_CREATORS[cfg.camera_type_id](cfg)) - return HardwareCameraSet(cameras) if cameras else None - - -def _create_franka_gripper(cfg: GripperConfig) -> Gripper: +def create_franka_gripper(cfg: GripperConfig) -> Gripper: if not isinstance(cfg, hw.FHConfig): - msg = f"Expected FHConfig for franka gripper, got {type(cfg).__name__}" + msg = f"Expected rcs_fr3 FHConfig for the franka hand, got {type(cfg).__module__}.{type(cfg).__qualname__}" raise TypeError(msg) return hw.FrankaHand(cfg) -def _create_robotiq_gripper(cfg: GripperConfig) -> Gripper: - try: - from rcs_robotiq2f85.hw import RobotiQ2F85Gripper, RobotiQ2F85GripperConfig - except ImportError as e: - msg = "Robotiq gripper support requires the `rcs_robotiq2f85` extension to be installed." - raise ImportError(msg) from e - - if not isinstance(cfg, RobotiQ2F85GripperConfig): - msg = f"Expected RobotiQ2F85GripperConfig for robotiq gripper, got {type(cfg).__name__}" - raise TypeError(msg) - return RobotiQ2F85Gripper(cfg) - - -HARDWARE_GRIPPER_CREATORS: dict[str, typing.Callable[[GripperConfig], Gripper]] = { - rcs.common.GripperType.FrankaHand.id: _create_franka_gripper, - rcs.common.GripperType("Robotiq2F85").id: _create_robotiq_gripper, -} - - @dataclass(kw_only=True) class FR3HardwareEnvCreatorConfig: robot_cfg: hw.FR3Config control_mode: ControlMode - gripper_cfg: GripperConfig | rcs.hand.tilburg_hand.THConfig | None = None + gripper_cfg: GripperConfig | None = None + hand_cfg: HandConfig | None = None camera_cfgs: dict[str, HardwareCameraCreatorConfig] | None = None max_relative_movement: float | tuple[float, float] | None = None relative_to: RelativeTo = RelativeTo.LAST_STEP @@ -172,7 +76,8 @@ class FR3HardwareEnvCreatorConfig: class FR3MultiHardwareEnvCreatorConfig: robot_cfgs: dict[str, hw.FR3Config] control_mode: ControlMode - gripper_cfgs: dict[str, GripperConfig | rcs.hand.tilburg_hand.THConfig | None] | None = None + gripper_cfgs: dict[str, GripperConfig | None] | None = None + hand_cfgs: dict[str, HandConfig | None] | None = None camera_cfgs: dict[str, HardwareCameraCreatorConfig] | None = None max_relative_movement: float | tuple[float, float] | None = None relative_to: RelativeTo = RelativeTo.LAST_STEP @@ -194,18 +99,14 @@ def create_env(self, cfg: FR3HardwareEnvCreatorConfig) -> gym.Env: env: gym.Env = HardwareEnv(frequency=cfg.frequency) env = RobotWrapper(env, robot, cfg.control_mode, home_on_reset=cfg.wrapper_cfg.home_on_reset) env = FR3HW(env) - if isinstance(cfg.gripper_cfg, rcs.hand.tilburg_hand.THConfig): - hand = TilburgHand(cfg.gripper_cfg) + if cfg.hand_cfg is not None: + hand = rcs.registry.HANDS.get(cfg.hand_cfg.hand_type.id)(cfg.hand_cfg) env = HandWrapper(env, hand, binary=cfg.wrapper_cfg.binary_gripper) elif cfg.gripper_cfg is not None: - gripper_type_id = cfg.gripper_cfg.gripper_type.id - if gripper_type_id not in HARDWARE_GRIPPER_CREATORS: - msg = f"Unknown hardware gripper type id: {gripper_type_id}" - raise ValueError(msg) - gripper = HARDWARE_GRIPPER_CREATORS[gripper_type_id](cfg.gripper_cfg) + gripper = rcs.registry.GRIPPERS.get(cfg.gripper_cfg.gripper_type.id)(cfg.gripper_cfg) env = GripperWrapper(env, gripper, binary=cfg.wrapper_cfg.binary_gripper) - camera_set = _create_hardware_camera_set(cfg.camera_cfgs) + camera_set = create_hardware_camera_set(cfg.camera_cfgs) if camera_set is not None: camera_set.start() camera_set.wait_for_frames() @@ -232,6 +133,7 @@ def create_env(self, cfg: FR3MultiHardwareEnvCreatorConfig) -> gym.Env: robot_cfg=robot_cfg, control_mode=cfg.control_mode, gripper_cfg=cfg.gripper_cfgs[robot_name] if cfg.gripper_cfgs is not None else None, + hand_cfg=cfg.hand_cfgs[robot_name] if cfg.hand_cfgs is not None else None, camera_cfgs=None, max_relative_movement=cfg.max_relative_movement, relative_to=cfg.relative_to, @@ -241,7 +143,7 @@ def create_env(self, cfg: FR3MultiHardwareEnvCreatorConfig) -> gym.Env: ) env: gym.Env = MultiRobotWrapper(envs, cfg.robot_to_shared_base_frame) - camera_set = _create_hardware_camera_set(cfg.camera_cfgs) + camera_set = create_hardware_camera_set(cfg.camera_cfgs) if camera_set is not None: camera_set.start() camera_set.wait_for_frames() diff --git a/extensions/rcs_panda/pyproject.toml b/extensions/rcs_panda/pyproject.toml index 8d24c488..77776d47 100644 --- a/extensions/rcs_panda/pyproject.toml +++ b/extensions/rcs_panda/pyproject.toml @@ -23,6 +23,9 @@ authors = [{ name = "Tobias Juelg", email = "tobias.juelg@utn.de" }] requires-python = ">=3.11" +[project.entry-points."rcs.grippers"] +PandaHand = "rcs_panda.creators:create_panda_gripper" + [dependency-groups] build_deps = [ "pin==3.7.0", diff --git a/extensions/rcs_panda/src/rcs_panda/configs.py b/extensions/rcs_panda/src/rcs_panda/configs.py index d90453e5..f646e703 100644 --- a/extensions/rcs_panda/src/rcs_panda/configs.py +++ b/extensions/rcs_panda/src/rcs_panda/configs.py @@ -29,6 +29,7 @@ def config(self) -> PandaHardwareEnvCreatorConfig: robot_cfg.async_control = False gripper_cfg = hw.FHConfig(ip=self.ip) + gripper_cfg.gripper_type = common.GripperType.PandaHand gripper_cfg.epsilon_inner = gripper_cfg.epsilon_outer = 0.1 gripper_cfg.speed = 0.1 gripper_cfg.force = 30 @@ -60,7 +61,7 @@ def config(self) -> PandaMultiHardwareEnvCreatorConfig: cfg = base.config() cfg.robot_cfg.async_control = True cfg.robot_cfg.ip = self.robot_ip - cfg.robot_cfg.tcp_offset = rcs.GRIPPER_TCP_OFFSETS[common.GripperType("Robotiq2F85")] + cfg.robot_cfg.tcp_offset = rcs.GRIPPER_TCP_OFFSETS[common.GripperType.Robotiq2F85] cfg.robot_cfg.q_home = rcs.ROBOTS[RobotType.Panda].q_home return PandaMultiHardwareEnvCreatorConfig( diff --git a/extensions/rcs_panda/src/rcs_panda/creators.py b/extensions/rcs_panda/src/rcs_panda/creators.py index 28bace5a..7b9140b9 100644 --- a/extensions/rcs_panda/src/rcs_panda/creators.py +++ b/extensions/rcs_panda/src/rcs_panda/creators.py @@ -1,16 +1,9 @@ import logging -import typing from dataclasses import dataclass, field import gymnasium as gym -import rcs.hand.tilburg_hand -from rcs._core.common import BaseCameraConfig, Gripper, GripperConfig, GripperType -from rcs.camera.hw import ( - CalibrationStrategy, - DummyCalibrationStrategy, - HardwareCamera, - HardwareCameraSet, -) +from rcs._core.common import Gripper, GripperConfig, HandConfig +from rcs.camera.hw import HardwareCameraCreatorConfig, create_hardware_camera_set from rcs.envs.base import ( CameraSetWrapper, ControlMode, @@ -24,7 +17,6 @@ RobotWrapper, ) from rcs.envs.scenes import RCSEnvCreator, WrapperConfig -from rcs.hand.tilburg_hand import TilburgHand from rcs_panda._core import hw from rcs_panda.envs import PandaHW @@ -34,92 +26,21 @@ logger.setLevel(logging.INFO) -@dataclass(kw_only=True) -class HardwareCameraCreatorConfig: - camera_type_id: str - camera_cfgs: dict[str, BaseCameraConfig] - kwargs: dict[str, typing.Any] = field(default_factory=dict) - - -def _create_realsense_camera(cfg: HardwareCameraCreatorConfig) -> HardwareCamera: - try: - - # from rcs_realsense.calibration import FR3BaseArucoCalibration - from rcs_realsense.camera import RealSenseCameraSet - except ImportError as e: - msg = "RealSense camera support requires the `rcs_realsense` extension to be installed." - raise ImportError(msg) from e - - calibration_strategy = { - name: typing.cast(CalibrationStrategy, DummyCalibrationStrategy()) for name in cfg.camera_cfgs - } - return typing.cast( - HardwareCamera, - RealSenseCameraSet(cameras=cfg.camera_cfgs, calibration_strategy=calibration_strategy, **cfg.kwargs), - ) - - -def _create_digit_camera(cfg: HardwareCameraCreatorConfig) -> HardwareCamera: - try: - from rcs.camera.digit_cam import DigitCam - except ImportError as e: - msg = "DIGIT camera support requires the `digit_interface` package to be installed." - raise ImportError(msg) from e - - return typing.cast(HardwareCamera, DigitCam(cameras=cfg.camera_cfgs)) - - -HARDWARE_CAMERA_CREATORS: dict[str, typing.Callable[[HardwareCameraCreatorConfig], HardwareCamera]] = { - "realsense": _create_realsense_camera, - "digit": _create_digit_camera, -} - - -def _create_hardware_camera_set( - camera_cfgs: dict[str, HardwareCameraCreatorConfig] | None, -) -> HardwareCameraSet | None: - if camera_cfgs is None: - return None - cameras: list[HardwareCamera] = [] - for cfg in camera_cfgs.values(): - if cfg.camera_type_id not in HARDWARE_CAMERA_CREATORS: - msg = f"Unknown hardware camera type id: {cfg.camera_type_id}" - raise ValueError(msg) - cameras.append(HARDWARE_CAMERA_CREATORS[cfg.camera_type_id](cfg)) - return HardwareCameraSet(cameras) if cameras else None - - -def _create_franka_gripper(cfg: GripperConfig) -> Gripper: +def create_panda_gripper(cfg: GripperConfig) -> Gripper: + # The Panda hand is the Franka hand driven by this extension's libfranka build, so it has its + # own type id to keep it apart from `rcs_fr3`'s `FrankaHand` when both are installed. if not isinstance(cfg, hw.FHConfig): - msg = f"Expected FHConfig for franka gripper, got {type(cfg).__name__}" + msg = f"Expected rcs_panda FHConfig for the panda hand, got {type(cfg).__module__}.{type(cfg).__qualname__}" raise TypeError(msg) return hw.FrankaHand(cfg) -def _create_robotiq_gripper(cfg: GripperConfig) -> Gripper: - try: - from rcs_robotiq2f85.hw import RobotiQ2F85Gripper, RobotiQ2F85GripperConfig - except ImportError as e: - msg = "Robotiq gripper support requires the `rcs_robotiq2f85` extension to be installed." - raise ImportError(msg) from e - - if not isinstance(cfg, RobotiQ2F85GripperConfig): - msg = f"Expected RobotiQ2F85GripperConfig, got {type(cfg).__name__}" - raise TypeError(msg) - return typing.cast(Gripper, RobotiQ2F85Gripper(cfg)) - - -HARDWARE_GRIPPER_CREATORS: dict[str, typing.Callable[[GripperConfig], Gripper]] = { - GripperType.FrankaHand.id: _create_franka_gripper, - "robotiq2f85": _create_robotiq_gripper, -} - - @dataclass(kw_only=True) class PandaHardwareEnvCreatorConfig: robot_cfg: hw.PandaConfig control_mode: ControlMode - gripper_cfg: GripperConfig | rcs.hand.tilburg_hand.THConfig | None = None + gripper_cfg: GripperConfig | None = None + hand_cfg: HandConfig | None = None camera_cfgs: dict[str, HardwareCameraCreatorConfig] | None = None max_relative_movement: float | tuple[float, float] | None = None relative_to: RelativeTo = RelativeTo.LAST_STEP @@ -132,7 +53,8 @@ class PandaHardwareEnvCreatorConfig: class PandaMultiHardwareEnvCreatorConfig: robot_cfgs: dict[str, hw.PandaConfig] control_mode: ControlMode - gripper_cfgs: dict[str, GripperConfig | rcs.hand.tilburg_hand.THConfig | None] | None = None + gripper_cfgs: dict[str, GripperConfig | None] | None = None + hand_cfgs: dict[str, HandConfig | None] | None = None camera_cfgs: dict[str, HardwareCameraCreatorConfig] | None = None max_relative_movement: float | tuple[float, float] | None = None relative_to: RelativeTo = RelativeTo.LAST_STEP @@ -154,18 +76,14 @@ def create_env(self, cfg: PandaHardwareEnvCreatorConfig) -> gym.Env: env: gym.Env = HardwareEnv(frequency=cfg.frequency) env = RobotWrapper(env, robot, cfg.control_mode, home_on_reset=cfg.wrapper_cfg.home_on_reset) env = PandaHW(env) - if isinstance(cfg.gripper_cfg, rcs.hand.tilburg_hand.THConfig): - hand = TilburgHand(cfg.gripper_cfg) + if cfg.hand_cfg is not None: + hand = rcs.registry.HANDS.get(cfg.hand_cfg.hand_type.id)(cfg.hand_cfg) env = HandWrapper(env, hand, binary=cfg.wrapper_cfg.binary_gripper) elif cfg.gripper_cfg is not None: - gripper_type_id = cfg.gripper_cfg.gripper_type.id - if gripper_type_id not in HARDWARE_GRIPPER_CREATORS: - msg = f"Unknown hardware gripper type id: {gripper_type_id}" - raise ValueError(msg) - gripper = HARDWARE_GRIPPER_CREATORS[gripper_type_id](cfg.gripper_cfg) + gripper = rcs.registry.GRIPPERS.get(cfg.gripper_cfg.gripper_type.id)(cfg.gripper_cfg) env = GripperWrapper(env, gripper, binary=cfg.wrapper_cfg.binary_gripper) - camera_set = _create_hardware_camera_set(cfg.camera_cfgs) + camera_set = create_hardware_camera_set(cfg.camera_cfgs) if camera_set is not None: camera_set.start() camera_set.wait_for_frames() @@ -190,6 +108,7 @@ def create_env(self, cfg: PandaMultiHardwareEnvCreatorConfig) -> gym.Env: robot_cfg=robot_cfg, control_mode=cfg.control_mode, gripper_cfg=cfg.gripper_cfgs[robot_name] if cfg.gripper_cfgs is not None else None, + hand_cfg=cfg.hand_cfgs[robot_name] if cfg.hand_cfgs is not None else None, camera_cfgs=None, max_relative_movement=cfg.max_relative_movement, relative_to=cfg.relative_to, @@ -199,7 +118,7 @@ def create_env(self, cfg: PandaMultiHardwareEnvCreatorConfig) -> gym.Env: ) env: gym.Env = MultiRobotWrapper(envs, cfg.robot_to_shared_base_frame) - camera_set = _create_hardware_camera_set(cfg.camera_cfgs) + camera_set = create_hardware_camera_set(cfg.camera_cfgs) if camera_set is not None: camera_set.start() camera_set.wait_for_frames() diff --git a/extensions/rcs_realsense/pyproject.toml b/extensions/rcs_realsense/pyproject.toml index 63574b1f..abdfeb9a 100644 --- a/extensions/rcs_realsense/pyproject.toml +++ b/extensions/rcs_realsense/pyproject.toml @@ -18,6 +18,9 @@ maintainers = [{ name = "Tobias Juelg", email = "tobias.juelg@utn.de" }] authors = [{ name = "Tobias Juelg", email = "tobias.juelg@utn.de" }] requires-python = ">=3.11" +[project.entry-points."rcs.cameras"] +realsense = "rcs_realsense.creators:create_camera_set" + [tool.black] line-length = 120 target-version = ["py310"] diff --git a/extensions/rcs_realsense/src/rcs_realsense/camera.py b/extensions/rcs_realsense/src/rcs_realsense/camera.py index 5125dddb..762d94f5 100644 --- a/extensions/rcs_realsense/src/rcs_realsense/camera.py +++ b/extensions/rcs_realsense/src/rcs_realsense/camera.py @@ -6,7 +6,11 @@ import numpy as np import pyrealsense2 as rs -from rcs.camera.hw import CalibrationStrategy, DummyCalibrationStrategy, HardwareCamera +from rcs.camera.hw import ( + CalibrationStrategy, + HardwareCamera, + IdentityCalibrationStrategy, +) from rcs.camera.interface import BaseCameraSet, CameraFrame, DataFrame, Frame, IMUFrame from rcs import common @@ -50,7 +54,7 @@ def __init__( self.cameras = cameras self.align_depth_to_color = align_depth_to_color if calibration_strategy is None: - calibration_strategy = {camera_name: DummyCalibrationStrategy() for camera_name in cameras} + calibration_strategy = {camera_name: IdentityCalibrationStrategy() for camera_name in cameras} self.calibration_strategy = calibration_strategy self._logger = logging.getLogger(__name__) assert ( diff --git a/extensions/rcs_realsense/src/rcs_realsense/creators.py b/extensions/rcs_realsense/src/rcs_realsense/creators.py new file mode 100644 index 00000000..c1e0df19 --- /dev/null +++ b/extensions/rcs_realsense/src/rcs_realsense/creators.py @@ -0,0 +1,14 @@ +"""Factory for the `realsense` camera backend, registered as an `rcs.cameras` entry point.""" + +import typing + +from rcs.camera.hw import HardwareCamera, HardwareCameraCreatorConfig +from rcs_realsense.camera import RealSenseCameraSet + + +def create_camera_set(cfg: HardwareCameraCreatorConfig) -> HardwareCamera: + # calibration=None leaves the set to build identity strategies for every camera. + return typing.cast( + HardwareCamera, + RealSenseCameraSet(cameras=cfg.camera_cfgs, calibration_strategy=cfg.calibration, **cfg.kwargs), + ) diff --git a/extensions/rcs_realsense/src/rcs_realsense/utils.py b/extensions/rcs_realsense/src/rcs_realsense/utils.py index 2797fd29..f537be9e 100644 --- a/extensions/rcs_realsense/src/rcs_realsense/utils.py +++ b/extensions/rcs_realsense/src/rcs_realsense/utils.py @@ -18,7 +18,7 @@ def default_realsense(name2id: dict[str, str] | None) -> RealSenseCameraSet | No return RealSenseCameraSet(cameras=cameras, calibration_strategy=calibration_strategy) -def default_realsense_dummy_calibration(name2id: dict[str, str] | None) -> RealSenseCameraSet | None: +def default_realsense_identity_calibration(name2id: dict[str, str] | None) -> RealSenseCameraSet | None: if name2id is None: return None cameras = { diff --git a/extensions/rcs_robotiq2f85/pyproject.toml b/extensions/rcs_robotiq2f85/pyproject.toml index d042108b..66bf8977 100644 --- a/extensions/rcs_robotiq2f85/pyproject.toml +++ b/extensions/rcs_robotiq2f85/pyproject.toml @@ -20,3 +20,6 @@ authors = [ ] requires-python = ">=3.11" license = "AGPL-3.0-or-later" + +[project.entry-points."rcs.grippers"] +Robotiq2F85 = "rcs_robotiq2f85.creators:create_gripper" diff --git a/extensions/rcs_robotiq2f85/src/rcs_robotiq2f85/creators.py b/extensions/rcs_robotiq2f85/src/rcs_robotiq2f85/creators.py new file mode 100644 index 00000000..95490e12 --- /dev/null +++ b/extensions/rcs_robotiq2f85/src/rcs_robotiq2f85/creators.py @@ -0,0 +1,11 @@ +"""Factory for the Robotiq 2F-85 gripper, registered as an `rcs.grippers` entry point.""" + +from rcs._core.common import Gripper, GripperConfig +from rcs_robotiq2f85.hw import RobotiQ2F85Gripper, RobotiQ2F85GripperConfig + + +def create_gripper(cfg: GripperConfig) -> Gripper: + if not isinstance(cfg, RobotiQ2F85GripperConfig): + msg = f"Expected RobotiQ2F85GripperConfig for robotiq gripper, got {type(cfg).__name__}" + raise TypeError(msg) + return RobotiQ2F85Gripper(cfg) diff --git a/extensions/rcs_robotiq2f85/src/rcs_robotiq2f85/hw.py b/extensions/rcs_robotiq2f85/src/rcs_robotiq2f85/hw.py index 8046648b..4a4465c8 100644 --- a/extensions/rcs_robotiq2f85/src/rcs_robotiq2f85/hw.py +++ b/extensions/rcs_robotiq2f85/src/rcs_robotiq2f85/hw.py @@ -31,7 +31,7 @@ def __init__( self.speed = speed self.force = force self.async_control = async_control - self.gripper_type = rcs.common.GripperType("Robotiq2F85") + self.gripper_type = rcs.common.GripperType.Robotiq2F85 class RobotiQ2F85GripperState(GripperState): diff --git a/extensions/rcs_so101/src/rcs_so101/configs.py b/extensions/rcs_so101/src/rcs_so101/configs.py index e64c77e9..747335b6 100644 --- a/extensions/rcs_so101/src/rcs_so101/configs.py +++ b/extensions/rcs_so101/src/rcs_so101/configs.py @@ -16,12 +16,12 @@ def config(self) -> SO101HardwareEnvCreatorConfig: id=self.id, port=self.port, calibration_dir=self.calibration_dir, - robot_type=RobotType("SO101"), - kinematic_model_path=rcs.ROBOTS[RobotType("SO101")].mjcf_model_path, - attachment_site=rcs.ROBOTS[RobotType("SO101")].attachment_site, - dof=rcs.ROBOTS[RobotType("SO101")].dof, - joint_limits=rcs.ROBOTS[RobotType("SO101")].joint_limits, - q_home=rcs.ROBOTS[RobotType("SO101")].q_home, + robot_type=RobotType.SO101, + kinematic_model_path=rcs.ROBOTS[RobotType.SO101].mjcf_model_path, + attachment_site=rcs.ROBOTS[RobotType.SO101].attachment_site, + dof=rcs.ROBOTS[RobotType.SO101].dof, + joint_limits=rcs.ROBOTS[RobotType.SO101].joint_limits, + q_home=rcs.ROBOTS[RobotType.SO101].q_home, tcp_offset=rcs.common.Pose(), ) diff --git a/extensions/rcs_so101/src/rcs_so101/creators.py b/extensions/rcs_so101/src/rcs_so101/creators.py index 7381b497..21d13f9a 100644 --- a/extensions/rcs_so101/src/rcs_so101/creators.py +++ b/extensions/rcs_so101/src/rcs_so101/creators.py @@ -1,10 +1,8 @@ import logging -import typing from dataclasses import dataclass, field import gymnasium as gym -from rcs._core.common import BaseCameraConfig -from rcs.camera.hw import CalibrationStrategy, HardwareCamera, HardwareCameraSet +from rcs.camera.hw import HardwareCameraCreatorConfig, create_hardware_camera_set from rcs.envs.base import ( CameraSetWrapper, ControlMode, @@ -23,60 +21,6 @@ logger.setLevel(logging.INFO) -@dataclass(kw_only=True) -class HardwareCameraCreatorConfig: - camera_type_id: str - camera_cfgs: dict[str, BaseCameraConfig] - kwargs: dict[str, typing.Any] = field(default_factory=dict) - - -def _create_realsense_camera(cfg: HardwareCameraCreatorConfig) -> HardwareCamera: - try: - from rcs_realsense.calibration import FR3BaseArucoCalibration - from rcs_realsense.camera import RealSenseCameraSet - except ImportError as e: - msg = "RealSense camera support requires the `rcs_realsense` extension to be installed." - raise ImportError(msg) from e - - calibration_strategy = { - name: typing.cast(CalibrationStrategy, FR3BaseArucoCalibration(name)) for name in cfg.camera_cfgs - } - return typing.cast( - HardwareCamera, - RealSenseCameraSet(cameras=cfg.camera_cfgs, calibration_strategy=calibration_strategy, **cfg.kwargs), - ) - - -def _create_digit_camera(cfg: HardwareCameraCreatorConfig) -> HardwareCamera: - try: - from rcs.camera.digit_cam import DigitCam - except ImportError as e: - msg = "DIGIT camera support requires the `digit_interface` package to be installed." - raise ImportError(msg) from e - - return typing.cast(HardwareCamera, DigitCam(cameras=cfg.camera_cfgs)) - - -HARDWARE_CAMERA_CREATORS: dict[str, typing.Callable[[HardwareCameraCreatorConfig], HardwareCamera]] = { - "realsense": _create_realsense_camera, - "digit": _create_digit_camera, -} - - -def _create_hardware_camera_set( - camera_cfgs: dict[str, HardwareCameraCreatorConfig] | None, -) -> HardwareCameraSet | None: - if camera_cfgs is None: - return None - cameras: list[HardwareCamera] = [] - for cfg in camera_cfgs.values(): - if cfg.camera_type_id not in HARDWARE_CAMERA_CREATORS: - msg = f"Unknown hardware camera type id: {cfg.camera_type_id}" - raise ValueError(msg) - cameras.append(HARDWARE_CAMERA_CREATORS[cfg.camera_type_id](cfg)) - return HardwareCameraSet(cameras) if cameras else None - - @dataclass(kw_only=True) class SO101HardwareEnvCreatorConfig: robot_cfg: SO101Config @@ -103,7 +47,7 @@ def create_env(self, cfg: SO101HardwareEnvCreatorConfig) -> gym.Env: gripper = SO101Gripper(robot._hf_robot, robot) env = GripperWrapper(env, gripper, binary=cfg.wrapper_cfg.binary_gripper) - camera_set = _create_hardware_camera_set(cfg.camera_cfgs) + camera_set = create_hardware_camera_set(cfg.camera_cfgs) if camera_set is not None: camera_set.start() camera_set.wait_for_frames() diff --git a/extensions/rcs_taxim/src/rcs_taxim/creators.py b/extensions/rcs_taxim/src/rcs_taxim/creators.py index 0c3e4984..1dc597c5 100644 --- a/extensions/rcs_taxim/src/rcs_taxim/creators.py +++ b/extensions/rcs_taxim/src/rcs_taxim/creators.py @@ -13,7 +13,7 @@ import rcs -_TAXIM_GRIPPER_TYPE = GripperType("Robotiq2F85Digit") +_TAXIM_GRIPPER_TYPE = GripperType.Robotiq2F85Digit def _prefixed(name: str) -> str: @@ -84,7 +84,7 @@ def __call__( scene = EmptyWorldFR3() cfg = scene.config() - cfg.robot_cfgs["right"].tcp_offset = rcs.GRIPPER_TCP_OFFSETS[rcs.common.GripperType("Robotiq2F85")] + cfg.robot_cfgs["right"].tcp_offset = rcs.GRIPPER_TCP_OFFSETS[rcs.common.GripperType.Robotiq2F85] cfg.control_mode = control_mode cfg.headless = render_mode != "human" cfg.sim_cfg.realtime = render_mode == "human" @@ -99,7 +99,7 @@ def __call__( if not delta_actions: cfg.max_relative_movement = None cfg.gripper_cfgs = {"right": _taxim_gripper_cfg()} - cfg.gripper_offsets = {"right": rcs.GRIPPER_MOUNT_OFFSETS[rcs.common.GripperType("Robotiq2F85")]} + cfg.gripper_offsets = {"right": rcs.GRIPPER_MOUNT_OFFSETS[rcs.common.GripperType.Robotiq2F85]} cfg.root_frame_objects = { "": ( rcs.OBJECT_PATHS["green_cuboid"], diff --git a/extensions/rcs_tilburg_hand/README.md b/extensions/rcs_tilburg_hand/README.md new file mode 100644 index 00000000..04226969 --- /dev/null +++ b/extensions/rcs_tilburg_hand/README.md @@ -0,0 +1,48 @@ +# RCS Tilburg Hand Extension + +Support for the Tilburg Hand in RCS, built on the +[`tilburg-hand`](https://pypi.org/project/tilburg-hand/) driver. + +This extension depends on [`rcs-core`](https://pypi.org/project/rcs-core/). +Documentation: + +This covers the **hardware** hand only. The simulated hand, `rcs.sim.SimTilburgHand`, is built into +`rcs-core` and needs nothing from here. + +## Installation + +Install from PyPI: + +```shell +pip install rcs-tilburg-hand +``` + +Warning: plain `pip install rcs-tilburg-hand` will install the published `rcs-core` dependency from PyPI. + +Install from a local checkout: + +```shell +pip install -ve . +``` + +If you want this extension to use your local RCS checkout instead of the published `rcs-core` package, first install the main package from the repository root: + +```shell +pip install -ve . --no-build-isolation +pip install -ve extensions/rcs_tilburg_hand +``` + +## Usage + +The arm extensions accept a `THConfig` wherever they take a gripper config, and attach the hand +through a `HandWrapper` once this extension is installed: + +```python +from rcs_tilburg_hand.hand import THConfig + +cfg.gripper_cfg = THConfig( + calibration_file="/path/to/calibration.json", + grasp_percentage=1, + hand_orientation="right", +) +``` diff --git a/extensions/rcs_tilburg_hand/pyproject.toml b/extensions/rcs_tilburg_hand/pyproject.toml new file mode 100644 index 00000000..24059aa6 --- /dev/null +++ b/extensions/rcs_tilburg_hand/pyproject.toml @@ -0,0 +1,26 @@ +[build-system] +requires = ["setuptools"] +build-backend = "setuptools.build_meta" + +[project] +name = "rcs_tilburg_hand" +version = "0.7.3" +description = "RCS Tilburg Hand module" +readme = "README.md" +license = "AGPL-3.0-or-later" +dependencies = ["rcs-core>=0.7.3", "tilburg-hand"] +maintainers = [ + { name = "Tobias Juelg", email = "tobias.juelg@utn.de" }, +] +authors = [{ name = "Tobias Juelg", email = "tobias.juelg@utn.de" }] +requires-python = ">=3.11" + +[project.entry-points."rcs.hands"] +TilburgHand = "rcs_tilburg_hand.creators:create_hand" + +[tool.black] +line-length = 120 +target-version = ["py310"] + +[tool.isort] +profile = "black" diff --git a/extensions/rcs_tilburg_hand/src/rcs_tilburg_hand/__init__.py b/extensions/rcs_tilburg_hand/src/rcs_tilburg_hand/__init__.py new file mode 100644 index 00000000..4910b9ec --- /dev/null +++ b/extensions/rcs_tilburg_hand/src/rcs_tilburg_hand/__init__.py @@ -0,0 +1 @@ +__version__ = "0.7.3" diff --git a/extensions/rcs_tilburg_hand/src/rcs_tilburg_hand/creators.py b/extensions/rcs_tilburg_hand/src/rcs_tilburg_hand/creators.py new file mode 100644 index 00000000..6c16739a --- /dev/null +++ b/extensions/rcs_tilburg_hand/src/rcs_tilburg_hand/creators.py @@ -0,0 +1,11 @@ +"""Factory for the Tilburg Hand, registered as an `rcs.hands` entry point.""" + +from rcs._core.common import Hand, HandConfig +from rcs_tilburg_hand.hand import THConfig, TilburgHand + + +def create_hand(cfg: HandConfig) -> Hand: + if not isinstance(cfg, THConfig): + msg = f"Expected THConfig for tilburg hand, got {type(cfg).__name__}" + raise TypeError(msg) + return TilburgHand(cfg, verbose=cfg.verbose) diff --git a/python/rcs/hand/tilburg_hand.py b/extensions/rcs_tilburg_hand/src/rcs_tilburg_hand/hand.py similarity index 98% rename from python/rcs/hand/tilburg_hand.py rename to extensions/rcs_tilburg_hand/src/rcs_tilburg_hand/hand.py index 9800dc31..7f72672d 100644 --- a/python/rcs/hand/tilburg_hand.py +++ b/extensions/rcs_tilburg_hand/src/rcs_tilburg_hand/hand.py @@ -24,13 +24,15 @@ def __init__( control_unit: Unit = Unit.NORMALIZED, hand_orientation: str = "right", grasp_type: common.GraspType = common.GraspType.POWER_GRASP, + verbose: bool = False, ) -> None: - super().__init__() + super().__init__(hand_type=common.HandType.TilburgHand) self.calibration_file = calibration_file self.grasp_percentage = grasp_percentage self.control_unit = control_unit self.hand_orientation = hand_orientation self.grasp_type = grasp_type + self.verbose = verbose class TilburgHandState(common.HandState): diff --git a/extensions/rcs_ur5e/src/rcs_ur5e/configs.py b/extensions/rcs_ur5e/src/rcs_ur5e/configs.py index cf5ab1fd..01028668 100644 --- a/extensions/rcs_ur5e/src/rcs_ur5e/configs.py +++ b/extensions/rcs_ur5e/src/rcs_ur5e/configs.py @@ -10,7 +10,7 @@ class DefaultUR5eHardwareEnv(RCSUR5eConfigEnvCreator): ip = "192.168.1.15" def config(self) -> UR5eHardwareEnvCreatorConfig: - robot_type = RobotType("UR5e") + robot_type = RobotType.UR5e robot_cfg = UR5eConfig( ip=self.ip, max_velocity=1.0, @@ -31,7 +31,7 @@ def config(self) -> UR5eHardwareEnvCreatorConfig: gripper_cfg = RobotiQGripperConfig( ip=self.ip, - gripper_type=GripperType("Robotiq2F85"), + gripper_type=GripperType.Robotiq2F85, ) return UR5eHardwareEnvCreatorConfig( diff --git a/extensions/rcs_ur5e/src/rcs_ur5e/creators.py b/extensions/rcs_ur5e/src/rcs_ur5e/creators.py index f89f3880..4726fb54 100644 --- a/extensions/rcs_ur5e/src/rcs_ur5e/creators.py +++ b/extensions/rcs_ur5e/src/rcs_ur5e/creators.py @@ -1,10 +1,9 @@ import logging -import typing from dataclasses import dataclass, field import gymnasium as gym -from rcs._core.common import BaseCameraConfig, Gripper, GripperConfig -from rcs.camera.hw import CalibrationStrategy, HardwareCamera, HardwareCameraSet +from rcs._core.common import GripperConfig +from rcs.camera.hw import HardwareCameraCreatorConfig, create_hardware_camera_set from rcs.envs.base import ( CameraSetWrapper, ControlMode, @@ -16,7 +15,7 @@ RobotWrapper, ) from rcs.envs.scenes import RCSEnvCreator, WrapperConfig -from rcs_ur5e.hw import RobotiQGripper, RobotiQGripperConfig, UR5e, UR5eConfig +from rcs_ur5e.hw import UR5e, UR5eConfig import rcs @@ -24,72 +23,6 @@ logger.setLevel(logging.INFO) -@dataclass(kw_only=True) -class HardwareCameraCreatorConfig: - camera_type_id: str - camera_cfgs: dict[str, BaseCameraConfig] - kwargs: dict[str, typing.Any] = field(default_factory=dict) - - -def _create_realsense_camera(cfg: HardwareCameraCreatorConfig) -> HardwareCamera: - try: - from rcs_realsense.calibration import FR3BaseArucoCalibration - from rcs_realsense.camera import RealSenseCameraSet - except ImportError as e: - msg = "RealSense camera support requires the `rcs_realsense` extension to be installed." - raise ImportError(msg) from e - - calibration_strategy = { - name: typing.cast(CalibrationStrategy, FR3BaseArucoCalibration(name)) for name in cfg.camera_cfgs - } - return typing.cast( - HardwareCamera, - RealSenseCameraSet(cameras=cfg.camera_cfgs, calibration_strategy=calibration_strategy, **cfg.kwargs), - ) - - -def _create_digit_camera(cfg: HardwareCameraCreatorConfig) -> HardwareCamera: - try: - from rcs.camera.digit_cam import DigitCam - except ImportError as e: - msg = "DIGIT camera support requires the `digit_interface` package to be installed." - raise ImportError(msg) from e - - return typing.cast(HardwareCamera, DigitCam(cameras=cfg.camera_cfgs)) - - -HARDWARE_CAMERA_CREATORS: dict[str, typing.Callable[[HardwareCameraCreatorConfig], HardwareCamera]] = { - "realsense": _create_realsense_camera, - "digit": _create_digit_camera, -} - - -def _create_hardware_camera_set( - camera_cfgs: dict[str, HardwareCameraCreatorConfig] | None, -) -> HardwareCameraSet | None: - if camera_cfgs is None: - return None - cameras: list[HardwareCamera] = [] - for cfg in camera_cfgs.values(): - if cfg.camera_type_id not in HARDWARE_CAMERA_CREATORS: - msg = f"Unknown hardware camera type id: {cfg.camera_type_id}" - raise ValueError(msg) - cameras.append(HARDWARE_CAMERA_CREATORS[cfg.camera_type_id](cfg)) - return HardwareCameraSet(cameras) if cameras else None - - -def _create_robotiq_gripper(cfg: GripperConfig) -> Gripper: - if not isinstance(cfg, RobotiQGripperConfig): - msg = f"Expected RobotiQGripperConfig, got {type(cfg).__name__}" - raise TypeError(msg) - return RobotiQGripper(cfg=cfg) - - -HARDWARE_GRIPPER_CREATORS: dict[str, typing.Callable[[GripperConfig], Gripper]] = { - "Robotiq2F85siemens": _create_robotiq_gripper, -} - - @dataclass(kw_only=True) class UR5eHardwareEnvCreatorConfig: robot_cfg: UR5eConfig @@ -115,14 +48,10 @@ def create_env(self, cfg: UR5eHardwareEnvCreatorConfig) -> gym.Env: env = RobotWrapper(env, robot, cfg.control_mode, home_on_reset=cfg.wrapper_cfg.home_on_reset) if cfg.gripper_cfg is not None: - gripper_type_id = cfg.gripper_cfg.gripper_type.id - if gripper_type_id not in HARDWARE_GRIPPER_CREATORS: - msg = f"Unknown hardware gripper type id: {gripper_type_id}" - raise ValueError(msg) - gripper = HARDWARE_GRIPPER_CREATORS[gripper_type_id](cfg.gripper_cfg) + gripper = rcs.registry.GRIPPERS.get(cfg.gripper_cfg.gripper_type.id)(cfg.gripper_cfg) env = GripperWrapper(env, gripper, binary=cfg.wrapper_cfg.binary_gripper) - camera_set = _create_hardware_camera_set(cfg.camera_cfgs) + camera_set = create_hardware_camera_set(cfg.camera_cfgs) if camera_set is not None: camera_set.start() camera_set.wait_for_frames() diff --git a/extensions/rcs_ur5e/src/rcs_ur5e/hw.py b/extensions/rcs_ur5e/src/rcs_ur5e/hw.py index 9fa9530a..e310329e 100644 --- a/extensions/rcs_ur5e/src/rcs_ur5e/hw.py +++ b/extensions/rcs_ur5e/src/rcs_ur5e/hw.py @@ -33,7 +33,7 @@ def __init__( ): super().__init__(**kwargs) self.robot_platform = common.RobotPlatform.HARDWARE - self.robot_type = common.RobotType("UR5e") + self.robot_type = common.RobotType.UR5e self.ip = ip # Robot movement parameters self.max_velocity = max_velocity @@ -194,7 +194,7 @@ def __init__(self, cfg: UR5eConfig, ik: common.Kinematics): super().__init__() self.ik = ik self._config = cfg - self._config.robot_type = common.RobotType("UR5e") + self._config.robot_type = common.RobotType.UR5e self._ip = cfg.ip # Delete shared memory if it exists diff --git a/extensions/rcs_usb_cam/pyproject.toml b/extensions/rcs_usb_cam/pyproject.toml index 33e14d95..eb57df8f 100644 --- a/extensions/rcs_usb_cam/pyproject.toml +++ b/extensions/rcs_usb_cam/pyproject.toml @@ -16,6 +16,9 @@ maintainers = [ authors = [{ name = "Seongjin Bien", email = "seongjin.bien@utn.de" }] requires-python = ">=3.11" +[project.entry-points."rcs.cameras"] +usb = "rcs_usb_cam.creators:create_camera_set" + [tool.black] line-length = 120 target-version = ["py310"] diff --git a/extensions/rcs_usb_cam/src/rcs_usb_cam/camera.py b/extensions/rcs_usb_cam/src/rcs_usb_cam/camera.py index a434d01f..18519fcf 100644 --- a/extensions/rcs_usb_cam/src/rcs_usb_cam/camera.py +++ b/extensions/rcs_usb_cam/src/rcs_usb_cam/camera.py @@ -6,7 +6,11 @@ import cv2 import numpy as np -from rcs.camera.hw import CalibrationStrategy, DummyCalibrationStrategy, HardwareCamera +from rcs.camera.hw import ( + CalibrationStrategy, + HardwareCamera, + IdentityCalibrationStrategy, +) from rcs.camera.interface import CameraFrame, DataFrame, Frame from rcs import common @@ -33,7 +37,7 @@ def __init__( self.cameras = cameras self.CALIBRATION_FRAME_SIZE = 30 if calibration_strategy is None: - calibration_strategy = {camera_name: DummyCalibrationStrategy() for camera_name in cameras} + calibration_strategy = {camera_name: IdentityCalibrationStrategy() for camera_name in cameras} for cam in self.cameras.values(): if cam.color_intrinsics is None: cam.color_intrinsics = np.zeros((3, 4), dtype=np.float64) # type: ignore diff --git a/extensions/rcs_usb_cam/src/rcs_usb_cam/creators.py b/extensions/rcs_usb_cam/src/rcs_usb_cam/creators.py new file mode 100644 index 00000000..66b232d7 --- /dev/null +++ b/extensions/rcs_usb_cam/src/rcs_usb_cam/creators.py @@ -0,0 +1,19 @@ +"""Factory for the `usb` camera backend, registered as an `rcs.cameras` entry point.""" + +import typing + +from rcs.camera.hw import HardwareCamera, HardwareCameraCreatorConfig +from rcs_usb_cam.camera import USBCameraConfig, USBCameraSet + + +def create_camera_set(cfg: HardwareCameraCreatorConfig) -> HardwareCamera: + # The set reads USB specific fields (intrinsics, distortion) off every camera config. + for name, camera_cfg in cfg.camera_cfgs.items(): + if not isinstance(camera_cfg, USBCameraConfig): + msg = f"Expected USBCameraConfig for usb camera {name!r}, got {type(camera_cfg).__name__}" + raise TypeError(msg) + cameras = typing.cast(dict[str, USBCameraConfig], cfg.camera_cfgs) + # calibration=None leaves the set to build identity strategies for every camera. + return typing.cast( + HardwareCamera, USBCameraSet(cameras=cameras, calibration_strategy=cfg.calibration, **cfg.kwargs) + ) diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/configs.py b/extensions/rcs_xarm7/src/rcs_xarm7/configs.py index 792518ee..020343c3 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/configs.py +++ b/extensions/rcs_xarm7/src/rcs_xarm7/configs.py @@ -10,7 +10,7 @@ class DefaultXArm7HardwareEnv(RCSXArm7ConfigEnvCreator): ip = "192.168.1.245" def config(self) -> XArm7HardwareEnvCreatorConfig: - robot_type = RobotType("XArm7") + robot_type = RobotType.XArm7 robot_cfg = XArm7Config( ip=self.ip, payload_weight=0.624, diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py index a4edb2be..9770c0bc 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py +++ b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py @@ -1,12 +1,11 @@ import logging -import typing from dataclasses import dataclass, field from os import PathLike from pathlib import Path import gymnasium as gym -from rcs._core.common import BaseCameraConfig -from rcs.camera.hw import CalibrationStrategy, HardwareCamera, HardwareCameraSet +from rcs._core.common import HandConfig +from rcs.camera.hw import HardwareCameraCreatorConfig, create_hardware_camera_set from rcs.envs.base import ( CameraSetWrapper, ControlMode, @@ -18,7 +17,6 @@ RobotWrapper, ) from rcs.envs.scenes import RCSEnvCreator, WrapperConfig -from rcs.hand.tilburg_hand import THConfig, TilburgHand from rcs_xarm7.hw import XArm7, XArm7Config import rcs @@ -27,67 +25,13 @@ logger.setLevel(logging.INFO) -@dataclass(kw_only=True) -class HardwareCameraCreatorConfig: - camera_type_id: str - camera_cfgs: dict[str, BaseCameraConfig] - kwargs: dict[str, typing.Any] = field(default_factory=dict) - - -def _create_realsense_camera(cfg: HardwareCameraCreatorConfig) -> HardwareCamera: - try: - from rcs_realsense.calibration import FR3BaseArucoCalibration - from rcs_realsense.camera import RealSenseCameraSet - except ImportError as e: - msg = "RealSense camera support requires the `rcs_realsense` extension to be installed." - raise ImportError(msg) from e - - calibration_strategy = { - name: typing.cast(CalibrationStrategy, FR3BaseArucoCalibration(name)) for name in cfg.camera_cfgs - } - return typing.cast( - HardwareCamera, - RealSenseCameraSet(cameras=cfg.camera_cfgs, calibration_strategy=calibration_strategy, **cfg.kwargs), - ) - - -def _create_digit_camera(cfg: HardwareCameraCreatorConfig) -> HardwareCamera: - try: - from rcs.camera.digit_cam import DigitCam - except ImportError as e: - msg = "DIGIT camera support requires the `digit_interface` package to be installed." - raise ImportError(msg) from e - - return typing.cast(HardwareCamera, DigitCam(cameras=cfg.camera_cfgs)) - - -HARDWARE_CAMERA_CREATORS: dict[str, typing.Callable[[HardwareCameraCreatorConfig], HardwareCamera]] = { - "realsense": _create_realsense_camera, - "digit": _create_digit_camera, -} - - -def _create_hardware_camera_set( - camera_cfgs: dict[str, HardwareCameraCreatorConfig] | None, -) -> HardwareCameraSet | None: - if camera_cfgs is None: - return None - cameras: list[HardwareCamera] = [] - for cfg in camera_cfgs.values(): - if cfg.camera_type_id not in HARDWARE_CAMERA_CREATORS: - msg = f"Unknown hardware camera type id: {cfg.camera_type_id}" - raise ValueError(msg) - cameras.append(HARDWARE_CAMERA_CREATORS[cfg.camera_type_id](cfg)) - return HardwareCameraSet(cameras) if cameras else None - - @dataclass(kw_only=True) class XArm7HardwareEnvCreatorConfig: robot_cfg: XArm7Config control_mode: ControlMode calibration_dir: PathLike | str | None = None camera_cfgs: dict[str, HardwareCameraCreatorConfig] | None = None - hand_cfg: THConfig | None = None + hand_cfg: HandConfig | None = None max_relative_movement: float | tuple[float, float] | None = None relative_to: RelativeTo = RelativeTo.LAST_STEP frequency: float | None = None @@ -109,14 +53,14 @@ def create_env(self, cfg: XArm7HardwareEnvCreatorConfig) -> gym.Env: env: gym.Env = HardwareEnv(frequency=cfg.frequency) env = RobotWrapper(env, robot, cfg.control_mode, home_on_reset=cfg.wrapper_cfg.home_on_reset) - camera_set = _create_hardware_camera_set(cfg.camera_cfgs) + camera_set = create_hardware_camera_set(cfg.camera_cfgs) if camera_set is not None: camera_set.start() camera_set.wait_for_frames() logger.info("CameraSet started") env = CameraSetWrapper(env, camera_set, include_depth=True) if cfg.hand_cfg is not None: - hand = TilburgHand(cfg=cfg.hand_cfg, verbose=True) + hand = rcs.registry.HANDS.get(cfg.hand_cfg.hand_type.id)(cfg.hand_cfg) env = HandWrapper(env, hand, cfg.wrapper_cfg.binary_gripper) if cfg.relative_to != RelativeTo.NONE: diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/env_grasp.py b/extensions/rcs_xarm7/src/rcs_xarm7/env_grasp.py index 53f4d1a4..4115487c 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/env_grasp.py +++ b/extensions/rcs_xarm7/src/rcs_xarm7/env_grasp.py @@ -5,7 +5,7 @@ from rcs._core.common import RobotPlatform from rcs.envs.base import ControlMode, RelativeTo from rcs.envs.configs import EmptyWorldXArm7 -from rcs.hand.tilburg_hand import THConfig +from rcs_tilburg_hand.hand import THConfig from rcs_xarm7.configs import DefaultXArm7HardwareEnv logger = logging.getLogger(__name__) diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/hw.py b/extensions/rcs_xarm7/src/rcs_xarm7/hw.py index dd48f56d..319057bc 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/hw.py +++ b/extensions/rcs_xarm7/src/rcs_xarm7/hw.py @@ -34,7 +34,7 @@ def __init__(self, cfg: XArm7Config, ik: common.Kinematics): self.ik = ik self._config = cfg self._config.robot_platform = common.RobotPlatform.HARDWARE - self._config.robot_type = common.RobotType("XArm7") + self._config.robot_type = common.RobotType.XArm7 self._xarm = XArmAPI(cfg.ip) self._xarm.set_mode(0) diff --git a/extensions/rcs_yam/src/rcs_yam/configs.py b/extensions/rcs_yam/src/rcs_yam/configs.py index 2d7a6f13..613ae6de 100644 --- a/extensions/rcs_yam/src/rcs_yam/configs.py +++ b/extensions/rcs_yam/src/rcs_yam/configs.py @@ -18,8 +18,8 @@ class DefaultYamHardwareEnv(RCSYamConfigEnvCreator): gripper_force = 50.0 def config(self) -> YamHardwareEnvCreatorConfig: - robot_type = RobotType("Yam") - gripper_type = GripperType("Yam") + robot_type = RobotType.Yam + gripper_type = GripperType.Yam robot_cfg = YamConfig( channel=self.channel, gripper_type_id="linear_4310", diff --git a/extensions/rcs_yam/src/rcs_yam/creators.py b/extensions/rcs_yam/src/rcs_yam/creators.py index aa42554a..16f663c7 100644 --- a/extensions/rcs_yam/src/rcs_yam/creators.py +++ b/extensions/rcs_yam/src/rcs_yam/creators.py @@ -1,15 +1,9 @@ import logging -import typing from dataclasses import dataclass, field import gymnasium as gym -from rcs._core.common import BaseCameraConfig, GripperConfig -from rcs.camera.hw import ( - CalibrationStrategy, - DummyCalibrationStrategy, - HardwareCamera, - HardwareCameraSet, -) +from rcs._core.common import GripperConfig +from rcs.camera.hw import HardwareCameraCreatorConfig, create_hardware_camera_set from rcs.envs.base import ( CameraSetWrapper, ControlMode, @@ -30,52 +24,10 @@ logger.setLevel(logging.INFO) -@dataclass(kw_only=True) -class HardwareCameraCreatorConfig: - camera_type_id: str - camera_cfgs: dict[str, BaseCameraConfig] - kwargs: dict[str, typing.Any] = field(default_factory=dict) - - -def _create_realsense_camera(cfg: HardwareCameraCreatorConfig) -> HardwareCamera: - try: - from rcs_realsense.camera import RealSenseCameraSet - except ImportError as e: - msg = "RealSense camera support requires the `rcs_realsense` extension to be installed." - raise ImportError(msg) from e - - calibration_strategy = { - name: typing.cast(CalibrationStrategy, DummyCalibrationStrategy()) for name in cfg.camera_cfgs - } - return typing.cast( - HardwareCamera, - RealSenseCameraSet(cameras=cfg.camera_cfgs, calibration_strategy=calibration_strategy, **cfg.kwargs), - ) - - -HARDWARE_CAMERA_CREATORS: dict[str, typing.Callable[[HardwareCameraCreatorConfig], HardwareCamera]] = { - "realsense": _create_realsense_camera, -} - - -def _create_hardware_camera_set( - camera_cfgs: dict[str, HardwareCameraCreatorConfig] | None, -) -> HardwareCameraSet | None: - if camera_cfgs is None: - return None - cameras: list[HardwareCamera] = [] - for cfg in camera_cfgs.values(): - if cfg.camera_type_id not in HARDWARE_CAMERA_CREATORS: - msg = f"Unknown hardware camera type id: {cfg.camera_type_id}" - raise ValueError(msg) - cameras.append(HARDWARE_CAMERA_CREATORS[cfg.camera_type_id](cfg)) - return HardwareCameraSet(cameras) if cameras else None - - def _attach_camera_set( env: gym.Env, camera_cfgs: dict[str, HardwareCameraCreatorConfig] | None, include_depth: bool ) -> gym.Env: - camera_set = _create_hardware_camera_set(camera_cfgs) + camera_set = create_hardware_camera_set(camera_cfgs) if camera_set is None: return env camera_set.start() diff --git a/extensions/rcs_yam/src/rcs_yam/hw.py b/extensions/rcs_yam/src/rcs_yam/hw.py index 3142b471..ecf414a7 100644 --- a/extensions/rcs_yam/src/rcs_yam/hw.py +++ b/extensions/rcs_yam/src/rcs_yam/hw.py @@ -69,7 +69,7 @@ def __init__( ): super().__init__(**kwargs) self.robot_platform = common.RobotPlatform.HARDWARE - self.robot_type = common.RobotType("Yam") + self.robot_type = common.RobotType.Yam self.channel = channel self.arm_type_id = arm_type_id self.gripper_type_id = gripper_type_id diff --git a/extensions/rcs_yam/src/rcs_yam/scripts/test_robot.py b/extensions/rcs_yam/src/rcs_yam/scripts/test_robot.py index 75a9908f..ddf2b58d 100644 --- a/extensions/rcs_yam/src/rcs_yam/scripts/test_robot.py +++ b/extensions/rcs_yam/src/rcs_yam/scripts/test_robot.py @@ -13,8 +13,8 @@ CHANNEL = "can0" -robot_type = common.RobotType("Yam") -gripper_type = common.GripperType("Yam") +robot_type = common.RobotType.Yam +gripper_type = common.GripperType.Yam robot_config = YamConfig( channel=CHANNEL, async_control=False, diff --git a/extensions/rcs_zed/README.md b/extensions/rcs_zed/README.md index a612e8f4..aad0354f 100644 --- a/extensions/rcs_zed/README.md +++ b/extensions/rcs_zed/README.md @@ -35,7 +35,7 @@ pip install -ve extensions/rcs_zed ## Calibration `default_zed(...)` is standalone and uses RCS's identity -`DummyCalibrationStrategy` by default. To use measured extrinsics, pass one +`IdentityCalibrationStrategy` by default. To use measured extrinsics, pass one calibration strategy per logical camera: ```python diff --git a/extensions/rcs_zed/pyproject.toml b/extensions/rcs_zed/pyproject.toml index b2683fb8..ba8c7d5e 100644 --- a/extensions/rcs_zed/pyproject.toml +++ b/extensions/rcs_zed/pyproject.toml @@ -17,6 +17,9 @@ maintainers = [{ name = "Tobias Juelg", email = "tobias.juelg@utn.de" }] authors = [{ name = "Tobias Juelg", email = "tobias.juelg@utn.de" }] requires-python = ">=3.11" +[project.entry-points."rcs.cameras"] +zed = "rcs_zed.creators:create_camera_set" + [tool.black] line-length = 120 target-version = ["py310"] diff --git a/extensions/rcs_zed/src/rcs_zed/camera.py b/extensions/rcs_zed/src/rcs_zed/camera.py index 8e86481d..67f81f6d 100644 --- a/extensions/rcs_zed/src/rcs_zed/camera.py +++ b/extensions/rcs_zed/src/rcs_zed/camera.py @@ -6,7 +6,11 @@ from time import time import numpy as np -from rcs.camera.hw import CalibrationStrategy, DummyCalibrationStrategy, HardwareCamera +from rcs.camera.hw import ( + CalibrationStrategy, + HardwareCamera, + IdentityCalibrationStrategy, +) from rcs.camera.interface import BaseCameraSet, CameraFrame, DataFrame, Frame, IMUFrame from rcs import common @@ -255,7 +259,7 @@ def __init__( ) -> None: self.cameras = cameras if calibration_strategy is None: - calibration_strategy = {camera_name: DummyCalibrationStrategy() for camera_name in cameras} + calibration_strategy = {camera_name: IdentityCalibrationStrategy() for camera_name in cameras} self.calibration_strategy = calibration_strategy self.enable_depth = enable_depth self.enable_imu = enable_imu diff --git a/extensions/rcs_zed/src/rcs_zed/creators.py b/extensions/rcs_zed/src/rcs_zed/creators.py new file mode 100644 index 00000000..3586e408 --- /dev/null +++ b/extensions/rcs_zed/src/rcs_zed/creators.py @@ -0,0 +1,14 @@ +"""Factory for the `zed` camera backend, registered as an `rcs.cameras` entry point.""" + +import typing + +from rcs.camera.hw import HardwareCamera, HardwareCameraCreatorConfig +from rcs_zed.camera import ZEDCameraSet + + +def create_camera_set(cfg: HardwareCameraCreatorConfig) -> HardwareCamera: + # calibration=None leaves the set to build identity strategies for every camera. + return typing.cast( + HardwareCamera, + ZEDCameraSet(cameras=cfg.camera_cfgs, calibration_strategy=cfg.calibration, **cfg.kwargs), + ) diff --git a/extensions/rcs_zed/src/rcs_zed/utils.py b/extensions/rcs_zed/src/rcs_zed/utils.py index 087cc55a..56840fad 100644 --- a/extensions/rcs_zed/src/rcs_zed/utils.py +++ b/extensions/rcs_zed/src/rcs_zed/utils.py @@ -14,7 +14,7 @@ def default_zed( name2id: Mapping from logical camera names to ZED serial numbers. calibration_strategy: Optional calibration strategy for each logical camera. When omitted, ``ZEDCameraSet`` uses - ``DummyCalibrationStrategy``. + ``IdentityCalibrationStrategy``. """ if name2id is None: return None @@ -25,6 +25,6 @@ def default_zed( return ZEDCameraSet(cameras=cameras, calibration_strategy=calibration_strategy) -def default_zed_dummy_calibration(name2id: dict[str, str] | None) -> ZEDCameraSet | None: - """Create the default ZED camera set with dummy calibration.""" +def default_zed_identity_calibration(name2id: dict[str, str] | None) -> ZEDCameraSet | None: + """Create the default ZED camera set with identity calibration.""" return default_zed(name2id) diff --git a/extensions/rcs_zed/tests/test_zed_extension.py b/extensions/rcs_zed/tests/test_zed_extension.py index 6943cc79..1aa44212 100644 --- a/extensions/rcs_zed/tests/test_zed_extension.py +++ b/extensions/rcs_zed/tests/test_zed_extension.py @@ -9,9 +9,9 @@ sys.path.insert(0, str(REPO_ROOT / "python")) sys.path.insert(0, str(REPO_ROOT / "extensions/rcs_zed/src")) -from rcs.camera.hw import DummyCalibrationStrategy # noqa: E402 +from rcs.camera.hw import IdentityCalibrationStrategy # noqa: E402 from rcs_zed.camera import ZEDCameraSet, ZEDDeviceInfo, ZEDFrameBundle # noqa: E402 -from rcs_zed.utils import default_zed, default_zed_dummy_calibration # noqa: E402 +from rcs_zed.utils import default_zed, default_zed_identity_calibration # noqa: E402 from rcs import common # noqa: E402 @@ -175,11 +175,11 @@ def test_zed_include_right_adds_logical_right_camera_without_double_grab(patch_z assert right_frame.camera.depth is None -def test_default_zed_uses_builtin_dummy_calibration(): +def test_default_zed_uses_builtin_identity_calibration(): camera_set = default_zed({"wrist": "123"}) assert camera_set is not None - assert isinstance(camera_set.calibration_strategy["wrist"], DummyCalibrationStrategy) + assert isinstance(camera_set.calibration_strategy["wrist"], IdentityCalibrationStrategy) def test_default_zed_accepts_explicit_calibration_strategy(): @@ -193,8 +193,8 @@ def test_default_zed_accepts_explicit_calibration_strategy(): assert camera_set.calibration_strategy == {"wrist": calibration} -def test_default_zed_dummy_calibration_remains_compatible(): - camera_set = default_zed_dummy_calibration({"wrist": "123"}) +def test_default_zed_identity_calibration_remains_compatible(): + camera_set = default_zed_identity_calibration({"wrist": "123"}) assert camera_set is not None - assert isinstance(camera_set.calibration_strategy["wrist"], DummyCalibrationStrategy) + assert isinstance(camera_set.calibration_strategy["wrist"], IdentityCalibrationStrategy) diff --git a/include/rcs/Robot.h b/include/rcs/Robot.h index cf33b36d..94b6dea0 100644 --- a/include/rcs/Robot.h +++ b/include/rcs/Robot.h @@ -46,9 +46,17 @@ struct RobotType : public TypeBase { static const RobotType FR3; static const RobotType Panda; + static const RobotType XArm7; + static const RobotType UR5e; + static const RobotType SO101; + static const RobotType Yam; }; inline const RobotType RobotType::FR3{"FR3"}; inline const RobotType RobotType::Panda{"Panda"}; +inline const RobotType RobotType::XArm7{"XArm7"}; +inline const RobotType RobotType::UR5e{"UR5e"}; +inline const RobotType RobotType::SO101{"SO101"}; +inline const RobotType RobotType::Yam{"Yam"}; struct RobotConfig { RobotType robot_type = RobotType::FR3; @@ -76,8 +84,18 @@ struct GripperType : public TypeBase { using TypeBase::TypeBase; static const GripperType FrankaHand; + static const GripperType PandaHand; + static const GripperType Robotiq2F85; + static const GripperType Robotiq2F85Digit; + static const GripperType SO101; + static const GripperType Yam; }; inline const GripperType GripperType::FrankaHand{"FrankaHand"}; +inline const GripperType GripperType::PandaHand{"PandaHand"}; +inline const GripperType GripperType::Robotiq2F85{"Robotiq2F85"}; +inline const GripperType GripperType::Robotiq2F85Digit{"Robotiq2F85Digit"}; +inline const GripperType GripperType::SO101{"SO101"}; +inline const GripperType GripperType::Yam{"Yam"}; struct GripperConfig { GripperType gripper_type = GripperType::FrankaHand; @@ -93,7 +111,15 @@ enum GraspType { LATERAL_GRASP, TRIPOD_GRASP }; +struct HandType : public TypeBase { + using TypeBase::TypeBase; + + static const HandType TilburgHand; +}; +inline const HandType HandType::TilburgHand{"TilburgHand"}; + struct HandConfig { + HandType hand_type = HandType::TilburgHand; virtual ~HandConfig() {}; }; struct HandState { diff --git a/pyproject.toml b/pyproject.toml index 2079ea41..bfa11d07 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -29,8 +29,6 @@ dependencies = [ "pillow>=10.3", "python-dotenv>=1.0.1", "opencv-python~=4.10.0.84", - "tilburg-hand", - "digit-interface", "pyopengl>=3.1.9", "ompl>=1.7.0; sys_platform == 'linux'", "rpyc~=6.0.2", @@ -213,6 +211,10 @@ version_files = [ "extensions/rcs_so101/pyproject.toml:\"rcs-core>=(.*)\"", # --- Other Extensions --- + "extensions/rcs_digit/pyproject.toml:version", + "extensions/rcs_digit/src/rcs_digit/__init__.py:__version__", + "extensions/rcs_digit/pyproject.toml:\"rcs-core>=(.*)\"", + "extensions/rcs_realsense/pyproject.toml:version", "extensions/rcs_realsense/src/rcs_realsense/__init__.py:__version__", "extensions/rcs_realsense/pyproject.toml:\"rcs-core>=(.*)\"", @@ -221,6 +223,10 @@ version_files = [ "extensions/rcs_xarm7/src/rcs_xarm7/__init__.py:__version__", "extensions/rcs_xarm7/pyproject.toml:\"rcs-core>=(.*)\"", + "extensions/rcs_tilburg_hand/pyproject.toml:version", + "extensions/rcs_tilburg_hand/src/rcs_tilburg_hand/__init__.py:__version__", + "extensions/rcs_tilburg_hand/pyproject.toml:\"rcs-core>=(.*)\"", + "extensions/rcs_ur5e/pyproject.toml:version", "extensions/rcs_ur5e/src/rcs_ur5e/__init__.py:__version__", "extensions/rcs_ur5e/pyproject.toml:\"rcs-core>=(.*)\"", diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py index d8ed06a9..6adf3174 100644 --- a/python/rcs/__init__.py +++ b/python/rcs/__init__.py @@ -12,7 +12,7 @@ import requests from rcs._core import __version__, common -from rcs import camera, envs, hand, sim +from rcs import camera, envs, registry, sim GITHUB_ASSET_ARCHIVE_URL = "https://github.com/RobotControlStack/robot-control-stack/archive/refs/tags/{tag}.zip" REQUIRED_ASSET = Path("assets/scenes/empty_world/scene.xml") @@ -142,7 +142,7 @@ class RobotMetaConfig: ] ), ), - common.RobotType("XArm7"): RobotMetaConfig( + common.RobotType.XArm7: RobotMetaConfig( mjcf_model_path="assets/robots/xarm7/xarm7.xml", dof=7, q_home=np.array([0, -45.0 / 180.0 * np.pi, 0, 15.0 / 180.0 * np.pi, 0, -25.0 / 180.0 * np.pi, 0]), @@ -153,7 +153,7 @@ class RobotMetaConfig: ] ), ), - common.RobotType("UR5e"): RobotMetaConfig( + common.RobotType.UR5e: RobotMetaConfig( mjcf_model_path="assets/robots/ur5e/ur5e.xml", dof=6, q_home=np.array([0.0, -2.02711196, 1.64630026, -1.18999615, -1.57079762, 0.0]), @@ -164,7 +164,7 @@ class RobotMetaConfig: ] ), ), - common.RobotType("SO101"): RobotMetaConfig( + common.RobotType.SO101: RobotMetaConfig( mjcf_model_path="assets/robots/so101/so101.xml", dof=5, q_home=np.array([-0.01914898, -1.90521916, 1.56476701, 1.04783839, -1.40323926]), @@ -182,7 +182,7 @@ class RobotMetaConfig: ), attachment_site="gripper", ), - common.RobotType("Yam"): RobotMetaConfig( + common.RobotType.Yam: RobotMetaConfig( mjcf_model_path="assets/robots/yam/yam.xml", dof=6, q_home=np.array([0.0, 1.047, 1.047, 0.0, 0.0, 0.0]), @@ -199,22 +199,27 @@ class RobotMetaConfig: GRIPPER_PATHS: dict[common.GripperType, str] = { common.GripperType.FrankaHand: "assets/grippers/franka_hand/franka_hand.xml", - common.GripperType("Robotiq2F85"): "assets/grippers/robotiq_2f85/robotiq_2f85.xml", + common.GripperType.PandaHand: "assets/grippers/franka_hand/franka_hand.xml", + common.GripperType.Robotiq2F85: "assets/grippers/robotiq_2f85/robotiq_2f85.xml", } GRIPPER_TCP_OFFSETS: dict[common.GripperType, common.Pose] = { common.GripperType.FrankaHand: common.Pose(pose_matrix=common.FrankaHandTCPOffset()), - common.GripperType("Robotiq2F85"): common.Pose(translation=np.array([0, 0.0, 0.1493])), + common.GripperType.PandaHand: common.Pose(pose_matrix=common.FrankaHandTCPOffset()), + common.GripperType.Robotiq2F85: common.Pose(translation=np.array([0, 0.0, 0.1493])), # The yam gripper is part of the robot mjcf, hence it needs no entry in GRIPPER_PATHS # and no mount offset, only the offset from the flange to the point between the fingers. - common.GripperType("Yam"): common.Pose(translation=np.array([0.0, 0.0, 0.1347])), + common.GripperType.Yam: common.Pose(translation=np.array([0.0, 0.0, 0.1347])), } GRIPPER_MOUNT_OFFSETS: dict[common.GripperType, common.Pose] = { common.GripperType.FrankaHand: common.Pose( rotation=common.FrankaHandTCPOffset()[:3, :3], translation=np.array([0.0, 0.0, 0.0]) ), - common.GripperType("Robotiq2F85"): common.Pose( + common.GripperType.PandaHand: common.Pose( + rotation=common.FrankaHandTCPOffset()[:3, :3], translation=np.array([0.0, 0.0, 0.0]) + ), + common.GripperType.Robotiq2F85: common.Pose( translation=np.array([0.0, 0.0, 0.0]), quaternion=np.array([0.0, 0.0, 0.7071068, 0.7071068]) ), } @@ -303,7 +308,7 @@ class RobotMetaConfig: "sim", "camera", "envs", - "hand", + "registry", "ROBOTS", "GRIPPER_PATHS", "SCENE_PATHS", diff --git a/python/rcs/_core/common.pyi b/python/rcs/_core/common.pyi index 05a073b6..ecbf5a03 100644 --- a/python/rcs/_core/common.pyi +++ b/python/rcs/_core/common.pyi @@ -20,6 +20,7 @@ __all__: list[str] = [ "HARDWARE", "Hand", "HandConfig", + "HandType", "HandState", "IdentityRotMatrix", "IdentityRotQuatVec", @@ -108,6 +109,11 @@ class GripperState: class GripperType: FrankaHand: typing.ClassVar[GripperType] # value = + PandaHand: typing.ClassVar[GripperType] # value = + Robotiq2F85: typing.ClassVar[GripperType] # value = + Robotiq2F85Digit: typing.ClassVar[GripperType] # value = + SO101: typing.ClassVar[GripperType] # value = + Yam: typing.ClassVar[GripperType] # value = @staticmethod def get_all() -> list[GripperType]: ... def __eq__(self, arg0: typing.Any) -> bool: ... @@ -130,8 +136,20 @@ class Hand: def set_normalized_joint_poses(self, q: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]) -> None: ... def shut(self) -> None: ... +class HandType: + TilburgHand: typing.ClassVar[HandType] # value = + @staticmethod + def get_all() -> list[HandType]: ... + def __eq__(self, arg0: typing.Any) -> bool: ... + def __hash__(self) -> int: ... + def __init__(self, arg0: str) -> None: ... + def __repr__(self) -> str: ... + @property + def id(self) -> str: ... + class HandConfig: - def __init__(self) -> None: ... + hand_type: HandType + def __init__(self, hand_type: HandType = ...) -> None: ... class HandState: def __init__(self) -> None: ... @@ -295,6 +313,10 @@ class RobotState: class RobotType: FR3: typing.ClassVar[RobotType] # value = Panda: typing.ClassVar[RobotType] # value = + SO101: typing.ClassVar[RobotType] # value = + UR5e: typing.ClassVar[RobotType] # value = + XArm7: typing.ClassVar[RobotType] # value = + Yam: typing.ClassVar[RobotType] # value = @staticmethod def get_all() -> list[RobotType]: ... def __eq__(self, arg0: typing.Any) -> bool: ... diff --git a/python/rcs/camera/hw.py b/python/rcs/camera/hw.py index da8dc8d9..0596a9d4 100644 --- a/python/rcs/camera/hw.py +++ b/python/rcs/camera/hw.py @@ -2,6 +2,7 @@ import threading import typing from collections.abc import Sequence +from dataclasses import dataclass, field from datetime import datetime from pathlib import Path from time import sleep @@ -12,6 +13,8 @@ from rcs.camera.interface import BaseCameraSet, Frame, FrameSet from rcs.utils import SimpleFrameRate +from rcs import registry + class HardwareCamera(typing.Protocol): """Implementation of a hardware camera potentially a set of cameras of the same kind.""" @@ -71,7 +74,7 @@ def get_extrinsics(self) -> np.ndarray[tuple[typing.Literal[4], typing.Literal[4 """ -class DummyCalibrationStrategy(CalibrationStrategy): +class IdentityCalibrationStrategy(CalibrationStrategy): """Always returns identity extrinsics.""" def calibrate( @@ -288,3 +291,25 @@ def __enter__(self): def __exit__(self, *args, **kwargs): self.close() + + +@dataclass(kw_only=True) +class HardwareCameraCreatorConfig: + """One camera backend and the cameras it should open, resolved through `rcs.registry.CAMERAS`.""" + + camera_type_id: str + camera_cfgs: dict[str, BaseCameraConfig] + # Calibration strategy per camera name, passed to the camera set as is. None leaves every + # camera at identity extrinsics, which is what the sets default to themselves. + calibration: dict[str, CalibrationStrategy] | None = None + kwargs: dict[str, typing.Any] = field(default_factory=dict) + + +def create_hardware_camera_set( + camera_cfgs: dict[str, HardwareCameraCreatorConfig] | None, +) -> HardwareCameraSet | None: + """Build one `HardwareCameraSet` from per-backend configs, or None when there is nothing to open.""" + if camera_cfgs is None: + return None + cameras = [registry.CAMERAS.get(cfg.camera_type_id)(cfg) for cfg in camera_cfgs.values()] + return HardwareCameraSet(cameras) if cameras else None diff --git a/python/rcs/envs/configs.py b/python/rcs/envs/configs.py index da792e64..1ea12b20 100644 --- a/python/rcs/envs/configs.py +++ b/python/rcs/envs/configs.py @@ -187,7 +187,7 @@ def config(self) -> SimEnvCreatorConfig: lead_robot_name = self.lead_robot_name(cfg) robot_cfg = cfg.robot_cfgs[lead_robot_name] - robot_cfg.tcp_offset = GRIPPER_TCP_OFFSETS[rcs.common.GripperType("Robotiq2F85")] + robot_cfg.tcp_offset = GRIPPER_TCP_OFFSETS[rcs.common.GripperType.Robotiq2F85] robot_cfg.q_home = rcs.ROBOTS[RobotType.FR3].q_home.copy() assert cfg.gripper_cfgs is not None @@ -200,9 +200,9 @@ def config(self) -> SimEnvCreatorConfig: gripper_cfg.min_actuator_width = 255 gripper_cfg.max_joint_width = 0.005 gripper_cfg.min_joint_width = 1.0 - gripper_cfg.gripper_type = GripperType("Robotiq2F85") + gripper_cfg.gripper_type = GripperType.Robotiq2F85 - cfg.gripper_offsets = {lead_robot_name: GRIPPER_MOUNT_OFFSETS[rcs.common.GripperType("Robotiq2F85")]} + cfg.gripper_offsets = {lead_robot_name: GRIPPER_MOUNT_OFFSETS[rcs.common.GripperType.Robotiq2F85]} cfg.robot_frame_objects = { "right": { @@ -286,7 +286,7 @@ class EmptyWorldFR3Duo(SimEnvCreator): def config(self) -> SimEnvCreatorConfig: robot_cfg: SimRobotConfig[Literal[7]] = SimRobotConfig( - tcp_offset=GRIPPER_TCP_OFFSETS[rcs.common.GripperType("Robotiq2F85")], + tcp_offset=GRIPPER_TCP_OFFSETS[rcs.common.GripperType.Robotiq2F85], robot_type=RobotType.FR3, attachment_site=rcs.ROBOTS[RobotType.FR3].attachment_site, kinematic_model_path=rcs.ROBOTS[RobotType.FR3].mjcf_model_path, @@ -348,7 +348,7 @@ def config(self) -> SimEnvCreatorConfig: actuator="fingers_actuator", max_actuator_width=0, min_actuator_width=255, - gripper_type=GripperType("Robotiq2F85"), + gripper_type=GripperType.Robotiq2F85, ) gripper_cfg_right = copy.deepcopy(gripper_cfg) @@ -426,7 +426,7 @@ def config(self) -> SimEnvCreatorConfig: robot_name="right", ), } - gripper_offset = GRIPPER_MOUNT_OFFSETS[rcs.common.GripperType("Robotiq2F85")] + gripper_offset = GRIPPER_MOUNT_OFFSETS[rcs.common.GripperType.Robotiq2F85] return SimEnvCreatorConfig( robot_cfgs=robot_cfgs, sim_cfg=sim_cfg, @@ -455,12 +455,12 @@ def config(self) -> SimEnvCreatorConfig: class EmptyWorldUR5e(EmptyWorldFR3): def config(self) -> SimEnvCreatorConfig: - rt = RobotType("UR5e") + rt = RobotType.UR5e cfg = super().config() lead_robot_name = self.lead_robot_name(cfg) robot_cfg = cfg.robot_cfgs[lead_robot_name] - robot_cfg.tcp_offset = GRIPPER_TCP_OFFSETS[rcs.common.GripperType("Robotiq2F85")] + robot_cfg.tcp_offset = GRIPPER_TCP_OFFSETS[rcs.common.GripperType.Robotiq2F85] robot_cfg.attachment_site = rcs.ROBOTS[rt].attachment_site robot_cfg.kinematic_model_path = rcs.ROBOTS[rt].mjcf_model_path robot_cfg.arm_collision_geoms = [] @@ -489,7 +489,7 @@ def config(self) -> SimEnvCreatorConfig: gripper_cfg.min_actuator_width = 255 gripper_cfg.max_joint_width = 0.005 gripper_cfg.min_joint_width = 1.0 - gripper_cfg.gripper_type = GripperType("Robotiq2F85") + gripper_cfg.gripper_type = GripperType.Robotiq2F85 cfg.camera_cfgs = None cfg.camera_adds = None @@ -501,7 +501,7 @@ def config(self) -> SimEnvCreatorConfig: class EmptyWorldXArm7(EmptyWorldFR3): def config(self) -> SimEnvCreatorConfig: - rt = RobotType("XArm7") + rt = RobotType.XArm7 cfg = super().config() lead_robot_name = self.lead_robot_name(cfg) @@ -547,7 +547,7 @@ class EmptyWorldSO101(EmptyWorldFR3): gripper_prefix_template = EmptyWorldFR3.robot_prefix_template def config(self) -> SimEnvCreatorConfig: - rt = RobotType("SO101") + rt = RobotType.SO101 cfg = super().config() lead_robot_name = self.lead_robot_name(cfg) @@ -574,7 +574,7 @@ def config(self) -> SimEnvCreatorConfig: gripper_cfg.joints = ["6"] gripper_cfg.collision_geoms = [] gripper_cfg.collision_geoms_fingers = [] - gripper_cfg.gripper_type = GripperType("SO101") + gripper_cfg.gripper_type = GripperType.SO101 cfg.camera_cfgs = None cfg.camera_adds = None @@ -588,13 +588,13 @@ class EmptyWorldYam(EmptyWorldFR3): gripper_prefix_template = EmptyWorldFR3.robot_prefix_template def config(self) -> SimEnvCreatorConfig: - rt = RobotType("Yam") + rt = RobotType.Yam cfg = super().config() lead_robot_name = self.lead_robot_name(cfg) robot_cfg = cfg.robot_cfgs[lead_robot_name] robot_cfg.robot_type = rt - robot_cfg.tcp_offset = GRIPPER_TCP_OFFSETS[GripperType("Yam")] + robot_cfg.tcp_offset = GRIPPER_TCP_OFFSETS[GripperType.Yam] robot_cfg.attachment_site = rcs.ROBOTS[rt].attachment_site robot_cfg.kinematic_model_path = rcs.ROBOTS[rt].mjcf_model_path robot_cfg.arm_collision_geoms = [] @@ -607,7 +607,7 @@ def config(self) -> SimEnvCreatorConfig: assert cfg.gripper_cfgs is not None gripper_cfg = cfg.gripper_cfgs[lead_robot_name] - gripper_cfg.gripper_type = GripperType("Yam") + gripper_cfg.gripper_type = GripperType.Yam gripper_cfg.actuator = "gripper" # right_finger is driven by an equality constraint, so only the actuated finger is listed gripper_cfg.joints = ["left_finger"] diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py index 7a33c049..57b52025 100644 --- a/python/rcs/envs/utils.py +++ b/python/rcs/envs/utils.py @@ -1,9 +1,6 @@ import logging -from digit_interface import Digit -from rcs._core.common import BaseCameraConfig from rcs._core.sim import SimCameraConfig -from rcs.camera.digit_cam import DigitCam import rcs from rcs import sim @@ -16,22 +13,6 @@ def default_sim_tilburg_hand_cfg() -> sim.SimTilburgHandConfig: return sim.SimTilburgHandConfig() -def default_digit(name2id: dict[str, str] | None, stream_name: str = "QVGA") -> DigitCam | None: - if name2id is None: - return None - stream_dict = Digit.STREAMS[stream_name] - cameras = { - name: BaseCameraConfig( - identifier=identifier, - resolution_width=stream_dict["resolution"]["width"], - resolution_height=stream_dict["resolution"]["height"], - frame_rate=stream_dict["fps"]["30fps"], - ) - for name, identifier in name2id.items() - } - return DigitCam(cameras=cameras) - - def default_mujoco_cameraset_cfg() -> dict[str, SimCameraConfig]: # Kept for backwards compatibility in docs/comments while examples migrate. return { diff --git a/python/rcs/hand/__init__.py b/python/rcs/hand/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/python/rcs/registry.py b/python/rcs/registry.py new file mode 100644 index 00000000..045ad8e2 --- /dev/null +++ b/python/rcs/registry.py @@ -0,0 +1,70 @@ +"""Factory registries for pluggable hardware: cameras, grippers and hands. + +An extension that provides a backend declares a factory as an entry point in its own +``pyproject.toml`` and nothing else has to know about it:: + + [project.entry-points."rcs.cameras"] + realsense = "rcs_realsense.creators:create_camera_set" + +The entry point name is the type id that configs already carry (``camera_type_id``, +``GripperType.id``, ``HandType.id``). Entry points are plain package metadata, so listing the +available backends imports nothing, and a backend is imported only when its id is requested. A +backend that is not installed is simply absent from the metadata, which is how a missing +extension surfaces: as an unknown id, with the installed ids listed next to it. + +Two installed distributions may provide the same id, for example `rcs_fr3` and `rcs_panda` both +implement `FrankaHand` against different libfranka versions. Resolving that silently would pick an +arbitrary one, so it is an error instead; `register` settles it explicitly for the process. +""" + +from collections.abc import Callable +from importlib.metadata import entry_points +from typing import TYPE_CHECKING, Generic, TypeVar + +if TYPE_CHECKING: + from rcs._core.common import Gripper, Hand + from rcs.camera.hw import HardwareCamera + +T = TypeVar("T") + + +class Registry(Generic[T]): + """Maps type ids to factories, filled from an entry point group and by `register`.""" + + def __init__(self, group: str) -> None: + self._group = group + self._factories: dict[str, Callable[..., T]] = {} + + def register(self, type_id: str, factory: Callable[..., T]) -> None: + """Add a factory at runtime, for tests and for backends not installed as a distribution.""" + self._factories[type_id] = factory + + def available(self) -> set[str]: + """Ids that can be resolved, from registered factories and installed entry points. Imports nothing.""" + return set(self._factories) | {ep.name for ep in entry_points(group=self._group)} + + def get(self, type_id: str) -> Callable[..., T]: + """Return the factory for `type_id`, importing its backend on first use.""" + if type_id not in self._factories: + matches = list(entry_points(group=self._group, name=type_id)) + if len(matches) > 1: + dists = sorted({ep.dist.name for ep in matches if ep.dist is not None}) + msg = ( + f"{self._group} type {type_id!r} is provided by more than one installed package {dists}, " + f"call `register({type_id!r}, factory)` to choose one" + ) + raise ValueError(msg) + if matches: + self._factories[type_id] = matches[0].load() + try: + return self._factories[type_id] + except KeyError: + msg = f"Unknown {self._group} type {type_id!r}, available: {sorted(self.available())}" + raise ValueError(msg) from None + + +CAMERAS: "Registry[HardwareCamera]" = Registry("rcs.cameras") +GRIPPERS: "Registry[Gripper]" = Registry("rcs.grippers") +HANDS: "Registry[Hand]" = Registry("rcs.hands") + +__all__ = ["CAMERAS", "GRIPPERS", "HANDS", "Registry"] diff --git a/python/tests/test_kinematics.py b/python/tests/test_kinematics.py index b530303a..7cba6f5f 100644 --- a/python/tests/test_kinematics.py +++ b/python/tests/test_kinematics.py @@ -8,10 +8,10 @@ # Panda currently segfaults in the native binding here, and SO101 uses a dedicated IK implementation. PIN_SUPPORTED_ROBOTS = [ common.RobotType.FR3, - common.RobotType("XArm7"), - common.RobotType("UR5e"), - common.RobotType("SO101"), - common.RobotType("Yam"), + common.RobotType.XArm7, + common.RobotType.UR5e, + common.RobotType.SO101, + common.RobotType.Yam, ] diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 307137ff..3d2a5769 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -385,7 +385,11 @@ PYBIND11_MODULE(_core, m) { bind_type_class(common, "RobotType") .def_readonly_static("FR3", &rcs::common::RobotType::FR3) - .def_readonly_static("Panda", &rcs::common::RobotType::Panda); + .def_readonly_static("Panda", &rcs::common::RobotType::Panda) + .def_readonly_static("XArm7", &rcs::common::RobotType::XArm7) + .def_readonly_static("UR5e", &rcs::common::RobotType::UR5e) + .def_readonly_static("SO101", &rcs::common::RobotType::SO101) + .def_readonly_static("Yam", &rcs::common::RobotType::Yam); py::enum_(common, "RobotPlatform") .value("HARDWARE", rcs::common::RobotPlatform::HARDWARE) @@ -436,7 +440,14 @@ PYBIND11_MODULE(_core, m) { py::class_(common, "RobotState").def(py::init<>()); bind_type_class(common, "GripperType") - .def_readonly_static("FrankaHand", &rcs::common::GripperType::FrankaHand); + .def_readonly_static("FrankaHand", &rcs::common::GripperType::FrankaHand) + .def_readonly_static("PandaHand", &rcs::common::GripperType::PandaHand) + .def_readonly_static("Robotiq2F85", + &rcs::common::GripperType::Robotiq2F85) + .def_readonly_static("Robotiq2F85Digit", + &rcs::common::GripperType::Robotiq2F85Digit) + .def_readonly_static("SO101", &rcs::common::GripperType::SO101) + .def_readonly_static("Yam", &rcs::common::GripperType::Yam); rcs::common::GripperConfig default_gripper_config; py::class_(common, "GripperConfig") @@ -455,7 +466,18 @@ PYBIND11_MODULE(_core, m) { .value("LATERAL_GRASP", rcs::common::GraspType::LATERAL_GRASP) .value("TRIPOD_GRASP", rcs::common::GraspType::TRIPOD_GRASP) .export_values(); - py::class_(common, "HandConfig").def(py::init<>()); + bind_type_class(common, "HandType") + .def_readonly_static("TilburgHand", &rcs::common::HandType::TilburgHand); + + rcs::common::HandConfig default_hand_config; + py::class_(common, "HandConfig") + .def(py::init([](rcs::common::HandType hand_type) { + rcs::common::HandConfig config; + config.hand_type = hand_type; + return config; + }), + py::arg("hand_type") = default_hand_config.hand_type) + .def_readwrite("hand_type", &rcs::common::HandConfig::hand_type); py::class_(common, "HandState").def(py::init<>()); // holder type should be smart pointer as we deal with smart pointer