From 59883f1a7747830cfa02ed24b4b0a1c48bd2ce24 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Tue, 17 Jun 2025 11:23:37 +0200 Subject: [PATCH 01/32] refactor: moved realsense into its own module --- pyproject.toml | 7 +++-- python/rcs/cli/main.py | 29 +------------------ python/rcs/envs/creators.py | 6 +--- python/rcs/envs/utils.py | 11 ------- python/rcs_realsense/__init__.py | 0 python/rcs_realsense/__main__.py | 21 ++++++++++++++ .../realsense.py => rcs_realsense/camera.py} | 6 ++-- python/rcs_realsense/utils.py | 12 ++++++++ 8 files changed, 42 insertions(+), 50 deletions(-) create mode 100644 python/rcs_realsense/__init__.py create mode 100644 python/rcs_realsense/__main__.py rename python/{rcs/camera/realsense.py => rcs_realsense/camera.py} (98%) create mode 100644 python/rcs_realsense/utils.py diff --git a/pyproject.toml b/pyproject.toml index f41a8a55..b0390d41 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -15,12 +15,10 @@ dependencies = ["websockets>=11.0", "etils[epath]>=1.7.0", "glfw~=2.7", "pyopengl", - "pyrealsense2~=2.55.1", "pynput~=1.7.7", "pillow~=10.3", "python-dotenv==1.0.1", "opencv-python~=4.10.0.84", - "h5py~=3.13.0", # NOTE: when changing the mujoco version, also change it in requirements_dev.txt "mujoco==3.2.6", # NOTE: Same for pinocchio (pin) @@ -58,6 +56,9 @@ classifiers = [ so101 = [ "lerobot @ git+https://github.com/huggingface/lerobot.git", ] +realsense = [ + "pyrealsense2~=2.55.1", +] [tool.scikit-build] @@ -65,7 +66,7 @@ build.verbose = true build.targets = ["_core", "scenes"] logging.level = "INFO" build-dir = "build" -wheel.packages = ["python/rcs", "python/rcs_so101"] +wheel.packages = ["python/rcs", "python/rcs_so101", "python/rcs_realsense"] install.components = ["python_package"] [tool.ruff] diff --git a/python/rcs/cli/main.py b/python/rcs/cli/main.py index e73c8f76..ff949b24 100644 --- a/python/rcs/cli/main.py +++ b/python/rcs/cli/main.py @@ -2,42 +2,15 @@ from time import sleep from typing import Annotated -import pyrealsense2 as rs import rcs import rcs.control.fr3_desk import typer -from rcs.camera.realsense import RealSenseCameraSet from rcs.control.fr3_desk import load_creds_fr3_desk logger = logging.getLogger(__name__) # MAIN CLI -main_app = typer.Typer(help="CLI tool for the Robot Control Stack (RCS).") - - -# REALSENSE CLI -realsense_app = typer.Typer() -main_app.add_typer( - realsense_app, - name="realsense", - help=( - "Commands to access the intel realsense camera. " - "This includes tools such as reading out the serial numbers of connected devices." - ), -) - - -@realsense_app.command() -def serials(): - """Reads out the serial numbers of the connected realsense devices.""" - context = rs.context() - devices = RealSenseCameraSet.enumerate_connected_devices(context) - if len(devices) == 0: - logger.warning("No realsense devices connected.") - return - logger.info("Connected devices:") - for device in devices.values(): - logger.info(" %s: %s", device.product_line, device.serial) +main_app = typer.Typer(help="CLI tool for the Robot Control Stack (RCS).", name="rcs") # FR3 CLI diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index e1c984f4..35bb22df 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -35,7 +35,6 @@ default_fr3_hw_robot_cfg, default_fr3_sim_gripper_cfg, default_fr3_sim_robot_cfg, - default_realsense, ) from rcs.hand.tilburg_hand import TilburgHand @@ -169,12 +168,9 @@ def __call__( # type: ignore robot_ip: str, control_mode: ControlMode = ControlMode.CARTESIAN_TRPY, delta_actions: bool = True, - camera_config: dict[str, str] | None = None, + camera_set: HardwareCameraSet | None = None, gripper: bool = True, ) -> gym.Env: - real_sense = default_realsense(camera_config) - camera_set_impl_list = [] if real_sense is None else [real_sense] - camera_set = HardwareCameraSet(cameras=camera_set_impl_list) return RCSFR3EnvCreator()( ip=robot_ip, camera_set=camera_set, diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py index e650c0eb..3e214054 100644 --- a/python/rcs/envs/utils.py +++ b/python/rcs/envs/utils.py @@ -11,7 +11,6 @@ from rcs._core.hw import FR3Config, IKSolver from rcs._core.sim import CameraType, SimCameraConfig from rcs.camera.digit_cam import DigitCam -from rcs.camera.realsense import RealSenseCameraSet from rcs.hand.tilburg_hand import THConfig logger = logging.getLogger(__name__) @@ -52,16 +51,6 @@ def default_tilburg_hw_hand_cfg(file: str | PathLike | None = None) -> THConfig: return hand_cfg -def default_realsense(name2id: dict[str, str] | None) -> RealSenseCameraSet | None: - if name2id is None: - return None - cameras = { - name: BaseCameraConfig(identifier=id, resolution_width=1280, resolution_height=720, frame_rate=30) - for name, id in name2id.items() - } - return RealSenseCameraSet(cameras=cameras) - - def default_fr3_sim_gripper_cfg(idx: str = "0") -> sim.SimGripperConfig: cfg = sim.SimGripperConfig() cfg.add_id(idx) diff --git a/python/rcs_realsense/__init__.py b/python/rcs_realsense/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/python/rcs_realsense/__main__.py b/python/rcs_realsense/__main__.py new file mode 100644 index 00000000..5dc902e2 --- /dev/null +++ b/python/rcs_realsense/__main__.py @@ -0,0 +1,21 @@ +import logging + +import pyrealsense2 as rs +import typer +from rcs_realsense.camera import RealSenseCameraSet + +logger = logging.getLogger(__name__) +realsense_app = typer.Typer(help="CLI tool for the intel realsense module of rcs.", name="rcs.realsense") + + +@realsense_app.command() +def serials(): + """Reads out the serial numbers of the connected realsense devices.""" + context = rs.context() + devices = RealSenseCameraSet.enumerate_connected_devices(context) + if len(devices) == 0: + logger.warning("No realsense devices connected.") + return + logger.info("Connected devices:") + for device in devices.values(): + logger.info(" %s: %s", device.product_line, device.serial) diff --git a/python/rcs/camera/realsense.py b/python/rcs_realsense/camera.py similarity index 98% rename from python/rcs/camera/realsense.py rename to python/rcs_realsense/camera.py index 28b89a1b..f0ddb65c 100644 --- a/python/rcs/camera/realsense.py +++ b/python/rcs_realsense/camera.py @@ -3,7 +3,7 @@ import numpy as np import pyrealsense2 as rs -from rcs._core.common import BaseCameraConfig +from rcs import common from rcs.camera.hw import HardwareCamera from rcs.camera.interface import CameraFrame, DataFrame, Frame, IMUFrame @@ -26,7 +26,7 @@ class RealSenseCameraSet(HardwareCamera): def __init__( self, - cameras: dict[str, BaseCameraConfig], + cameras: dict[str, common.BaseCameraConfig], enable_ir_emitter: bool = False, enable_ir: bool = False, laser_power: int = 330, @@ -96,7 +96,7 @@ def camera_names(self) -> list[str]: """Returns the names of the cameras in this set.""" return self._camera_names - def config(self, camera_name) -> BaseCameraConfig: + def config(self, camera_name) -> common.BaseCameraConfig: return self.cameras[camera_name] def open(self): diff --git a/python/rcs_realsense/utils.py b/python/rcs_realsense/utils.py new file mode 100644 index 00000000..645f3595 --- /dev/null +++ b/python/rcs_realsense/utils.py @@ -0,0 +1,12 @@ +from rcs import common +from rcs_realsense.camera import RealSenseCameraSet + + +def default_realsense(name2id: dict[str, str] | None) -> RealSenseCameraSet | None: + if name2id is None: + return None + cameras = { + name: common.BaseCameraConfig(identifier=id, resolution_width=1280, resolution_height=720, frame_rate=30) + for name, id in name2id.items() + } + return RealSenseCameraSet(cameras=cameras) From 0a2a3c70c3bcc25b5451026e9f4b9a305c37296a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Tue, 17 Jun 2025 12:35:37 +0200 Subject: [PATCH 02/32] remove kinect support --- install_kinect_k4a.sh | 58 ------------------------------- python/rcs/camera/kinect.py | 68 ------------------------------------- 2 files changed, 126 deletions(-) delete mode 100755 install_kinect_k4a.sh delete mode 100644 python/rcs/camera/kinect.py diff --git a/install_kinect_k4a.sh b/install_kinect_k4a.sh deleted file mode 100755 index 224a2957..00000000 --- a/install_kinect_k4a.sh +++ /dev/null @@ -1,58 +0,0 @@ -#!/bin/bash - -# This installation script follows the instruction from (only works under debian based systems) -# https://github.com/microsoft/Azure-Kinect-Sensor-SDK/blob/develop/src/python/k4a/docs/building.md -# Dependencies for this script: git, wget, python3 - -# force input to continue -echo "Ensure that you have sourced a virtual python environment before continuing" -read -p "Press enter to continue" -echo "continuing..." - -# store current path -cwd=$(pwd) - -# create tmp folder -mkdir /tmp/kinect_k4a - -# cd into tmp folder -pushd /tmp/kinect_k4a - -# clone repo -git clone https://github.com/microsoft/Azure-Kinect-Sensor-SDK.git - -# download binaries -wget https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4a1.4/libk4a1.4_1.4.1_amd64.deb - -# unzip binaries, this only works under debian based systems -dpkg -x libk4a1.4_1.4.1_amd64.deb extracted - - -# copy files -cp -r extracted/usr/lib/x86_64-linux-gnu/libk4a.* Azure-Kinect-Sensor-SDK/src/python/k4a/src/k4a/_libs -cp -r extracted/usr/lib/x86_64-linux-gnu/libk4a1.4/libdepthengine.* Azure-Kinect-Sensor-SDK/src/python/k4a/src/k4a/_libs - -pushd Azure-Kinect-Sensor-SDK/src/python/k4a - -pip install build - -python -m build --wheel --outdir dist/ - -mkdir $cwd/dist - -# copy wheel file -cp dist/*.whl $cwd/dist - - -# ask user to install wheel package, has to input y or n -echo "Do you want to install the wheel package? (y/n)" -read install_wheel -if [ $install_wheel == "y" ]; then - echo "Installing wheel package" - pip install . -else - echo "Wheel package not installed but can be found in $cwd/dist/" -fi - -popd -popd \ No newline at end of file diff --git a/python/rcs/camera/kinect.py b/python/rcs/camera/kinect.py deleted file mode 100644 index ad213be4..00000000 --- a/python/rcs/camera/kinect.py +++ /dev/null @@ -1,68 +0,0 @@ -import k4a -import numpy as np -from rcs._core.common import BaseCameraConfig -from rcs.camera.hw import HardwareCamera -from rcs.camera.interface import CameraFrame, DataFrame, Frame, IMUFrame - - -class KinectCamera(HardwareCamera): - # ATTENTION: this code is untested - def __init__(self, cameras: dict[str, BaseCameraConfig], include_imu: bool = False, timeout_ms: int = 2000) -> None: - self.include_imu = include_imu - self.timeout_ms = timeout_ms - self.cameras = cameras - assert len(self.cameras) == 1, "Kinect only supports one camera." - self._camera_names = list(self.cameras.keys()) - - @property - def camera_names(self) -> list[str]: - """Returns the names of the cameras in this set.""" - return self._camera_names - - def open(self): - self._device = k4a.Device.open() - device_config = k4a.DEVICE_CONFIG_BGRA32_1080P_NFOV_2X2BINNED_FPS15 - if self._device is None: - msg = "Failed to open device." - raise OSError(msg) - if self._device.start_cameras(device_config) != k4a.EStatus.SUCCEEDED: - msg = "Failed to start cameras." - raise OSError(msg) - if self.include_imu and self._device.start_imu() != k4a.EWaitStatus.SUCCEEDED: - msg = "Failed to start IMU." - raise OSError(msg) - - def poll_frame(self, camera_name: str = "") -> Frame: - assert camera_name == "kinect", "Kinect code only supports one camera." - capture = self._device.get_capture(self.timeout_ms) - if capture is None: - msg = "Timeout error while waiting for camera sample." - raise OSError(msg) - assert capture.color is not None, "Color frame is None." - camera_frame = CameraFrame( - color=capture.color.data, ir=capture.ir.data, depth=capture.depth.data, temperature=capture.temperature - ) - imu_frame = None - if self.include_imu: - imu_sample = self._device.get_imu_sample(self.timeout_ms) - if imu_sample is None: - msg = "Timeout error while waiting for IMU sample." - raise OSError(msg) - imu_sample_np = np.array((imu_sample.acc_sample.x, imu_sample.acc_sample.y, imu_sample.acc_sample.z)) - gyro_sample_np = np.array((imu_sample.gyro_sample.x, imu_sample.gyro_sample.y, imu_sample.gyro_sample.z)) - imu_frame = IMUFrame( - accel=DataFrame(data=imu_sample_np, timestamp=float(imu_sample.acc_sample_usec)), - gyro=DataFrame(data=gyro_sample_np, timestamp=float(imu_sample.gyro_sample_usec)), - temperature=imu_sample.temperature, - ) - return Frame(camera=camera_frame, imu=imu_frame, avg_timestamp=None) - - def config(self, camera_name) -> BaseCameraConfig: - return self.cameras[camera_name] - - def close(self): - if self._device is not None: - self._device.stop_cameras() - if self.include_imu: - self._device.stop_imu() - self._device = None From 8537649096acc3c8072f5353b1e793a1ef8f97ff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Tue, 17 Jun 2025 13:04:07 +0200 Subject: [PATCH 03/32] refactor: fr3 module with desk in it --- README.md | 7 ++-- pyproject.toml | 2 +- python/examples/env_cartesian_control.py | 10 ++--- .../examples/env_cartesian_control_digit.py | 17 +++++---- .../examples/env_cartesian_control_tilburg.py | 10 ++--- python/examples/env_joint_control.py | 10 ++--- python/examples/fr3.py | 10 ++--- python/rcs/__main__.py | 4 -- python/rcs/cli/__init__.py | 0 python/rcs/control/__init__.py | 0 python/rcs_fr3/__init__.py | 5 +++ .../{rcs/cli/main.py => rcs_fr3/__main__.py} | 38 ++++++++----------- .../control/fr3_desk.py => rcs_fr3/desk.py} | 0 python/rcs_realsense/__main__.py | 4 ++ 14 files changed, 59 insertions(+), 58 deletions(-) delete mode 100644 python/rcs/__main__.py delete mode 100644 python/rcs/cli/__init__.py delete mode 100644 python/rcs/control/__init__.py create mode 100644 python/rcs_fr3/__init__.py rename python/{rcs/cli/main.py => rcs_fr3/__main__.py} (72%) rename python/{rcs/control/fr3_desk.py => rcs_fr3/desk.py} (100%) diff --git a/README.md b/README.md index 97b7b83c..2a3c7ec8 100644 --- a/README.md +++ b/README.md @@ -42,10 +42,11 @@ DESK_PASSWORD=... ``` ### Command Line Interface -The package includes a command line interface which define useful commands to handle the FR3 robot without the need to use the Desk Website. -To list all available subcommands use: +Some modules include command line interfaces, e.g. rcs_fr3 defines useful commands to handle the FR3 robot without the need to use the Desk Website. +You can see the available subcommands as follows: ```shell -python -m rcs --help +python -m rcs.fr3 --help +python -m rcs.realsense --help ``` ## Development diff --git a/pyproject.toml b/pyproject.toml index b0390d41..1f0c4e2a 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -66,7 +66,7 @@ build.verbose = true build.targets = ["_core", "scenes"] logging.level = "INFO" build-dir = "build" -wheel.packages = ["python/rcs", "python/rcs_so101", "python/rcs_realsense"] +wheel.packages = ["python/rcs", "python/rcs_fr3", "python/rcs_so101", "python/rcs_realsense"] install.components = ["python_package"] [tool.ruff] diff --git a/python/examples/env_cartesian_control.py b/python/examples/env_cartesian_control.py index 62f374e8..5fcba82f 100644 --- a/python/examples/env_cartesian_control.py +++ b/python/examples/env_cartesian_control.py @@ -1,7 +1,6 @@ import logging from rcs._core.common import RobotPlatform -from rcs.control.fr3_desk import FCI, ContextManager, Desk, load_creds_fr3_desk from rcs.envs.base import ControlMode, RelativeTo from rcs.envs.creators import FR3SimEnvCreator, RCSFR3EnvCreator from rcs.envs.utils import ( @@ -11,6 +10,7 @@ default_fr3_sim_robot_cfg, default_mujoco_cameraset_cfg, ) +from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_fr3_desk logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -26,19 +26,19 @@ When you use a real FR3 you first need to unlock its joints using the following cli script: -python -m rcs fr3 unlock +python -m rcs.fr3 unlock or put it into guiding mode using: -python -m rcs fr3 guiding-mode +python -m rcs.fr3 guiding-mode When you are done you lock it again using: -python -m rcs fr3 lock +python -m rcs.fr3 lock or even shut it down using: -python -m rcs fr3 shutdown +python -m rcs.fr3 shutdown """ diff --git a/python/examples/env_cartesian_control_digit.py b/python/examples/env_cartesian_control_digit.py index 776c8fd7..ad078d75 100644 --- a/python/examples/env_cartesian_control_digit.py +++ b/python/examples/env_cartesian_control_digit.py @@ -1,7 +1,8 @@ import logging +import typing from rcs._core.common import RobotPlatform -from rcs.control.fr3_desk import FCI, ContextManager, Desk, load_creds_fr3_desk +from rcs.camera.hw import HardwareCamera, HardwareCameraSet from rcs.envs.base import ControlMode, RelativeTo from rcs.envs.creators import FR3SimEnvCreator, RCSFR3EnvCreator from rcs.envs.utils import ( @@ -12,8 +13,7 @@ default_fr3_sim_robot_cfg, default_mujoco_cameraset_cfg, ) - -from python.rcs.camera.hw import HardwareCameraSet +from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_fr3_desk logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -29,19 +29,19 @@ When you use a real FR3 you first need to unlock its joints using the following cli script: -python -m rcs fr3 unlock +python -m rcs.fr3 unlock or put it into guiding mode using: -python -m rcs fr3 guiding-mode +python -m rcs.fr3 guiding-mode When you are done you lock it again using: -python -m rcs fr3 lock +python -m rcs.fr3 lock or even shut it down using: -python -m rcs fr3 shutdown +python -m rcs.fr3 shutdown """ @@ -54,12 +54,13 @@ def main(): resource_manger = ContextManager() with resource_manger: if ROBOT_INSTANCE == RobotPlatform.HARDWARE: + camera_set = typing.cast(HardwareCamera, default_digit({"digit_0": "D21182"})) env_rel = RCSFR3EnvCreator()( ip=ROBOT_IP, control_mode=ControlMode.CARTESIAN_TQuat, robot_cfg=default_fr3_hw_robot_cfg(), collision_guard="lab", - camera_set=HardwareCameraSet(default_digit({"digit_0": "D21182"})), + camera_set=HardwareCameraSet([camera_set]), gripper_cfg=default_fr3_hw_gripper_cfg(), max_relative_movement=0.5, relative_to=RelativeTo.LAST_STEP, diff --git a/python/examples/env_cartesian_control_tilburg.py b/python/examples/env_cartesian_control_tilburg.py index 4d97b95b..2dc84f09 100644 --- a/python/examples/env_cartesian_control_tilburg.py +++ b/python/examples/env_cartesian_control_tilburg.py @@ -1,7 +1,6 @@ import logging from rcs._core.common import RobotPlatform -from rcs.control.fr3_desk import FCI, ContextManager, Desk, load_creds_fr3_desk from rcs.envs.base import ControlMode, RelativeTo from rcs.envs.creators import FR3SimEnvCreator, RCSFR3EnvCreator from rcs.envs.utils import ( @@ -11,6 +10,7 @@ default_mujoco_cameraset_cfg, default_tilburg_hw_hand_cfg, ) +from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_fr3_desk logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -26,19 +26,19 @@ When you use a real FR3 you first need to unlock its joints using the following cli script: -python -m rcs fr3 unlock +python -m rcs.fr3 unlock or put it into guiding mode using: -python -m rcs fr3 guiding-mode +python -m rcs.fr3 guiding-mode When you are done you lock it again using: -python -m rcs fr3 lock +python -m rcs.fr3 lock or even shut it down using: -python -m rcs fr3 shutdown +python -m rcs.fr3 shutdown """ diff --git a/python/examples/env_joint_control.py b/python/examples/env_joint_control.py index 4ee966a8..811bc571 100644 --- a/python/examples/env_joint_control.py +++ b/python/examples/env_joint_control.py @@ -2,7 +2,6 @@ import numpy as np from rcs._core.common import RobotPlatform -from rcs.control.fr3_desk import FCI, ContextManager, Desk, load_creds_fr3_desk from rcs.envs.base import ControlMode, RelativeTo from rcs.envs.creators import FR3SimEnvCreator, RCSFR3EnvCreator from rcs.envs.utils import ( @@ -12,6 +11,7 @@ default_fr3_sim_robot_cfg, default_mujoco_cameraset_cfg, ) +from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_fr3_desk logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -27,19 +27,19 @@ When you use a real FR3 you first need to unlock its joints using the following cli script: -python -m rcs fr3 unlock +python -m rcs.fr3 unlock or put it into guiding mode using: -python -m rcs fr3 guiding-mode +python -m rcs.fr3 guiding-mode When you are done you lock it again using: -python -m rcs fr3 lock +python -m rcs.fr3 lock or even shut it down using: -python -m rcs fr3 shutdown +python -m rcs.fr3 shutdown """ diff --git a/python/examples/fr3.py b/python/examples/fr3.py index 6652c95f..7a22d014 100644 --- a/python/examples/fr3.py +++ b/python/examples/fr3.py @@ -7,7 +7,7 @@ from rcs._core.hw import FR3Config, IKSolver from rcs._core.sim import CameraType from rcs.camera.sim import SimCameraConfig, SimCameraSet -from rcs.control.fr3_desk import FCI, ContextManager, Desk, load_creds_fr3_desk +from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_fr3_desk ROBOT_IP = "192.168.101.1" ROBOT_INSTANCE = RobotPlatform.SIMULATION @@ -30,19 +30,19 @@ When you use a real FR3 you first need to unlock its joints using the following cli script: -python -m rcs fr3 unlock +python -m rcs.fr3 unlock or put it into guiding mode using: -python -m rcs fr3 guiding-mode +python -m rcs.fr3 guiding-mode When you are done you lock it again using: -python -m rcs fr3 lock +python -m rcs.fr3 lock or even shut it down using: -python -m rcs fr3 shutdown +python -m rcs.fr3 shutdown """ diff --git a/python/rcs/__main__.py b/python/rcs/__main__.py deleted file mode 100644 index a71d4e71..00000000 --- a/python/rcs/__main__.py +++ /dev/null @@ -1,4 +0,0 @@ -from rcs.cli.main import main - -if __name__ == "__main__": - main() diff --git a/python/rcs/cli/__init__.py b/python/rcs/cli/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/python/rcs/control/__init__.py b/python/rcs/control/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/python/rcs_fr3/__init__.py b/python/rcs_fr3/__init__.py new file mode 100644 index 00000000..35fa0538 --- /dev/null +++ b/python/rcs_fr3/__init__.py @@ -0,0 +1,5 @@ +from rcs_fr3 import desk + +__all__ = [ + "desk", +] diff --git a/python/rcs/cli/main.py b/python/rcs_fr3/__main__.py similarity index 72% rename from python/rcs/cli/main.py rename to python/rcs_fr3/__main__.py index ff949b24..be0d8462 100644 --- a/python/rcs/cli/main.py +++ b/python/rcs_fr3/__main__.py @@ -2,24 +2,18 @@ from time import sleep from typing import Annotated -import rcs -import rcs.control.fr3_desk +import rcs_fr3 import typer -from rcs.control.fr3_desk import load_creds_fr3_desk +from rcs_fr3.desk import load_creds_fr3_desk logger = logging.getLogger(__name__) -# MAIN CLI -main_app = typer.Typer(help="CLI tool for the Robot Control Stack (RCS).", name="rcs") - # FR3 CLI -fr3_app = typer.Typer() -main_app.add_typer( - fr3_app, - name="fr3", +fr3_app = typer.Typer( + name="rcs.fr3", help=( - "Commands to control a Franka Research 3. " + "Commands to control a Franka Research 3 in RCS. " "This includes tools that you would usually do with Franka's Desk interface." ), ) @@ -33,7 +27,7 @@ def home( ): """Moves the FR3 to home position""" user, pw = load_creds_fr3_desk() - rcs.control.fr3_desk.home(ip, user, pw, shut, unlock) + rcs_fr3.desk.home(ip, user, pw, shut, unlock) @fr3_app.command() @@ -43,7 +37,7 @@ def info( ): """Prints info about the robots current joint position and end effector pose, optionally also the gripper.""" user, pw = load_creds_fr3_desk() - rcs.control.fr3_desk.info(ip, user, pw, include_gripper) + rcs_fr3.desk.info(ip, user, pw, include_gripper) @fr3_app.command() @@ -52,7 +46,7 @@ def lock( ): """Locks the robot.""" user, pw = load_creds_fr3_desk() - rcs.control.fr3_desk.lock(ip, user, pw) + rcs_fr3.desk.lock(ip, user, pw) @fr3_app.command() @@ -61,8 +55,8 @@ def unlock( ): """Prepares the robot by unlocking the joints and putting the robot into the FCI mode.""" user, pw = load_creds_fr3_desk() - rcs.control.fr3_desk.unlock(ip, user, pw) - with rcs.control.fr3_desk.Desk(ip, user, pw) as d: + rcs_fr3.desk.unlock(ip, user, pw) + with rcs_fr3.desk.Desk(ip, user, pw) as d: d.activate_fci() @@ -75,12 +69,12 @@ def fci( """Puts the robot into FCI mode, optionally unlocks the robot. Waits for ctrl+c to exit.""" user, pw = load_creds_fr3_desk() try: - with rcs.control.fr3_desk.FCI(rcs.control.fr3_desk.Desk(ip, user, pw), unlock=unlock, lock_when_done=False): + with rcs_fr3.desk.FCI(rcs_fr3.desk.Desk(ip, user, pw), unlock=unlock, lock_when_done=False): while True: sleep(1) except KeyboardInterrupt: if shutdown: - rcs.control.fr3_desk.shutdown(ip, user, pw) + rcs_fr3.desk.shutdown(ip, user, pw) @fr3_app.command() @@ -91,7 +85,7 @@ def guiding_mode( ): """Enables or disables guiding mode.""" user, pw = load_creds_fr3_desk() - rcs.control.fr3_desk.guiding_mode(ip, user, pw, disable, unlock) + rcs_fr3.desk.guiding_mode(ip, user, pw, disable, unlock) @fr3_app.command() @@ -100,8 +94,8 @@ def shutdown( ): """Shuts the robot down""" user, pw = load_creds_fr3_desk() - rcs.control.fr3_desk.shutdown(ip, user, pw) + rcs_fr3.desk.shutdown(ip, user, pw) -def main(): - main_app() +if __name__ == "__main__": + fr3_app() diff --git a/python/rcs/control/fr3_desk.py b/python/rcs_fr3/desk.py similarity index 100% rename from python/rcs/control/fr3_desk.py rename to python/rcs_fr3/desk.py diff --git a/python/rcs_realsense/__main__.py b/python/rcs_realsense/__main__.py index 5dc902e2..b803d5d0 100644 --- a/python/rcs_realsense/__main__.py +++ b/python/rcs_realsense/__main__.py @@ -19,3 +19,7 @@ def serials(): logger.info("Connected devices:") for device in devices.values(): logger.info(" %s: %s", device.product_line, device.serial) + + +if __name__ == "__main__": + realsense_app() From 635dc99d5bcc513833ce071adf9f145ba86f434e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Tue, 17 Jun 2025 13:09:32 +0200 Subject: [PATCH 04/32] refactor(fr3): moved envs in fr3 module --- python/rcs/envs/creators.py | 2 +- python/rcs_fr3/__init__.py | 3 ++- python/{rcs/envs/hw.py => rcs_fr3/envs.py} | 0 3 files changed, 3 insertions(+), 2 deletions(-) rename python/{rcs/envs/hw.py => rcs_fr3/envs.py} (100%) diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index 35bb22df..7c926ce1 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -21,7 +21,7 @@ RelativeTo, RobotEnv, ) -from rcs.envs.hw import FR3HW +from rcs_fr3.envs import FR3HW from rcs.envs.sim import ( CollisionGuard, GripperWrapperSim, diff --git a/python/rcs_fr3/__init__.py b/python/rcs_fr3/__init__.py index 35fa0538..cd4f6f15 100644 --- a/python/rcs_fr3/__init__.py +++ b/python/rcs_fr3/__init__.py @@ -1,5 +1,6 @@ -from rcs_fr3 import desk +from rcs_fr3 import desk, envs __all__ = [ "desk", + "envs", ] diff --git a/python/rcs/envs/hw.py b/python/rcs_fr3/envs.py similarity index 100% rename from python/rcs/envs/hw.py rename to python/rcs_fr3/envs.py From 649095fbb31f670477774341972231b533386219 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Tue, 17 Jun 2025 13:35:25 +0200 Subject: [PATCH 05/32] refactor(fr3): moved env creators and utils for fr3 module --- python/examples/env_cartesian_control.py | 16 +- .../examples/env_cartesian_control_digit.py | 16 +- .../examples/env_cartesian_control_tilburg.py | 15 +- python/examples/env_joint_control.py | 16 +- python/rcs/__init__.py | 6 +- python/rcs/envs/creators.py | 159 +---------------- python/rcs/envs/sim.py | 6 +- python/rcs/envs/utils.py | 24 +-- python/rcs_fr3/creators.py | 163 ++++++++++++++++++ python/rcs_fr3/desk.py | 2 +- python/rcs_fr3/utils.py | 19 ++ python/tests/test_sim_envs.py | 50 +++--- 12 files changed, 254 insertions(+), 238 deletions(-) create mode 100644 python/rcs_fr3/creators.py create mode 100644 python/rcs_fr3/utils.py diff --git a/python/examples/env_cartesian_control.py b/python/examples/env_cartesian_control.py index 5fcba82f..83809d1a 100644 --- a/python/examples/env_cartesian_control.py +++ b/python/examples/env_cartesian_control.py @@ -2,15 +2,15 @@ from rcs._core.common import RobotPlatform from rcs.envs.base import ControlMode, RelativeTo -from rcs.envs.creators import FR3SimEnvCreator, RCSFR3EnvCreator +from rcs.envs.creators import SimEnvCreator from rcs.envs.utils import ( - default_fr3_hw_gripper_cfg, - default_fr3_hw_robot_cfg, - default_fr3_sim_gripper_cfg, - default_fr3_sim_robot_cfg, default_mujoco_cameraset_cfg, + default_sim_gripper_cfg, + default_sim_robot_cfg, ) +from rcs_fr3.creators import RCSFR3EnvCreator from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_fr3_desk +from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -62,11 +62,11 @@ def main(): relative_to=RelativeTo.LAST_STEP, ) else: - env_rel = FR3SimEnvCreator()( + env_rel = SimEnvCreator()( control_mode=ControlMode.CARTESIAN_TQuat, - robot_cfg=default_fr3_sim_robot_cfg(), + robot_cfg=default_sim_robot_cfg(), collision_guard=False, - gripper_cfg=default_fr3_sim_gripper_cfg(), + gripper_cfg=default_sim_gripper_cfg(), cameras=default_mujoco_cameraset_cfg(), max_relative_movement=0.5, relative_to=RelativeTo.LAST_STEP, diff --git a/python/examples/env_cartesian_control_digit.py b/python/examples/env_cartesian_control_digit.py index ad078d75..e624b121 100644 --- a/python/examples/env_cartesian_control_digit.py +++ b/python/examples/env_cartesian_control_digit.py @@ -4,16 +4,16 @@ from rcs._core.common import RobotPlatform from rcs.camera.hw import HardwareCamera, HardwareCameraSet from rcs.envs.base import ControlMode, RelativeTo -from rcs.envs.creators import FR3SimEnvCreator, RCSFR3EnvCreator +from rcs.envs.creators import SimEnvCreator from rcs.envs.utils import ( default_digit, - default_fr3_hw_gripper_cfg, - default_fr3_hw_robot_cfg, - default_fr3_sim_gripper_cfg, - default_fr3_sim_robot_cfg, default_mujoco_cameraset_cfg, + default_sim_gripper_cfg, + default_sim_robot_cfg, ) +from rcs_fr3.creators import RCSFR3EnvCreator from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_fr3_desk +from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -66,11 +66,11 @@ def main(): relative_to=RelativeTo.LAST_STEP, ) else: - env_rel = FR3SimEnvCreator()( + env_rel = SimEnvCreator()( control_mode=ControlMode.CARTESIAN_TQuat, - robot_cfg=default_fr3_sim_robot_cfg(), + robot_cfg=default_sim_robot_cfg(), collision_guard=False, - gripper_cfg=default_fr3_sim_gripper_cfg(), + gripper_cfg=default_sim_gripper_cfg(), cameras=default_mujoco_cameraset_cfg(), max_relative_movement=0.5, relative_to=RelativeTo.LAST_STEP, diff --git a/python/examples/env_cartesian_control_tilburg.py b/python/examples/env_cartesian_control_tilburg.py index 2dc84f09..4aac2359 100644 --- a/python/examples/env_cartesian_control_tilburg.py +++ b/python/examples/env_cartesian_control_tilburg.py @@ -2,15 +2,16 @@ from rcs._core.common import RobotPlatform from rcs.envs.base import ControlMode, RelativeTo -from rcs.envs.creators import FR3SimEnvCreator, RCSFR3EnvCreator +from rcs.envs.creators import SimEnvCreator from rcs.envs.utils import ( - default_fr3_hw_robot_cfg, - default_fr3_sim_gripper_cfg, - default_fr3_sim_robot_cfg, default_mujoco_cameraset_cfg, + default_sim_gripper_cfg, + default_sim_robot_cfg, default_tilburg_hw_hand_cfg, ) +from rcs_fr3.creators import RCSFR3EnvCreator from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_fr3_desk +from rcs_fr3.utils import default_fr3_hw_robot_cfg logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -61,11 +62,11 @@ def main(): relative_to=RelativeTo.LAST_STEP, ) else: - env_rel = FR3SimEnvCreator()( + env_rel = SimEnvCreator()( control_mode=ControlMode.CARTESIAN_TQuat, - robot_cfg=default_fr3_sim_robot_cfg(), + robot_cfg=default_sim_robot_cfg(), collision_guard=False, - gripper_cfg=default_fr3_sim_gripper_cfg(), + gripper_cfg=default_sim_gripper_cfg(), cameras=default_mujoco_cameraset_cfg(), max_relative_movement=0.5, relative_to=RelativeTo.LAST_STEP, diff --git a/python/examples/env_joint_control.py b/python/examples/env_joint_control.py index 811bc571..7bb45e10 100644 --- a/python/examples/env_joint_control.py +++ b/python/examples/env_joint_control.py @@ -3,15 +3,15 @@ import numpy as np from rcs._core.common import RobotPlatform from rcs.envs.base import ControlMode, RelativeTo -from rcs.envs.creators import FR3SimEnvCreator, RCSFR3EnvCreator +from rcs.envs.creators import SimEnvCreator from rcs.envs.utils import ( - default_fr3_hw_gripper_cfg, - default_fr3_hw_robot_cfg, - default_fr3_sim_gripper_cfg, - default_fr3_sim_robot_cfg, default_mujoco_cameraset_cfg, + default_sim_gripper_cfg, + default_sim_robot_cfg, ) +from rcs_fr3.creators import RCSFR3EnvCreator from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_fr3_desk +from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -63,11 +63,11 @@ def main(): relative_to=RelativeTo.LAST_STEP, ) else: - env_rel = FR3SimEnvCreator()( + env_rel = SimEnvCreator()( control_mode=ControlMode.JOINTS, collision_guard=False, - robot_cfg=default_fr3_sim_robot_cfg(), - gripper_cfg=default_fr3_sim_gripper_cfg(), + robot_cfg=default_sim_robot_cfg(), + gripper_cfg=default_sim_gripper_cfg(), cameras=default_mujoco_cameraset_cfg(), max_relative_movement=np.deg2rad(5), relative_to=RelativeTo.LAST_STEP, diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py index 856cb4d7..de6ebb1d 100644 --- a/python/rcs/__init__.py +++ b/python/rcs/__init__.py @@ -4,7 +4,7 @@ import site from gymnasium import register -from rcs import camera, control, envs, hand, sim +from rcs import camera, envs, hand, sim from rcs._core import __version__, common, hw from rcs.envs.creators import FR3SimplePickUpSimEnvCreator @@ -18,7 +18,7 @@ } # make submodules available -__all__ = ["__doc__", "__version__", "common", "hw", "sim", "camera", "scenes", "control", "envs", "hand"] +__all__ = ["__doc__", "__version__", "common", "hw", "sim", "camera", "scenes", "envs", "hand"] # register gymnasium environments register( @@ -30,5 +30,5 @@ # cf. https://pybind11.readthedocs.io/en/stable/advanced/classes.html#deepcopy-support # register( # id="rcs/FR3SimEnv-v0", -# entry_point=FR3SimEnvCreator(), +# entry_point=SimEnvCreator(), # ) diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index 7c926ce1..37c76be1 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -5,23 +5,18 @@ import gymnasium as gym import numpy as np import rcs -import rcs.hand.tilburg_hand from gymnasium.envs.registration import EnvCreator from rcs import sim from rcs._core.sim import CameraType -from rcs.camera.hw import HardwareCameraSet from rcs.camera.sim import SimCameraConfig, SimCameraSet from rcs.envs.base import ( CameraSetWrapper, ControlMode, GripperWrapper, - HandWrapper, - MultiRobotWrapper, RelativeActionSpace, RelativeTo, RobotEnv, ) -from rcs_fr3.envs import FR3HW from rcs.envs.sim import ( CollisionGuard, GripperWrapperSim, @@ -30,13 +25,8 @@ RobotSimWrapper, SimWrapper, ) -from rcs.envs.utils import ( - default_fr3_hw_gripper_cfg, - default_fr3_hw_robot_cfg, - default_fr3_sim_gripper_cfg, - default_fr3_sim_robot_cfg, -) -from rcs.hand.tilburg_hand import TilburgHand +from rcs.envs.space_utils import VecType +from rcs.envs.utils import default_sim_gripper_cfg, default_sim_robot_cfg logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -46,144 +36,7 @@ class RCSHardwareEnvCreator(EnvCreator): pass -class RCSFR3EnvCreator(RCSHardwareEnvCreator): - def __call__( # type: ignore - self, - ip: str, - control_mode: ControlMode, - robot_cfg: rcs.hw.FR3Config, - collision_guard: str | PathLike | None = None, - gripper_cfg: rcs.hw.FHConfig | rcs.hand.tilburg_hand.THConfig | None = None, - camera_set: HardwareCameraSet | None = None, - max_relative_movement: float | tuple[float, float] | None = None, - relative_to: RelativeTo = RelativeTo.LAST_STEP, - urdf_path: str | PathLike | None = None, - ) -> gym.Env: - """ - Creates a hardware environment for the FR3 robot. - - Args: - ip (str): IP address of the robot. - control_mode (ControlMode): Control mode for the robot. - robot_cfg (rcs.hw.FR3Config): Configuration for the FR3 robot. - collision_guard (str | PathLike | None): Key to a built-in scene - robot_cfg (rcs.hw.FR3Config): Configuration for the FR3 robot. - collision_guard (str | PathLike | None): Key to a scene (requires UTN compatible scene package to be present) - or the path to a mujoco scene for collision guarding. If None, collision guarding is not used. - gripper_cfg (rcs.hw.FHConfig | None): Configuration for the gripper. If None, no gripper is used. - camera_set (BaseHardwareCameraSet | None): Camera set to be used. If None, no cameras are used. - max_relative_movement (float | tuple[float, float] | None): Maximum allowed movement. If float, it restricts - translational movement in meters. If tuple, it restricts both translational (in meters) and rotational - (in radians) movements. If None, no restriction is applied. - relative_to (RelativeTo): Specifies whether the movement is relative to a configured origin or the last step. - urdf_path (str | PathLike | None): Path to the URDF file. If None the included one is used. A URDF file is needed for collision guarding. - - Returns: - gym.Env: The configured hardware environment for the FR3 robot. - """ - if urdf_path is None: - urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] - ik = rcs.common.IK(str(urdf_path)) if urdf_path is not None else None - robot = rcs.hw.FR3(ip, ik) - robot.set_parameters(robot_cfg) - - env: gym.Env = RobotEnv(robot, ControlMode.JOINTS if collision_guard is not None else control_mode) - - env = FR3HW(env) - if isinstance(gripper_cfg, rcs.hw.FHConfig): - gripper = rcs.hw.FrankaHand(ip, gripper_cfg) - env = GripperWrapper(env, gripper, binary=True) - elif isinstance(gripper_cfg, rcs.hand.tilburg_hand.THConfig): - hand = TilburgHand(gripper_cfg) - env = HandWrapper(env, hand, binary=True) - - if camera_set is not None: - camera_set.start() - camera_set.wait_for_frames() - logger.info("CameraSet started") - env = CameraSetWrapper(env, camera_set) - - if collision_guard is not None: - assert urdf_path is not None - env = CollisionGuard.env_from_xml_paths( - env, - str(rcs.scenes.get(str(collision_guard), collision_guard)), - str(urdf_path), - gripper=True, - check_home_collision=False, - control_mode=control_mode, - tcp_offset=rcs.common.Pose(rcs.common.FrankaHandTCPOffset()), - sim_gui=True, - truncate_on_collision=False, - ) - if max_relative_movement is not None: - env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) - - return env - - -class RCSFR3MultiEnvCreator(RCSHardwareEnvCreator): - def __call__( # type: ignore - ips: list[str], - control_mode: ControlMode, - robot_cfg: rcs.hw.FR3Config, - gripper_cfg: rcs.hw.FHConfig | None = None, - camera_set: HardwareCameraSet | None = None, - max_relative_movement: float | tuple[float, float] | None = None, - relative_to: RelativeTo = RelativeTo.LAST_STEP, - urdf_path: str | PathLike | None = None, - ) -> gym.Env: - - urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] - ik = rcs.common.IK(str(urdf_path)) if urdf_path is not None else None - robots: dict[str, rcs.hw.FR3] = {} - for ip in ips: - robots[ip] = rcs.hw.FR3(ip, ik) - robots[ip].set_parameters(robot_cfg) - - envs = {} - for ip in ips: - env: gym.Env = RobotEnv(robots[ip], control_mode) - env = FR3HW(env) - if gripper_cfg is not None: - gripper = rcs.hw.FrankaHand(ip, gripper_cfg) - env = GripperWrapper(env, gripper, binary=True) - - if max_relative_movement is not None: - env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) - envs[ip] = env - - env = MultiRobotWrapper(envs) - if camera_set is not None: - camera_set.start() - camera_set.wait_for_frames() - logger.info("CameraSet started") - env = CameraSetWrapper(env, camera_set) - return env - - -class RCSFR3DefaultEnvCreator(RCSHardwareEnvCreator): - def __call__( # type: ignore - self, - robot_ip: str, - control_mode: ControlMode = ControlMode.CARTESIAN_TRPY, - delta_actions: bool = True, - camera_set: HardwareCameraSet | None = None, - gripper: bool = True, - ) -> gym.Env: - return RCSFR3EnvCreator()( - ip=robot_ip, - camera_set=camera_set, - control_mode=control_mode, - robot_cfg=default_fr3_hw_robot_cfg(), - collision_guard=None, - gripper_cfg=default_fr3_hw_gripper_cfg() if gripper else None, - max_relative_movement=(0.2, np.deg2rad(45)) if delta_actions else None, - relative_to=RelativeTo.LAST_STEP, - ) - - -class FR3SimEnvCreator(EnvCreator): +class SimEnvCreator(EnvCreator): def __call__( # type: ignore self, control_mode: ControlMode, @@ -270,11 +123,11 @@ def __call__( # type: ignore cameras: dict[str, SimCameraConfig] | None = None, ) -> gym.Env: - env_rel = FR3SimEnvCreator()( + env_rel = SimEnvCreator()( control_mode=control_mode, - robot_cfg=default_fr3_sim_robot_cfg(mjcf), + robot_cfg=default_sim_robot_cfg(mjcf), collision_guard=False, - gripper_cfg=default_fr3_sim_gripper_cfg(), + gripper_cfg=default_sim_gripper_cfg(), cameras=cameras, max_relative_movement=(0.2, np.deg2rad(45)) if delta_actions else None, relative_to=RelativeTo.LAST_STEP, diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index c1698e1c..9c3ec169 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -6,8 +6,8 @@ import rcs from rcs import sim from rcs.envs.base import ControlMode, GripperWrapper, MultiRobotWrapper, RobotEnv -from rcs.envs.space_utils import ActObsInfoWrapper -from rcs.envs.utils import default_fr3_sim_robot_cfg +from rcs.envs.space_utils import ActObsInfoWrapper, VecType +from rcs.envs.utils import default_sim_robot_cfg logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -205,7 +205,7 @@ def env_from_xml_paths( assert isinstance(env.unwrapped, RobotEnv) simulation = sim.Sim(mjmld) ik = rcs.common.IK(urdf, max_duration_ms=300) - cfg = default_fr3_sim_robot_cfg(mjmld, id) + cfg = default_sim_robot_cfg(mjmld, id) cfg.realtime = False if tcp_offset is not None: cfg.tcp_offset = tcp_offset diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py index 3e214054..9d644386 100644 --- a/python/rcs/envs/utils.py +++ b/python/rcs/envs/utils.py @@ -8,7 +8,6 @@ from digit_interface import Digit from rcs import sim from rcs._core.common import BaseCameraConfig -from rcs._core.hw import FR3Config, IKSolver from rcs._core.sim import CameraType, SimCameraConfig from rcs.camera.digit_cam import DigitCam from rcs.hand.tilburg_hand import THConfig @@ -17,7 +16,7 @@ logger.setLevel(logging.INFO) -def default_fr3_sim_robot_cfg(mjcf: str | Path = "fr3_empty_world", idx: str = "0") -> sim.SimRobotConfig: +def default_sim_robot_cfg(mjcf: str | Path = "fr3_empty_world", idx: str = "0") -> sim.SimRobotConfig: cfg = sim.SimRobotConfig() cfg.tcp_offset = get_tcp_offset(mjcf) cfg.realtime = False @@ -26,24 +25,6 @@ def default_fr3_sim_robot_cfg(mjcf: str | Path = "fr3_empty_world", idx: str = " return cfg -def default_fr3_hw_robot_cfg(async_control: bool = False) -> FR3Config: - robot_cfg = FR3Config() - robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) - robot_cfg.speed_factor = 0.1 - robot_cfg.ik_solver = IKSolver.rcs_ik - robot_cfg.async_control = async_control - return robot_cfg - - -def default_fr3_hw_gripper_cfg(async_control: bool = False) -> rcs.hw.FHConfig: - gripper_cfg = rcs.hw.FHConfig() - gripper_cfg.epsilon_inner = gripper_cfg.epsilon_outer = 0.1 - gripper_cfg.speed = 0.1 - gripper_cfg.force = 30 - gripper_cfg.async_control = async_control - return gripper_cfg - - def default_tilburg_hw_hand_cfg(file: str | PathLike | None = None) -> THConfig: hand_cfg = THConfig() hand_cfg.grasp_percentage = 1.0 @@ -51,7 +32,7 @@ def default_tilburg_hw_hand_cfg(file: str | PathLike | None = None) -> THConfig: return hand_cfg -def default_fr3_sim_gripper_cfg(idx: str = "0") -> sim.SimGripperConfig: +def default_sim_gripper_cfg(idx: str = "0") -> sim.SimGripperConfig: cfg = sim.SimGripperConfig() cfg.add_id(idx) return cfg @@ -86,6 +67,7 @@ def default_mujoco_cameraset_cfg() -> dict[str, SimCameraConfig]: } +# TODO: franka hand offset needs to be removed here def get_tcp_offset(mjcf: str | Path) -> rcs.common.Pose: """Reads out tcp offset set in mjcf file. diff --git a/python/rcs_fr3/creators.py b/python/rcs_fr3/creators.py new file mode 100644 index 00000000..53591c09 --- /dev/null +++ b/python/rcs_fr3/creators.py @@ -0,0 +1,163 @@ +import logging +from os import PathLike + +import gymnasium as gym +import numpy as np +import rcs +import rcs.hand.tilburg_hand +from rcs.camera.hw import HardwareCameraSet +from rcs.envs.base import ( + CameraSetWrapper, + ControlMode, + GripperWrapper, + HandWrapper, + MultiRobotWrapper, + RelativeActionSpace, + RelativeTo, + RobotEnv, +) +from rcs.envs.creators import RCSHardwareEnvCreator +from rcs.envs.sim import CollisionGuard +from rcs.hand.tilburg_hand import TilburgHand +from rcs_fr3.envs import FR3HW +from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + + +class RCSFR3EnvCreator(RCSHardwareEnvCreator): + def __call__( # type: ignore + self, + ip: str, + control_mode: ControlMode, + robot_cfg: rcs.hw.FR3Config, + collision_guard: str | PathLike | None = None, + gripper_cfg: rcs.hw.FHConfig | rcs.hand.tilburg_hand.THConfig | None = None, + camera_set: HardwareCameraSet | None = None, + max_relative_movement: float | tuple[float, float] | None = None, + relative_to: RelativeTo = RelativeTo.LAST_STEP, + urdf_path: str | PathLike | None = None, + ) -> gym.Env: + """ + Creates a hardware environment for the FR3 robot. + + Args: + ip (str): IP address of the robot. + control_mode (ControlMode): Control mode for the robot. + robot_cfg (rcs.hw.FR3Config): Configuration for the FR3 robot. + collision_guard (str | PathLike | None): Key to a built-in scene + robot_cfg (rcs.hw.FR3Config): Configuration for the FR3 robot. + collision_guard (str | PathLike | None): Key to a scene (requires UTN compatible scene package to be present) + or the path to a mujoco scene for collision guarding. If None, collision guarding is not used. + gripper_cfg (rcs.hw.FHConfig | None): Configuration for the gripper. If None, no gripper is used. + camera_set (BaseHardwareCameraSet | None): Camera set to be used. If None, no cameras are used. + max_relative_movement (float | tuple[float, float] | None): Maximum allowed movement. If float, it restricts + translational movement in meters. If tuple, it restricts both translational (in meters) and rotational + (in radians) movements. If None, no restriction is applied. + relative_to (RelativeTo): Specifies whether the movement is relative to a configured origin or the last step. + urdf_path (str | PathLike | None): Path to the URDF file. If None the included one is used. A URDF file is needed for collision guarding. + + Returns: + gym.Env: The configured hardware environment for the FR3 robot. + """ + if urdf_path is None: + urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] + ik = rcs.common.IK(str(urdf_path)) if urdf_path is not None else None + robot = rcs.hw.FR3(ip, ik) + robot.set_parameters(robot_cfg) + + env: gym.Env = RobotEnv(robot, ControlMode.JOINTS if collision_guard is not None else control_mode) + + env = FR3HW(env) + if isinstance(gripper_cfg, rcs.hw.FHConfig): + gripper = rcs.hw.FrankaHand(ip, gripper_cfg) + env = GripperWrapper(env, gripper, binary=True) + elif isinstance(gripper_cfg, rcs.hand.tilburg_hand.THConfig): + hand = TilburgHand(gripper_cfg) + env = HandWrapper(env, hand, binary=True) + + if camera_set is not None: + camera_set.start() + camera_set.wait_for_frames() + logger.info("CameraSet started") + env = CameraSetWrapper(env, camera_set) + + if collision_guard is not None: + assert urdf_path is not None + env = CollisionGuard.env_from_xml_paths( + env, + str(rcs.scenes.get(str(collision_guard), collision_guard)), + str(urdf_path), + gripper=True, + check_home_collision=False, + control_mode=control_mode, + tcp_offset=rcs.common.Pose(rcs.common.FrankaHandTCPOffset()), + sim_gui=True, + truncate_on_collision=False, + ) + if max_relative_movement is not None: + env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) + + return env + + +class RCSFR3MultiEnvCreator(RCSHardwareEnvCreator): + def __call__( # type: ignore + ips: list[str], + control_mode: ControlMode, + robot_cfg: rcs.hw.FR3Config, + gripper_cfg: rcs.hw.FHConfig | None = None, + camera_set: HardwareCameraSet | None = None, + max_relative_movement: float | tuple[float, float] | None = None, + relative_to: RelativeTo = RelativeTo.LAST_STEP, + urdf_path: str | PathLike | None = None, + ) -> gym.Env: + + urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] + ik = rcs.common.IK(str(urdf_path)) if urdf_path is not None else None + robots: dict[str, rcs.hw.FR3] = {} + for ip in ips: + robots[ip] = rcs.hw.FR3(ip, ik) + robots[ip].set_parameters(robot_cfg) + + envs = {} + for ip in ips: + env: gym.Env = RobotEnv(robots[ip], control_mode) + env = FR3HW(env) + if gripper_cfg is not None: + gripper = rcs.hw.FrankaHand(ip, gripper_cfg) + env = GripperWrapper(env, gripper, binary=True) + + if max_relative_movement is not None: + env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) + envs[ip] = env + + env = MultiRobotWrapper(envs) + if camera_set is not None: + camera_set.start() + camera_set.wait_for_frames() + logger.info("CameraSet started") + env = CameraSetWrapper(env, camera_set) + return env + + +class RCSFR3DefaultEnvCreator(RCSHardwareEnvCreator): + def __call__( # type: ignore + self, + robot_ip: str, + control_mode: ControlMode = ControlMode.CARTESIAN_TRPY, + delta_actions: bool = True, + camera_set: HardwareCameraSet | None = None, + gripper: bool = True, + ) -> gym.Env: + return RCSFR3EnvCreator()( + ip=robot_ip, + camera_set=camera_set, + control_mode=control_mode, + robot_cfg=default_fr3_hw_robot_cfg(), + collision_guard=None, + gripper_cfg=default_fr3_hw_gripper_cfg() if gripper else None, + max_relative_movement=(0.2, np.deg2rad(45)) if delta_actions else None, + relative_to=RelativeTo.LAST_STEP, + ) diff --git a/python/rcs_fr3/desk.py b/python/rcs_fr3/desk.py index 3cedad71..140ab14d 100644 --- a/python/rcs_fr3/desk.py +++ b/python/rcs_fr3/desk.py @@ -13,7 +13,7 @@ import rcs import requests from dotenv import load_dotenv -from rcs.envs.creators import default_fr3_hw_gripper_cfg +from rcs_fr3.utils import default_fr3_hw_gripper_cfg from requests.packages import urllib3 # type: ignore[attr-defined] from websockets.sync.client import connect diff --git a/python/rcs_fr3/utils.py b/python/rcs_fr3/utils.py new file mode 100644 index 00000000..6c8fa7dc --- /dev/null +++ b/python/rcs_fr3/utils.py @@ -0,0 +1,19 @@ +from rcs import common, hw + + +def default_fr3_hw_robot_cfg(async_control: bool = False) -> hw.FR3Config: + robot_cfg = hw.FR3Config() + robot_cfg.tcp_offset = common.Pose(common.FrankaHandTCPOffset()) + robot_cfg.speed_factor = 0.1 + robot_cfg.ik_solver = hw.IKSolver.rcs_ik + robot_cfg.async_control = async_control + return robot_cfg + + +def default_fr3_hw_gripper_cfg(async_control: bool = False) -> hw.FHConfig: + gripper_cfg = hw.FHConfig() + gripper_cfg.epsilon_inner = gripper_cfg.epsilon_outer = 0.1 + gripper_cfg.speed = 0.1 + gripper_cfg.force = 30 + gripper_cfg.async_control = async_control + return gripper_cfg diff --git a/python/tests/test_sim_envs.py b/python/tests/test_sim_envs.py index cd1bfb36..c5e2fd5c 100644 --- a/python/tests/test_sim_envs.py +++ b/python/tests/test_sim_envs.py @@ -11,22 +11,22 @@ TQuatDictType, TRPYDictType, ) -from rcs.envs.creators import FR3SimEnvCreator +from rcs.envs.creators import SimEnvCreator from rcs.envs.utils import ( - default_fr3_sim_gripper_cfg, - default_fr3_sim_robot_cfg, default_mujoco_cameraset_cfg, + default_sim_gripper_cfg, + default_sim_robot_cfg, ) @pytest.fixture() def cfg(): - return default_fr3_sim_robot_cfg() + return default_sim_robot_cfg() @pytest.fixture() def gripper_cfg(): - return default_fr3_sim_gripper_cfg() + return default_sim_gripper_cfg() @pytest.fixture() @@ -64,7 +64,7 @@ def test_reset(self, cfg, gripper_cfg, cam_cfg): # TODO: # - test initial pose after reset. # - test initial gripper config. - env = FR3SimEnvCreator()( + env = SimEnvCreator()( ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=gripper_cfg, cameras=cam_cfg, max_relative_movement=None ) # Test double reset. Regression test. A lot can go wrong when resetting. @@ -75,7 +75,7 @@ def test_zero_action_trpy(self, cfg): """ Test that a zero action does not change the state significantly """ - env = FR3SimEnvCreator()( + env = SimEnvCreator()( ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=None, cameras=None, max_relative_movement=None ) obs_initial, _ = env.reset() @@ -88,7 +88,7 @@ def test_non_zero_action_trpy(self, cfg): This is for testing that a certain action leads to the expected change in state """ # env creation - env = FR3SimEnvCreator()( + env = SimEnvCreator()( ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=None, cameras=None, max_relative_movement=None ) obs_initial, _ = env.reset() @@ -111,7 +111,7 @@ def test_non_zero_action_trpy(self, cfg): def test_relative_zero_action_trpy(self, cfg, gripper_cfg): # env creation - env = FR3SimEnvCreator()( + env = SimEnvCreator()( ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=gripper_cfg, cameras=None, max_relative_movement=0.5 ) obs_initial, _ = env.reset() @@ -124,7 +124,7 @@ def test_relative_zero_action_trpy(self, cfg, gripper_cfg): def test_relative_non_zero_action(self, cfg, gripper_cfg): # env creation - env = FR3SimEnvCreator()( + env = SimEnvCreator()( ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=gripper_cfg, cameras=None, max_relative_movement=0.5 ) obs_initial, _ = env.reset() @@ -142,7 +142,7 @@ def test_collision_trpy(self, cfg, gripper_cfg): Check that an obvious collision is detected by sim """ # env creation - env = FR3SimEnvCreator()( + env = SimEnvCreator()( ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=gripper_cfg, cameras=None, max_relative_movement=None ) obs, _ = env.reset() @@ -159,7 +159,7 @@ def test_collision_guard_trpy(self, cfg, gripper_cfg): Check that an obvious collision is detected by the CollisionGuard """ # env creation - env = FR3SimEnvCreator()( + env = SimEnvCreator()( ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=gripper_cfg, @@ -192,7 +192,7 @@ def test_reset(self, cfg, gripper_cfg, cam_cfg): # TODO: # - test initial pose after reset. # - test initial gripper config. - env = FR3SimEnvCreator()( + env = SimEnvCreator()( ControlMode.CARTESIAN_TQuat, cfg, gripper_cfg=gripper_cfg, @@ -208,7 +208,7 @@ def test_non_zero_action_tquat(self, cfg): Test that a zero action does not change the state significantly in the tquat configuration """ # env creation - env = FR3SimEnvCreator()( + env = SimEnvCreator()( ControlMode.CARTESIAN_TQuat, cfg, gripper_cfg=None, cameras=None, max_relative_movement=None ) obs_initial, _ = env.reset() @@ -229,7 +229,7 @@ def test_zero_action_tquat(self, cfg): Test that a zero action does not change the state significantly in the tquat configuration """ # env creation - env = FR3SimEnvCreator()( + env = SimEnvCreator()( ControlMode.CARTESIAN_TQuat, cfg, gripper_cfg=None, cameras=None, max_relative_movement=None ) obs_initial, info_ = env.reset() @@ -240,7 +240,7 @@ def test_zero_action_tquat(self, cfg): def test_relative_zero_action_tquat(self, cfg, gripper_cfg): # env creation - env_rel = FR3SimEnvCreator()( + env_rel = SimEnvCreator()( ControlMode.CARTESIAN_TQuat, cfg, gripper_cfg=gripper_cfg, @@ -258,7 +258,7 @@ def test_collision_tquat(self, cfg, gripper_cfg): Check that an obvious collision is detected by sim """ # env creation - env = FR3SimEnvCreator()( + env = SimEnvCreator()( ControlMode.CARTESIAN_TQuat, cfg, gripper_cfg=gripper_cfg, @@ -279,7 +279,7 @@ def test_collision_guard_tquat(self, cfg, gripper_cfg): Check that an obvious collision is detected by the CollisionGuard """ # env creation - env = FR3SimEnvCreator()( + env = SimEnvCreator()( ControlMode.CARTESIAN_TQuat, cfg, gripper_cfg=gripper_cfg, @@ -312,7 +312,7 @@ def test_reset(self, cfg, gripper_cfg, cam_cfg): # TODO: # - test initial pose after reset. # - test initial gripper config. - env = FR3SimEnvCreator()( + env = SimEnvCreator()( ControlMode.JOINTS, cfg, gripper_cfg=gripper_cfg, cameras=cam_cfg, max_relative_movement=None ) # Test double reset. Regression test. A lot can go wrong when resetting. @@ -324,7 +324,7 @@ def test_zero_action_joints(self, cfg): This is for testing that a certain action leads to the expected change in state """ # env creation - env = FR3SimEnvCreator()(ControlMode.JOINTS, cfg, gripper_cfg=None, cameras=None, max_relative_movement=None) + env = SimEnvCreator()(ControlMode.JOINTS, cfg, gripper_cfg=None, cameras=None, max_relative_movement=None) obs_initial, _ = env.reset() # action to be performed zero_action = JointsDictType(joints=np.array(obs_initial["joints"])) @@ -338,7 +338,7 @@ def test_non_zero_action_joints(self, cfg): This is for testing that a certain action leads to the expected change in state """ # env creation - env = FR3SimEnvCreator()(ControlMode.JOINTS, cfg, gripper_cfg=None, cameras=None, max_relative_movement=None) + env = SimEnvCreator()(ControlMode.JOINTS, cfg, gripper_cfg=None, cameras=None, max_relative_movement=None) obs_initial, _ = env.reset() new_joint_vals = obs_initial["joints"] + np.array([0.1, 0.1, 0.1, 0.1, -0.1, -0.1, 0.1], dtype=np.float32) # action to be performed @@ -353,7 +353,7 @@ def test_collision_joints(self, cfg, gripper_cfg): Check that an obvious collision is detected by the CollisionGuard """ # env creation - env = FR3SimEnvCreator()( + env = SimEnvCreator()( ControlMode.JOINTS, cfg, gripper_cfg=gripper_cfg, cameras=None, max_relative_movement=None ) env.reset() @@ -368,7 +368,7 @@ def test_collision_guard_joints(self, cfg, gripper_cfg): Check that an obvious collision is detected by sim """ # env creation - env = FR3SimEnvCreator()( + env = SimEnvCreator()( ControlMode.JOINTS, cfg, gripper_cfg=gripper_cfg, @@ -394,9 +394,7 @@ def test_relative_zero_action_joints(self, cfg, gripper_cfg): Check that an obvious collision is detected by the CollisionGuard """ # env creation - env = FR3SimEnvCreator()( - ControlMode.JOINTS, cfg, gripper_cfg=gripper_cfg, cameras=None, max_relative_movement=0.5 - ) + env = SimEnvCreator()(ControlMode.JOINTS, cfg, gripper_cfg=gripper_cfg, cameras=None, max_relative_movement=0.5) obs_initial, _ = env.reset() act = JointsDictType(joints=np.array([0, 0, 0, 0, 0, 0, 0], dtype=np.float32)) act.update(GripperDictType(gripper=1)) From bb1d031ff2d028a7fa85bffa7742c19916a6dc4f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Wed, 18 Jun 2025 14:58:07 +0200 Subject: [PATCH 06/32] refactor: seperate hardware extensions --- {src => extensions/fr3/src}/hw/CMakeLists.txt | 0 {src => extensions/fr3/src}/hw/FR3.cpp | 0 {src => extensions/fr3/src}/hw/FR3.h | 0 {src => extensions/fr3/src}/hw/FR3MotionGenerator.cpp | 0 {src => extensions/fr3/src}/hw/FR3MotionGenerator.h | 0 {src => extensions/fr3/src}/hw/FrankaHand.cpp | 0 {src => extensions/fr3/src}/hw/FrankaHand.h | 0 {src => extensions/fr3/src}/hw/main.cpp | 0 {python => extensions/fr3/src}/rcs_fr3/__init__.py | 0 {python => extensions/fr3/src}/rcs_fr3/__main__.py | 0 {python => extensions/fr3/src}/rcs_fr3/creators.py | 0 {python => extensions/fr3/src}/rcs_fr3/desk.py | 0 {python => extensions/fr3/src}/rcs_fr3/envs.py | 0 {python => extensions/fr3/src}/rcs_fr3/utils.py | 0 {python => extensions/realsense}/rcs_realsense/__init__.py | 0 {python => extensions/realsense}/rcs_realsense/__main__.py | 0 {python => extensions/realsense}/rcs_realsense/camera.py | 0 {python => extensions/realsense}/rcs_realsense/utils.py | 0 {python => extensions/so101/src}/rcs_so101/__init__.py | 0 {python => extensions/so101/src}/rcs_so101/creators.py | 0 {python => extensions/so101/src}/rcs_so101/env_joint_control.py | 0 {python => extensions/so101/src}/rcs_so101/hw.py | 0 22 files changed, 0 insertions(+), 0 deletions(-) rename {src => extensions/fr3/src}/hw/CMakeLists.txt (100%) rename {src => extensions/fr3/src}/hw/FR3.cpp (100%) rename {src => extensions/fr3/src}/hw/FR3.h (100%) rename {src => extensions/fr3/src}/hw/FR3MotionGenerator.cpp (100%) rename {src => extensions/fr3/src}/hw/FR3MotionGenerator.h (100%) rename {src => extensions/fr3/src}/hw/FrankaHand.cpp (100%) rename {src => extensions/fr3/src}/hw/FrankaHand.h (100%) rename {src => extensions/fr3/src}/hw/main.cpp (100%) rename {python => extensions/fr3/src}/rcs_fr3/__init__.py (100%) rename {python => extensions/fr3/src}/rcs_fr3/__main__.py (100%) rename {python => extensions/fr3/src}/rcs_fr3/creators.py (100%) rename {python => extensions/fr3/src}/rcs_fr3/desk.py (100%) rename {python => extensions/fr3/src}/rcs_fr3/envs.py (100%) rename {python => extensions/fr3/src}/rcs_fr3/utils.py (100%) rename {python => extensions/realsense}/rcs_realsense/__init__.py (100%) rename {python => extensions/realsense}/rcs_realsense/__main__.py (100%) rename {python => extensions/realsense}/rcs_realsense/camera.py (100%) rename {python => extensions/realsense}/rcs_realsense/utils.py (100%) rename {python => extensions/so101/src}/rcs_so101/__init__.py (100%) rename {python => extensions/so101/src}/rcs_so101/creators.py (100%) rename {python => extensions/so101/src}/rcs_so101/env_joint_control.py (100%) rename {python => extensions/so101/src}/rcs_so101/hw.py (100%) diff --git a/src/hw/CMakeLists.txt b/extensions/fr3/src/hw/CMakeLists.txt similarity index 100% rename from src/hw/CMakeLists.txt rename to extensions/fr3/src/hw/CMakeLists.txt diff --git a/src/hw/FR3.cpp b/extensions/fr3/src/hw/FR3.cpp similarity index 100% rename from src/hw/FR3.cpp rename to extensions/fr3/src/hw/FR3.cpp diff --git a/src/hw/FR3.h b/extensions/fr3/src/hw/FR3.h similarity index 100% rename from src/hw/FR3.h rename to extensions/fr3/src/hw/FR3.h diff --git a/src/hw/FR3MotionGenerator.cpp b/extensions/fr3/src/hw/FR3MotionGenerator.cpp similarity index 100% rename from src/hw/FR3MotionGenerator.cpp rename to extensions/fr3/src/hw/FR3MotionGenerator.cpp diff --git a/src/hw/FR3MotionGenerator.h b/extensions/fr3/src/hw/FR3MotionGenerator.h similarity index 100% rename from src/hw/FR3MotionGenerator.h rename to extensions/fr3/src/hw/FR3MotionGenerator.h diff --git a/src/hw/FrankaHand.cpp b/extensions/fr3/src/hw/FrankaHand.cpp similarity index 100% rename from src/hw/FrankaHand.cpp rename to extensions/fr3/src/hw/FrankaHand.cpp diff --git a/src/hw/FrankaHand.h b/extensions/fr3/src/hw/FrankaHand.h similarity index 100% rename from src/hw/FrankaHand.h rename to extensions/fr3/src/hw/FrankaHand.h diff --git a/src/hw/main.cpp b/extensions/fr3/src/hw/main.cpp similarity index 100% rename from src/hw/main.cpp rename to extensions/fr3/src/hw/main.cpp diff --git a/python/rcs_fr3/__init__.py b/extensions/fr3/src/rcs_fr3/__init__.py similarity index 100% rename from python/rcs_fr3/__init__.py rename to extensions/fr3/src/rcs_fr3/__init__.py diff --git a/python/rcs_fr3/__main__.py b/extensions/fr3/src/rcs_fr3/__main__.py similarity index 100% rename from python/rcs_fr3/__main__.py rename to extensions/fr3/src/rcs_fr3/__main__.py diff --git a/python/rcs_fr3/creators.py b/extensions/fr3/src/rcs_fr3/creators.py similarity index 100% rename from python/rcs_fr3/creators.py rename to extensions/fr3/src/rcs_fr3/creators.py diff --git a/python/rcs_fr3/desk.py b/extensions/fr3/src/rcs_fr3/desk.py similarity index 100% rename from python/rcs_fr3/desk.py rename to extensions/fr3/src/rcs_fr3/desk.py diff --git a/python/rcs_fr3/envs.py b/extensions/fr3/src/rcs_fr3/envs.py similarity index 100% rename from python/rcs_fr3/envs.py rename to extensions/fr3/src/rcs_fr3/envs.py diff --git a/python/rcs_fr3/utils.py b/extensions/fr3/src/rcs_fr3/utils.py similarity index 100% rename from python/rcs_fr3/utils.py rename to extensions/fr3/src/rcs_fr3/utils.py diff --git a/python/rcs_realsense/__init__.py b/extensions/realsense/rcs_realsense/__init__.py similarity index 100% rename from python/rcs_realsense/__init__.py rename to extensions/realsense/rcs_realsense/__init__.py diff --git a/python/rcs_realsense/__main__.py b/extensions/realsense/rcs_realsense/__main__.py similarity index 100% rename from python/rcs_realsense/__main__.py rename to extensions/realsense/rcs_realsense/__main__.py diff --git a/python/rcs_realsense/camera.py b/extensions/realsense/rcs_realsense/camera.py similarity index 100% rename from python/rcs_realsense/camera.py rename to extensions/realsense/rcs_realsense/camera.py diff --git a/python/rcs_realsense/utils.py b/extensions/realsense/rcs_realsense/utils.py similarity index 100% rename from python/rcs_realsense/utils.py rename to extensions/realsense/rcs_realsense/utils.py diff --git a/python/rcs_so101/__init__.py b/extensions/so101/src/rcs_so101/__init__.py similarity index 100% rename from python/rcs_so101/__init__.py rename to extensions/so101/src/rcs_so101/__init__.py diff --git a/python/rcs_so101/creators.py b/extensions/so101/src/rcs_so101/creators.py similarity index 100% rename from python/rcs_so101/creators.py rename to extensions/so101/src/rcs_so101/creators.py diff --git a/python/rcs_so101/env_joint_control.py b/extensions/so101/src/rcs_so101/env_joint_control.py similarity index 100% rename from python/rcs_so101/env_joint_control.py rename to extensions/so101/src/rcs_so101/env_joint_control.py diff --git a/python/rcs_so101/hw.py b/extensions/so101/src/rcs_so101/hw.py similarity index 100% rename from python/rcs_so101/hw.py rename to extensions/so101/src/rcs_so101/hw.py From 4d982071a83914456cf720dc9d1d4f26b9f9f02b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Wed, 18 Jun 2025 15:00:45 +0200 Subject: [PATCH 07/32] fix: realsense structure --- extensions/realsense/{ => src}/rcs_realsense/__init__.py | 0 extensions/realsense/{ => src}/rcs_realsense/__main__.py | 0 extensions/realsense/{ => src}/rcs_realsense/camera.py | 0 extensions/realsense/{ => src}/rcs_realsense/utils.py | 0 4 files changed, 0 insertions(+), 0 deletions(-) rename extensions/realsense/{ => src}/rcs_realsense/__init__.py (100%) rename extensions/realsense/{ => src}/rcs_realsense/__main__.py (100%) rename extensions/realsense/{ => src}/rcs_realsense/camera.py (100%) rename extensions/realsense/{ => src}/rcs_realsense/utils.py (100%) diff --git a/extensions/realsense/rcs_realsense/__init__.py b/extensions/realsense/src/rcs_realsense/__init__.py similarity index 100% rename from extensions/realsense/rcs_realsense/__init__.py rename to extensions/realsense/src/rcs_realsense/__init__.py diff --git a/extensions/realsense/rcs_realsense/__main__.py b/extensions/realsense/src/rcs_realsense/__main__.py similarity index 100% rename from extensions/realsense/rcs_realsense/__main__.py rename to extensions/realsense/src/rcs_realsense/__main__.py diff --git a/extensions/realsense/rcs_realsense/camera.py b/extensions/realsense/src/rcs_realsense/camera.py similarity index 100% rename from extensions/realsense/rcs_realsense/camera.py rename to extensions/realsense/src/rcs_realsense/camera.py diff --git a/extensions/realsense/rcs_realsense/utils.py b/extensions/realsense/src/rcs_realsense/utils.py similarity index 100% rename from extensions/realsense/rcs_realsense/utils.py rename to extensions/realsense/src/rcs_realsense/utils.py From b713f068bdef3593b47c00c2944be091239babd4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Wed, 18 Jun 2025 15:04:24 +0200 Subject: [PATCH 08/32] feat: added initial cmake structure for fr3 --- CMakeLists.txt | 22 +--- extensions/README.md | 1 + extensions/fr3/CMakeLists.txt | 66 ++++++++++ extensions/fr3/pyproject.toml | 36 ++++++ extensions/fr3/src/CMakeLists.txt | 3 + extensions/fr3/src/pybind/CMakeLists.txt | 10 ++ extensions/fr3/src/pybind/rcs.cpp | 156 +++++++++++++++++++++++ extensions/realsense/pyproject.toml | 27 ++++ extensions/so101/pyproject.toml | 27 ++++ pyproject.toml | 10 +- src/CMakeLists.txt | 1 - src/pybind/CMakeLists.txt | 9 +- src/pybind/rcs.cpp | 109 ---------------- 13 files changed, 333 insertions(+), 144 deletions(-) create mode 100644 extensions/README.md create mode 100644 extensions/fr3/CMakeLists.txt create mode 100644 extensions/fr3/pyproject.toml create mode 100644 extensions/fr3/src/CMakeLists.txt create mode 100644 extensions/fr3/src/pybind/CMakeLists.txt create mode 100644 extensions/fr3/src/pybind/rcs.cpp create mode 100644 extensions/realsense/pyproject.toml create mode 100644 extensions/so101/pyproject.toml diff --git a/CMakeLists.txt b/CMakeLists.txt index e99caeb8..ca3c907a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,7 @@ project( rcs LANGUAGES C CXX VERSION 0.4.0 - DESCRIPTION "UTNs Robot Control Stack Library" + DESCRIPTION "Robot Control Stack Library" ) set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH}) @@ -31,29 +31,13 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set(BUILD_SHARED_LIBS OFF) set(CMAKE_POSITION_INDEPENDENT_CODE ON) -# turn off libfranka tests -set(BUILD_TESTS OFF) -set(BUILD_EXAMPLES OFF) -set(RL_BUILD_DEMOS OFF) -set(RL_BUILD_RL_SG OFF) -set(RL_BUILD_TESTS OFF) -set(RL_BUILD_EXTRAS OFF) -set(BUILD_PYTHON_INTERFACE OFF) -set(BUILD_DOCUMENTATION OFF) - include(FetchContent) find_package(Eigen3 REQUIRED) find_package(Python3 COMPONENTS Interpreter Development REQUIRED) find_package(MuJoCo REQUIRED) -FetchContent_Declare( - libfranka - GIT_REPOSITORY https://github.com/frankaemika/libfranka.git - GIT_TAG 0.15.0 - GIT_PROGRESS TRUE - EXCLUDE_FROM_ALL -) + FetchContent_Declare(rl GIT_REPOSITORY https://github.com/roboticslibrary/rl.git GIT_TAG 0b3797215345a1d37903634095361233d190b2e6 @@ -67,7 +51,7 @@ FetchContent_Declare(pybind11 EXCLUDE_FROM_ALL ) -FetchContent_MakeAvailable(libfranka rl pybind11) +FetchContent_MakeAvailable(rl pybind11) include(compile_scenes) add_subdirectory(src) diff --git a/extensions/README.md b/extensions/README.md new file mode 100644 index 00000000..5692de4c --- /dev/null +++ b/extensions/README.md @@ -0,0 +1 @@ +# Hardware Extensions \ No newline at end of file diff --git a/extensions/fr3/CMakeLists.txt b/extensions/fr3/CMakeLists.txt new file mode 100644 index 00000000..28d7917e --- /dev/null +++ b/extensions/fr3/CMakeLists.txt @@ -0,0 +1,66 @@ +cmake_minimum_required(VERSION 3.19) + +project( + rcs + LANGUAGES C CXX + VERSION 0.4.0 + DESCRIPTION "RCS Libfranka integration" +) + +set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH}) + +set(CMAKE_POLICY_DEFAULT_CMP0077 NEW) # Allow us to set options for subprojects + +cmake_policy(SET CMP0048 NEW) # Set version in project +# Allow target properties affecting visibility during linking in static libraries +set(CMAKE_POLICY_DEFAULT_CMP0063 NEW) +cmake_policy(SET CMP0072 NEW) # Use GLVND instead of legacy libGL.so +cmake_policy(SET CMP0135 NEW) # Use timestamp of file extraction not download +cmake_policy(SET CMP0140 NEW) # Check return arguments + +set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) +set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) +set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) + +set(CMAKE_CXX_STANDARD 20) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +set(BUILD_SHARED_LIBS OFF) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +# turn off libfranka tests +set(BUILD_TESTS OFF) +set(BUILD_EXAMPLES OFF) +set(RL_BUILD_DEMOS OFF) +set(RL_BUILD_RL_SG OFF) +set(RL_BUILD_TESTS OFF) +set(RL_BUILD_EXTRAS OFF) +set(BUILD_PYTHON_INTERFACE OFF) +set(BUILD_DOCUMENTATION OFF) + +include(FetchContent) + +find_package(Eigen3 REQUIRED) +find_package(Python3 COMPONENTS Interpreter Development REQUIRED) +find_package(MuJoCo REQUIRED) + +FetchContent_Declare( + libfranka + GIT_REPOSITORY https://github.com/frankaemika/libfranka.git + GIT_TAG 0.15.0 + GIT_PROGRESS TRUE + EXCLUDE_FROM_ALL +) +FetchContent_Declare(pybind11 + GIT_REPOSITORY https://github.com/pybind/pybind11.git + GIT_TAG v2.13.4 + GIT_PROGRESS TRUE + EXCLUDE_FROM_ALL +) + +FetchContent_MakeAvailable(libfranka pybind11) + +add_subdirectory(src) diff --git a/extensions/fr3/pyproject.toml b/extensions/fr3/pyproject.toml new file mode 100644 index 00000000..9cbdcb90 --- /dev/null +++ b/extensions/fr3/pyproject.toml @@ -0,0 +1,36 @@ +[build-system] +requires = [] +build-backend = "scikit_build_core.build" + +[project] +name = "rcs_fr3" +version = "0.4.0" +description="RCS libfranka integration" +dependencies = [ + # NOTE: when changing the pin version, also change it in requirements_dev.txt + "pin==2.7.0", +] +readme = "README.md" +maintainers = [ + { name = "Tobias Jülg", email = "tobias.juelg@utn.de" }, +] +authors = [ + { name = "Tobias Jülg", email = "tobias.juelg@utn.de" }, + { name = "Pierre Krack", email = "pierre.krack@utn.de" }, +] +requires-python = ">=3.10" +license = {file = "LICENSE"} +classifiers = [ + "Development Status :: 3 - Alpha", + "License :: OSI Approved :: GNU Affero General Public License v3 (AGPLv3)", + "Programming Language :: Python :: 3", +] + + +[tool.scikit-build] +build.verbose = true +build.targets = ["_core"] +logging.level = "INFO" +build-dir = "build" +wheel.packages = ["src/rcs_fr3"] +install.components = ["python_package"] \ No newline at end of file diff --git a/extensions/fr3/src/CMakeLists.txt b/extensions/fr3/src/CMakeLists.txt new file mode 100644 index 00000000..b61f8ab9 --- /dev/null +++ b/extensions/fr3/src/CMakeLists.txt @@ -0,0 +1,3 @@ +target_include_directories(common INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) +add_subdirectory(hw) +add_subdirectory(pybind) diff --git a/extensions/fr3/src/pybind/CMakeLists.txt b/extensions/fr3/src/pybind/CMakeLists.txt new file mode 100644 index 00000000..d6ad1efc --- /dev/null +++ b/extensions/fr3/src/pybind/CMakeLists.txt @@ -0,0 +1,10 @@ +pybind11_add_module(_core MODULE rcs.cpp) +target_link_libraries(_core PRIVATE hw common) +target_compile_definitions(_core PRIVATE VERSION_INFO=${PROJECT_VERSION}) + +set_target_properties(franka PROPERTIES + INSTALL_RPATH "$ORIGIN/../cmeel.prefix/lib" + INTERPROCEDURAL_OPTIMIZATION TRUE +) +# in pip +install(TARGETS _core franka DESTINATION rcs COMPONENT python_package) diff --git a/extensions/fr3/src/pybind/rcs.cpp b/extensions/fr3/src/pybind/rcs.cpp new file mode 100644 index 00000000..f1fe862e --- /dev/null +++ b/extensions/fr3/src/pybind/rcs.cpp @@ -0,0 +1,156 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "rl/mdl/UrdfFactory.h" + +// TODO: define exceptions + +#define STRINGIFY(x) #x +#define MACRO_STRINGIFY(x) STRINGIFY(x) + +namespace py = pybind11; + +PYBIND11_MODULE(_core, m) { + m.doc() = R"pbdoc( + Robot Control Stack Python Bindings + ----------------------- + + .. currentmodule:: _core + + .. autosummary:: + :toctree: _generate + + )pbdoc"; +#ifdef VERSION_INFO + m.attr("__version__") = MACRO_STRINGIFY(VERSION_INFO); +#else + m.attr("__version__") = "dev"; +#endif + + // HARDWARE MODULE + auto hw = m.def_submodule("hw", "rcs fr3 module"); + + py::class_(hw, "FR3State") + .def(py::init<>()); + py::class_(hw, "FR3Load") + .def(py::init<>()) + .def_readwrite("load_mass", &rcs::hw::FR3Load::load_mass) + .def_readwrite("f_x_cload", &rcs::hw::FR3Load::f_x_cload) + .def_readwrite("load_inertia", &rcs::hw::FR3Load::load_inertia); + + py::enum_(hw, "IKSolver") + .value("franka_ik", rcs::hw::IKSolver::franka_ik) + .value("rcs_ik", rcs::hw::IKSolver::rcs_ik) + .export_values(); + + py::class_(hw, "FR3Config") + .def(py::init<>()) + .def_readwrite("ik_solver", &rcs::hw::FR3Config::ik_solver) + .def_readwrite("speed_factor", &rcs::hw::FR3Config::speed_factor) + .def_readwrite("load_parameters", &rcs::hw::FR3Config::load_parameters) + .def_readwrite("nominal_end_effector_frame", + &rcs::hw::FR3Config::nominal_end_effector_frame) + .def_readwrite("world_to_robot", &rcs::hw::FR3Config::world_to_robot) + .def_readwrite("tcp_offset", &rcs::hw::FR3Config::tcp_offset) + .def_readwrite("async_control", &rcs::hw::FR3Config::async_control); + + py::class_(hw, "FHConfig") + .def(py::init<>()) + .def_readwrite("grasping_width", &rcs::hw::FHConfig::grasping_width) + .def_readwrite("speed", &rcs::hw::FHConfig::speed) + .def_readwrite("force", &rcs::hw::FHConfig::force) + .def_readwrite("epsilon_inner", &rcs::hw::FHConfig::epsilon_inner) + .def_readwrite("epsilon_outer", &rcs::hw::FHConfig::epsilon_outer) + .def_readwrite("async_control", &rcs::hw::FHConfig::async_control); + + py::class_(hw, "FHState") + .def(py::init<>()) + .def_readonly("width", &rcs::hw::FHState::width) + .def_readonly("is_grasped", &rcs::hw::FHState::is_grasped) + .def_readonly("is_moving", &rcs::hw::FHState::is_moving) + .def_readonly("bool_state", &rcs::hw::FHState::bool_state) + .def_readonly("last_commanded_width", + &rcs::hw::FHState::last_commanded_width) + .def_readonly("max_unnormalized_width", + &rcs::hw::FHState::max_unnormalized_width) + .def_readonly("temperature", &rcs::hw::FHState::temperature); + + py::class_>( + hw, "FR3") + .def(py::init>>(), + py::arg("ip"), py::arg("ik") = std::nullopt) + .def("set_parameters", &rcs::hw::FR3::set_parameters, py::arg("cfg")) + .def("get_parameters", &rcs::hw::FR3::get_parameters) + .def("get_state", &rcs::hw::FR3::get_state) + .def("set_default_robot_behavior", + &rcs::hw::FR3::set_default_robot_behavior) + .def("set_guiding_mode", &rcs::hw::FR3::set_guiding_mode, + py::arg("x") = true, py::arg("y") = true, py::arg("z") = true, + py::arg("roll") = true, py::arg("pitch") = true, + py::arg("yaw") = true, py::arg("elbow") = true) + .def("zero_torque_guiding", &rcs::hw::FR3::zero_torque_guiding) + .def("osc_set_cartesian_position", + &rcs::hw::FR3::osc_set_cartesian_position, + py::arg("desired_pos_EE_in_base_frame")) + .def("controller_set_joint_position", + &rcs::hw::FR3::controller_set_joint_position, py::arg("desired_q")) + .def("stop_control_thread", &rcs::hw::FR3::stop_control_thread) + .def("automatic_error_recovery", &rcs::hw::FR3::automatic_error_recovery) + .def("double_tap_robot_to_continue", + &rcs::hw::FR3::double_tap_robot_to_continue) + .def("set_cartesian_position_internal", + &rcs::hw::FR3::set_cartesian_position_ik, py::arg("pose")) + .def("set_cartesian_position_ik", + &rcs::hw::FR3::set_cartesian_position_internal, py::arg("pose"), + py::arg("max_time"), py::arg("elbow"), py::arg("max_force") = 5); + + py::class_>(hw, "FrankaHand") + .def(py::init(), + py::arg("ip"), py::arg("cfg")) + .def("get_parameters", &rcs::hw::FrankaHand::get_parameters) + .def("get_state", &rcs::hw::FrankaHand::get_state) + .def("set_parameters", &rcs::hw::FrankaHand::set_parameters, + py::arg("cfg")) + .def("is_grasped", &rcs::hw::FrankaHand::is_grasped) + .def("homing", &rcs::hw::FrankaHand::homing); + + auto hw_except = + hw.def_submodule("exceptions", "exceptions from the hardware module"); + py::register_exception(hw_except, "FrankaException", + PyExc_RuntimeError); + py::register_exception( + hw_except, "FrankaModelException", PyExc_RuntimeError); + py::register_exception( + hw_except, "FrankaNetworkException", PyExc_RuntimeError); + py::register_exception( + hw_except, "FrankaProtocolException", PyExc_RuntimeError); + py::register_exception( + hw_except, "FrankaIncompatibleVersionException", PyExc_RuntimeError); + py::register_exception( + hw_except, "FrankaControlException", PyExc_RuntimeError); + py::register_exception( + hw_except, "FrankaCommandException", PyExc_RuntimeError); + py::register_exception( + hw_except, "FrankaRealtimeException", PyExc_RuntimeError); + py::register_exception( + hw_except, "FrankaInvalidOperationException", PyExc_RuntimeError); + +} diff --git a/extensions/realsense/pyproject.toml b/extensions/realsense/pyproject.toml new file mode 100644 index 00000000..ac3e8067 --- /dev/null +++ b/extensions/realsense/pyproject.toml @@ -0,0 +1,27 @@ +[build-system] +requires = ["setuptools"] +build-backend = "setuptools.build_meta" + +[project] +name = "rcs_realsense" +version = "0.4.0" +description="RCS realsense module" +dependencies = [ + "rcs==0.4.0", + "pyrealsense2~=2.55.1", +] +readme = "README.md" +maintainers = [ + { name = "Tobias Jülg", email = "tobias.juelg@utn.de" }, +] +authors = [ + { name = "Tobias Jülg", email = "tobias.juelg@utn.de" }, + { name = "Pierre Krack", email = "pierre.krack@utn.de" }, +] +requires-python = ">=3.10" +classifiers = [ + "Development Status :: 3 - Alpha", + "License :: OSI Approved :: GNU Affero General Public License v3 (AGPLv3)", + "Programming Language :: Python :: 3", +] + diff --git a/extensions/so101/pyproject.toml b/extensions/so101/pyproject.toml new file mode 100644 index 00000000..40ee8309 --- /dev/null +++ b/extensions/so101/pyproject.toml @@ -0,0 +1,27 @@ +[build-system] +requires = ["setuptools"] +build-backend = "setuptools.build_meta" + +[project] +name = "rcs_so101" +version = "0.4.0" +description="RCS SO101 module" +dependencies = [ + "rcs==0.4.0", + "lerobot @ git+https://github.com/huggingface/lerobot.git", +] +readme = "README.md" +maintainers = [ + { name = "Tobias Jülg", email = "tobias.juelg@utn.de" }, +] +authors = [ + { name = "Tobias Jülg", email = "tobias.juelg@utn.de" }, + { name = "Pierre Krack", email = "pierre.krack@utn.de" }, +] +requires-python = ">=3.10" +classifiers = [ + "Development Status :: 3 - Alpha", + "License :: OSI Approved :: GNU Affero General Public License v3 (AGPLv3)", + "Programming Language :: Python :: 3", +] + diff --git a/pyproject.toml b/pyproject.toml index 1f0c4e2a..b43cbf84 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -52,21 +52,13 @@ classifiers = [ # "mypy==1.4.1", # ] -[project.optional-dependencies] -so101 = [ - "lerobot @ git+https://github.com/huggingface/lerobot.git", -] -realsense = [ - "pyrealsense2~=2.55.1", -] - [tool.scikit-build] build.verbose = true build.targets = ["_core", "scenes"] logging.level = "INFO" build-dir = "build" -wheel.packages = ["python/rcs", "python/rcs_fr3", "python/rcs_so101", "python/rcs_realsense"] +wheel.packages = ["python/rcs"] install.components = ["python_package"] [tool.ruff] diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index c01d7a98..39d80e13 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,5 +1,4 @@ add_subdirectory(common) target_include_directories(common INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) -add_subdirectory(hw) add_subdirectory(sim) add_subdirectory(pybind) diff --git a/src/pybind/CMakeLists.txt b/src/pybind/CMakeLists.txt index ff76e118..3d1a6ec2 100644 --- a/src/pybind/CMakeLists.txt +++ b/src/pybind/CMakeLists.txt @@ -1,13 +1,10 @@ pybind11_add_module(_core MODULE rcs.cpp) -target_link_libraries(_core PRIVATE hw sim common) +target_link_libraries(_core PRIVATE sim common) target_compile_definitions(_core PRIVATE VERSION_INFO=${PROJECT_VERSION}) set_target_properties(_core PROPERTIES INSTALL_RPATH "$ORIGIN;$ORIGIN/../mujoco" INTERPROCEDURAL_OPTIMIZATION TRUE ) -set_target_properties(franka PROPERTIES - INSTALL_RPATH "$ORIGIN/../cmeel.prefix/lib" - INTERPROCEDURAL_OPTIMIZATION TRUE -) -install(TARGETS _core franka DESTINATION rcs COMPONENT python_package) +# in pip +install(TARGETS _core DESTINATION rcs COMPONENT python_package) diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 1aa6387f..2cb2136c 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -308,115 +308,6 @@ PYBIND11_MODULE(_core, m) { .def("reset", &rcs::common::Gripper::reset, py::call_guard()); - // HARDWARE MODULE - auto hw = m.def_submodule("hw", "hardware module"); - - py::class_(hw, "FR3State") - .def(py::init<>()); - py::class_(hw, "FR3Load") - .def(py::init<>()) - .def_readwrite("load_mass", &rcs::hw::FR3Load::load_mass) - .def_readwrite("f_x_cload", &rcs::hw::FR3Load::f_x_cload) - .def_readwrite("load_inertia", &rcs::hw::FR3Load::load_inertia); - - py::enum_(hw, "IKSolver") - .value("franka_ik", rcs::hw::IKSolver::franka_ik) - .value("rcs_ik", rcs::hw::IKSolver::rcs_ik) - .export_values(); - - py::class_(hw, "FR3Config") - .def(py::init<>()) - .def_readwrite("ik_solver", &rcs::hw::FR3Config::ik_solver) - .def_readwrite("speed_factor", &rcs::hw::FR3Config::speed_factor) - .def_readwrite("load_parameters", &rcs::hw::FR3Config::load_parameters) - .def_readwrite("nominal_end_effector_frame", - &rcs::hw::FR3Config::nominal_end_effector_frame) - .def_readwrite("world_to_robot", &rcs::hw::FR3Config::world_to_robot) - .def_readwrite("tcp_offset", &rcs::hw::FR3Config::tcp_offset) - .def_readwrite("async_control", &rcs::hw::FR3Config::async_control); - - py::class_(hw, "FHConfig") - .def(py::init<>()) - .def_readwrite("grasping_width", &rcs::hw::FHConfig::grasping_width) - .def_readwrite("speed", &rcs::hw::FHConfig::speed) - .def_readwrite("force", &rcs::hw::FHConfig::force) - .def_readwrite("epsilon_inner", &rcs::hw::FHConfig::epsilon_inner) - .def_readwrite("epsilon_outer", &rcs::hw::FHConfig::epsilon_outer) - .def_readwrite("async_control", &rcs::hw::FHConfig::async_control); - - py::class_(hw, "FHState") - .def(py::init<>()) - .def_readonly("width", &rcs::hw::FHState::width) - .def_readonly("is_grasped", &rcs::hw::FHState::is_grasped) - .def_readonly("is_moving", &rcs::hw::FHState::is_moving) - .def_readonly("bool_state", &rcs::hw::FHState::bool_state) - .def_readonly("last_commanded_width", - &rcs::hw::FHState::last_commanded_width) - .def_readonly("max_unnormalized_width", - &rcs::hw::FHState::max_unnormalized_width) - .def_readonly("temperature", &rcs::hw::FHState::temperature); - - py::class_>( - hw, "FR3") - .def(py::init>>(), - py::arg("ip"), py::arg("ik") = std::nullopt) - .def("set_parameters", &rcs::hw::FR3::set_parameters, py::arg("cfg")) - .def("get_parameters", &rcs::hw::FR3::get_parameters) - .def("get_state", &rcs::hw::FR3::get_state) - .def("set_default_robot_behavior", - &rcs::hw::FR3::set_default_robot_behavior) - .def("set_guiding_mode", &rcs::hw::FR3::set_guiding_mode, - py::arg("x") = true, py::arg("y") = true, py::arg("z") = true, - py::arg("roll") = true, py::arg("pitch") = true, - py::arg("yaw") = true, py::arg("elbow") = true) - .def("zero_torque_guiding", &rcs::hw::FR3::zero_torque_guiding) - .def("osc_set_cartesian_position", - &rcs::hw::FR3::osc_set_cartesian_position, - py::arg("desired_pos_EE_in_base_frame")) - .def("controller_set_joint_position", - &rcs::hw::FR3::controller_set_joint_position, py::arg("desired_q")) - .def("stop_control_thread", &rcs::hw::FR3::stop_control_thread) - .def("automatic_error_recovery", &rcs::hw::FR3::automatic_error_recovery) - .def("double_tap_robot_to_continue", - &rcs::hw::FR3::double_tap_robot_to_continue) - .def("set_cartesian_position_internal", - &rcs::hw::FR3::set_cartesian_position_ik, py::arg("pose")) - .def("set_cartesian_position_ik", - &rcs::hw::FR3::set_cartesian_position_internal, py::arg("pose"), - py::arg("max_time"), py::arg("elbow"), py::arg("max_force") = 5); - - py::class_>(hw, "FrankaHand") - .def(py::init(), - py::arg("ip"), py::arg("cfg")) - .def("get_parameters", &rcs::hw::FrankaHand::get_parameters) - .def("get_state", &rcs::hw::FrankaHand::get_state) - .def("set_parameters", &rcs::hw::FrankaHand::set_parameters, - py::arg("cfg")) - .def("is_grasped", &rcs::hw::FrankaHand::is_grasped) - .def("homing", &rcs::hw::FrankaHand::homing); - - auto hw_except = - hw.def_submodule("exceptions", "exceptions from the hardware module"); - py::register_exception(hw_except, "FrankaException", - PyExc_RuntimeError); - py::register_exception( - hw_except, "FrankaModelException", PyExc_RuntimeError); - py::register_exception( - hw_except, "FrankaNetworkException", PyExc_RuntimeError); - py::register_exception( - hw_except, "FrankaProtocolException", PyExc_RuntimeError); - py::register_exception( - hw_except, "FrankaIncompatibleVersionException", PyExc_RuntimeError); - py::register_exception( - hw_except, "FrankaControlException", PyExc_RuntimeError); - py::register_exception( - hw_except, "FrankaCommandException", PyExc_RuntimeError); - py::register_exception( - hw_except, "FrankaRealtimeException", PyExc_RuntimeError); - py::register_exception( - hw_except, "FrankaInvalidOperationException", PyExc_RuntimeError); // SIM MODULE auto sim = m.def_submodule("sim", "sim module"); From 173da5a1f988cc6a20b545724773b4aaa6bd45d0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 23 Jun 2025 13:33:19 +0200 Subject: [PATCH 09/32] refactor: removed hw module from bindings and python code --- python/rcs/__init__.py | 4 +- python/rcs/_core/__init__.pyi | 4 +- python/rcs/_core/hw/__init__.pyi | 140 ----------------------------- python/rcs/_core/hw/exceptions.pyi | 44 --------- src/pybind/rcs.cpp | 3 - 5 files changed, 4 insertions(+), 191 deletions(-) delete mode 100644 python/rcs/_core/hw/__init__.pyi delete mode 100644 python/rcs/_core/hw/exceptions.pyi diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py index de6ebb1d..72270331 100644 --- a/python/rcs/__init__.py +++ b/python/rcs/__init__.py @@ -5,7 +5,7 @@ from gymnasium import register from rcs import camera, envs, hand, sim -from rcs._core import __version__, common, hw +from rcs._core import __version__, common from rcs.envs.creators import FR3SimplePickUpSimEnvCreator # available mujoco scenes @@ -18,7 +18,7 @@ } # make submodules available -__all__ = ["__doc__", "__version__", "common", "hw", "sim", "camera", "scenes", "envs", "hand"] +__all__ = ["__doc__", "__version__", "common", "sim", "camera", "scenes", "envs", "hand"] # register gymnasium environments register( diff --git a/python/rcs/_core/__init__.pyi b/python/rcs/_core/__init__.pyi index 707576c1..567a9cf8 100644 --- a/python/rcs/_core/__init__.pyi +++ b/python/rcs/_core/__init__.pyi @@ -13,7 +13,7 @@ """ from __future__ import annotations -from . import common, hw, sim +from . import common, sim -__all__ = ["common", "hw", "sim"] +__all__ = ["common", "sim"] __version__: str = "0.4.0" diff --git a/python/rcs/_core/hw/__init__.pyi b/python/rcs/_core/hw/__init__.pyi deleted file mode 100644 index 995f33e4..00000000 --- a/python/rcs/_core/hw/__init__.pyi +++ /dev/null @@ -1,140 +0,0 @@ -# ATTENTION: auto generated from C++ code, use `make stubgen` to update! -""" -hardware module -""" -from __future__ import annotations - -import typing - -import numpy -import rcs._core.common - -from . import exceptions - -__all__ = [ - "FHConfig", - "FHState", - "FR3", - "FR3Config", - "FR3Load", - "FR3State", - "FrankaHand", - "IKSolver", - "exceptions", - "franka_ik", - "rcs_ik", -] - -class FHConfig(rcs._core.common.GripperConfig): - async_control: bool - epsilon_inner: float - epsilon_outer: float - force: float - grasping_width: float - speed: float - def __init__(self) -> None: ... - -class FHState(rcs._core.common.GripperState): - def __init__(self) -> None: ... - @property - def bool_state(self) -> bool: ... - @property - def is_grasped(self) -> bool: ... - @property - def is_moving(self) -> bool: ... - @property - def last_commanded_width(self) -> float: ... - @property - def max_unnormalized_width(self) -> float: ... - @property - def temperature(self) -> int: ... - @property - def width(self) -> float: ... - -class FR3(rcs._core.common.Robot): - def __init__(self, ip: str, ik: rcs._core.common.IK | None = None) -> None: ... - def automatic_error_recovery(self) -> None: ... - def controller_set_joint_position( - self, desired_q: numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]] - ) -> None: ... - def double_tap_robot_to_continue(self) -> None: ... - def get_parameters(self) -> FR3Config: ... - def get_state(self) -> FR3State: ... - def osc_set_cartesian_position(self, desired_pos_EE_in_base_frame: rcs._core.common.Pose) -> None: ... - def set_cartesian_position_ik( - self, pose: rcs._core.common.Pose, max_time: float, elbow: float | None, max_force: float | None = 5 - ) -> None: ... - def set_cartesian_position_internal(self, pose: rcs._core.common.Pose) -> None: ... - def set_default_robot_behavior(self) -> None: ... - def set_guiding_mode( - self, - x: bool = True, - y: bool = True, - z: bool = True, - roll: bool = True, - pitch: bool = True, - yaw: bool = True, - elbow: bool = True, - ) -> None: ... - def set_parameters(self, cfg: FR3Config) -> bool: ... - def stop_control_thread(self) -> None: ... - def zero_torque_guiding(self) -> None: ... - -class FR3Config(rcs._core.common.RobotConfig): - async_control: bool - ik_solver: IKSolver - load_parameters: FR3Load | None - nominal_end_effector_frame: rcs._core.common.Pose | None - speed_factor: float - tcp_offset: rcs._core.common.Pose - world_to_robot: rcs._core.common.Pose | None - def __init__(self) -> None: ... - -class FR3Load: - f_x_cload: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]] | None - load_inertia: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]] | None - load_mass: float - def __init__(self) -> None: ... - -class FR3State(rcs._core.common.RobotState): - def __init__(self) -> None: ... - -class FrankaHand(rcs._core.common.Gripper): - def __init__(self, ip: str, cfg: FHConfig) -> None: ... - def get_parameters(self) -> FHConfig: ... - def get_state(self) -> FHState: ... - def homing(self) -> bool: ... - def is_grasped(self) -> bool: ... - def set_parameters(self, cfg: FHConfig) -> bool: ... - -class IKSolver: - """ - Members: - - franka_ik - - rcs_ik - """ - - __members__: typing.ClassVar[ - dict[str, IKSolver] - ] # value = {'franka_ik': , 'rcs_ik': } - franka_ik: typing.ClassVar[IKSolver] # value = - rcs_ik: typing.ClassVar[IKSolver] # value = - def __eq__(self, other: typing.Any) -> bool: ... - def __getstate__(self) -> int: ... - def __hash__(self) -> int: ... - def __index__(self) -> int: ... - def __init__(self, value: int) -> None: ... - def __int__(self) -> int: ... - def __ne__(self, other: typing.Any) -> bool: ... - def __repr__(self) -> str: ... - def __setstate__(self, state: int) -> None: ... - def __str__(self) -> str: ... - @property - def name(self) -> str: ... - @property - def value(self) -> int: ... - -franka_ik: IKSolver # value = -rcs_ik: IKSolver # value = diff --git a/python/rcs/_core/hw/exceptions.pyi b/python/rcs/_core/hw/exceptions.pyi deleted file mode 100644 index 3576a88d..00000000 --- a/python/rcs/_core/hw/exceptions.pyi +++ /dev/null @@ -1,44 +0,0 @@ -# ATTENTION: auto generated from C++ code, use `make stubgen` to update! -""" -exceptions from the hardware module -""" -from __future__ import annotations - -__all__ = [ - "FrankaCommandException", - "FrankaControlException", - "FrankaException", - "FrankaIncompatibleVersionException", - "FrankaInvalidOperationException", - "FrankaModelException", - "FrankaNetworkException", - "FrankaProtocolException", - "FrankaRealtimeException", -] - -class FrankaCommandException(RuntimeError): - pass - -class FrankaControlException(RuntimeError): - pass - -class FrankaException(RuntimeError): - pass - -class FrankaIncompatibleVersionException(RuntimeError): - pass - -class FrankaInvalidOperationException(RuntimeError): - pass - -class FrankaModelException(RuntimeError): - pass - -class FrankaNetworkException(RuntimeError): - pass - -class FrankaProtocolException(RuntimeError): - pass - -class FrankaRealtimeException(RuntimeError): - pass diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 2cb2136c..daefbff1 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -2,9 +2,6 @@ #include #include #include -#include -#include -#include #include #include #include From 73d596678fd4a7ef6789c40e69dd7272749b52f2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 23 Jun 2025 16:37:39 +0200 Subject: [PATCH 10/32] refactor: build shared librcs --- pyproject.toml | 2 +- src/CMakeLists.txt | 2 +- src/common/CMakeLists.txt | 6 +++--- src/pybind/CMakeLists.txt | 4 ++-- src/sim/CMakeLists.txt | 2 +- 5 files changed, 8 insertions(+), 8 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index b43cbf84..098e2ce6 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -55,7 +55,7 @@ classifiers = [ [tool.scikit-build] build.verbose = true -build.targets = ["_core", "scenes"] +build.targets = ["_core", "scenes", "rcs"] logging.level = "INFO" build-dir = "build" wheel.packages = ["python/rcs"] diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 39d80e13..f661c5a4 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,4 +1,4 @@ add_subdirectory(common) -target_include_directories(common INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) +target_include_directories(rcs INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) add_subdirectory(sim) add_subdirectory(pybind) diff --git a/src/common/CMakeLists.txt b/src/common/CMakeLists.txt index 72e7733d..58a0c2ad 100644 --- a/src/common/CMakeLists.txt +++ b/src/common/CMakeLists.txt @@ -1,3 +1,3 @@ -add_library(common) -target_sources(common PRIVATE Pose.cpp Robot.cpp IK.cpp utils.cpp) -target_link_libraries(common PUBLIC Eigen3::Eigen mdl) +add_library(rcs SHARED) +target_sources(rcs PRIVATE Pose.cpp Robot.cpp IK.cpp utils.cpp) +target_link_libraries(rcs PUBLIC Eigen3::Eigen mdl) diff --git a/src/pybind/CMakeLists.txt b/src/pybind/CMakeLists.txt index 3d1a6ec2..5a1678ad 100644 --- a/src/pybind/CMakeLists.txt +++ b/src/pybind/CMakeLists.txt @@ -1,5 +1,5 @@ pybind11_add_module(_core MODULE rcs.cpp) -target_link_libraries(_core PRIVATE sim common) +target_link_libraries(_core PRIVATE sim rcs) target_compile_definitions(_core PRIVATE VERSION_INFO=${PROJECT_VERSION}) set_target_properties(_core PROPERTIES @@ -7,4 +7,4 @@ set_target_properties(_core PROPERTIES INTERPROCEDURAL_OPTIMIZATION TRUE ) # in pip -install(TARGETS _core DESTINATION rcs COMPONENT python_package) +install(TARGETS _core rcs DESTINATION rcs COMPONENT python_package) diff --git a/src/sim/CMakeLists.txt b/src/sim/CMakeLists.txt index f04d5639..b6b197f7 100644 --- a/src/sim/CMakeLists.txt +++ b/src/sim/CMakeLists.txt @@ -2,7 +2,7 @@ add_library(sim) target_sources(sim PRIVATE sim.cpp SimRobot.cpp renderer.cpp camera.cpp SimGripper.cpp gui_server.cpp gui_client.cpp ) -target_link_libraries(sim PUBLIC common MuJoCo::MuJoCo) +target_link_libraries(sim PUBLIC rcs MuJoCo::MuJoCo) #add_executable(test_sim test.cpp sim.cpp) #target_link_libraries(test_sim PRIVATE sim MuJoCo::MuJoCo common egl) From f9ccfe1cd6b8f3fd5dfcd58196769f9e389ebb04 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 23 Jun 2025 16:58:22 +0200 Subject: [PATCH 11/32] feat: added findrcs.cmake to fr3 and used rcs instead of common --- extensions/fr3/CMakeLists.txt | 4 +- extensions/fr3/cmake/Findrcs.cmake | 45 +++++++++++++++++++ extensions/fr3/pyproject.toml | 4 +- extensions/fr3/src/CMakeLists.txt | 2 +- extensions/fr3/src/hw/CMakeLists.txt | 2 +- extensions/fr3/src/pybind/CMakeLists.txt | 4 +- extensions/fr3/src/rcs_fr3/__main__.py | 1 - .../realsense/src/rcs_realsense/__main__.py | 2 +- 8 files changed, 54 insertions(+), 10 deletions(-) create mode 100644 extensions/fr3/cmake/Findrcs.cmake diff --git a/extensions/fr3/CMakeLists.txt b/extensions/fr3/CMakeLists.txt index 28d7917e..32220287 100644 --- a/extensions/fr3/CMakeLists.txt +++ b/extensions/fr3/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.19) project( - rcs + rcs_fr3 LANGUAGES C CXX VERSION 0.4.0 DESCRIPTION "RCS Libfranka integration" @@ -45,7 +45,7 @@ include(FetchContent) find_package(Eigen3 REQUIRED) find_package(Python3 COMPONENTS Interpreter Development REQUIRED) -find_package(MuJoCo REQUIRED) +find_package(rcs REQUIRED) FetchContent_Declare( libfranka diff --git a/extensions/fr3/cmake/Findrcs.cmake b/extensions/fr3/cmake/Findrcs.cmake new file mode 100644 index 00000000..cb67dab5 --- /dev/null +++ b/extensions/fr3/cmake/Findrcs.cmake @@ -0,0 +1,45 @@ +if (NOT rcs_FOUND) + if (NOT Python3_FOUND) + set(rcs_FOUND FALSE) + if (rcs_FIND_REQUIRED) + message(FATAL_ERROR "Could not find rcs. Please install rcs using pip.") + endif() + return() + endif() + + # Check if the include directory exists + cmake_path(APPEND Python3_SITELIB rcs include OUTPUT_VARIABLE rcs_INCLUDE_DIRS) + if (NOT EXISTS ${rcs_INCLUDE_DIRS}) + set(rcs_FOUND FALSE) + if (rcs_FIND_REQUIRED) + message(FATAL_ERROR "Could not find rcs. Please install rcs using pip.") + endif() + return() + endif() + + # Check if the library file exists + cmake_path(APPEND Python3_SITELIB rcs lib librcs.so OUTPUT_VARIABLE rcs_library_path) + if (NOT EXISTS ${rcs_library_path}) + set(rcs_FOUND FALSE) + if (rcs_FIND_REQUIRED) + message(FATAL_ERROR "Could not find rcs. Please install rcs using pip.") + endif() + return() + endif() + + # Extract version from the library filename + file(GLOB rcs_dist_info "${Python3_SITELIB}/rcs-*.dist-info") + cmake_path(GET rcs_dist_info FILENAME rcs_library_filename) + string(REPLACE "rcs-" "" rcs_VERSION "${rcs_library_filename}") + string(REPLACE ".dist-info" "" rcs_VERSION "${rcs_VERSION}") + + # Create the imported target + add_library(rcs::rcs SHARED IMPORTED) + target_include_directories(rcs::rcs INTERFACE ${rcs_INCLUDE_DIRS}) + set_target_properties( + rcs::rcs + PROPERTIES + IMPORTED_LOCATION "${rcs_library_path}" + ) + set(rcs_FOUND TRUE) +endif() \ No newline at end of file diff --git a/extensions/fr3/pyproject.toml b/extensions/fr3/pyproject.toml index 9cbdcb90..3eae2aa4 100644 --- a/extensions/fr3/pyproject.toml +++ b/extensions/fr3/pyproject.toml @@ -10,7 +10,7 @@ dependencies = [ # NOTE: when changing the pin version, also change it in requirements_dev.txt "pin==2.7.0", ] -readme = "README.md" +readme = "../../README.md" maintainers = [ { name = "Tobias Jülg", email = "tobias.juelg@utn.de" }, ] @@ -19,7 +19,7 @@ authors = [ { name = "Pierre Krack", email = "pierre.krack@utn.de" }, ] requires-python = ">=3.10" -license = {file = "LICENSE"} +license = {file = "../../LICENSE"} classifiers = [ "Development Status :: 3 - Alpha", "License :: OSI Approved :: GNU Affero General Public License v3 (AGPLv3)", diff --git a/extensions/fr3/src/CMakeLists.txt b/extensions/fr3/src/CMakeLists.txt index b61f8ab9..1bbcc258 100644 --- a/extensions/fr3/src/CMakeLists.txt +++ b/extensions/fr3/src/CMakeLists.txt @@ -1,3 +1,3 @@ -target_include_directories(common INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) +# target_include_directories(rcs INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) add_subdirectory(hw) add_subdirectory(pybind) diff --git a/extensions/fr3/src/hw/CMakeLists.txt b/extensions/fr3/src/hw/CMakeLists.txt index a8669235..8d8f438e 100644 --- a/extensions/fr3/src/hw/CMakeLists.txt +++ b/extensions/fr3/src/hw/CMakeLists.txt @@ -2,7 +2,7 @@ add_library(hw) target_sources(hw PRIVATE FR3.cpp FrankaHand.cpp FR3MotionGenerator.cpp) target_link_libraries( hw - PUBLIC franka common + PUBLIC franka rcs PRIVATE math ) diff --git a/extensions/fr3/src/pybind/CMakeLists.txt b/extensions/fr3/src/pybind/CMakeLists.txt index d6ad1efc..01556292 100644 --- a/extensions/fr3/src/pybind/CMakeLists.txt +++ b/extensions/fr3/src/pybind/CMakeLists.txt @@ -1,5 +1,5 @@ pybind11_add_module(_core MODULE rcs.cpp) -target_link_libraries(_core PRIVATE hw common) +target_link_libraries(_core PRIVATE hw rcs) target_compile_definitions(_core PRIVATE VERSION_INFO=${PROJECT_VERSION}) set_target_properties(franka PROPERTIES @@ -7,4 +7,4 @@ set_target_properties(franka PROPERTIES INTERPROCEDURAL_OPTIMIZATION TRUE ) # in pip -install(TARGETS _core franka DESTINATION rcs COMPONENT python_package) +install(TARGETS _core franka DESTINATION rcs_fr3 COMPONENT python_package) diff --git a/extensions/fr3/src/rcs_fr3/__main__.py b/extensions/fr3/src/rcs_fr3/__main__.py index be0d8462..f77613b0 100644 --- a/extensions/fr3/src/rcs_fr3/__main__.py +++ b/extensions/fr3/src/rcs_fr3/__main__.py @@ -11,7 +11,6 @@ # FR3 CLI fr3_app = typer.Typer( - name="rcs.fr3", help=( "Commands to control a Franka Research 3 in RCS. " "This includes tools that you would usually do with Franka's Desk interface." diff --git a/extensions/realsense/src/rcs_realsense/__main__.py b/extensions/realsense/src/rcs_realsense/__main__.py index b803d5d0..ff342679 100644 --- a/extensions/realsense/src/rcs_realsense/__main__.py +++ b/extensions/realsense/src/rcs_realsense/__main__.py @@ -5,7 +5,7 @@ from rcs_realsense.camera import RealSenseCameraSet logger = logging.getLogger(__name__) -realsense_app = typer.Typer(help="CLI tool for the intel realsense module of rcs.", name="rcs.realsense") +realsense_app = typer.Typer(help="CLI tool for the intel realsense module of rcs.") @realsense_app.command() From 73eefda7200702e4917e2763d0e528ff92e13f4c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 23 Jun 2025 18:19:56 +0200 Subject: [PATCH 12/32] refactor: rcs headers in own include folder, header copy and fr3 rcs renaming --- extensions/fr3/CMakeLists.txt | 2 +- extensions/fr3/cmake/Findpinocchio.cmake | 45 +++++++++++++++++++ extensions/fr3/cmake/Findrcs.cmake | 9 ++-- extensions/fr3/src/hw/FR3.cpp | 2 +- extensions/fr3/src/hw/FR3.h | 10 ++--- extensions/fr3/src/hw/FR3MotionGenerator.h | 4 +- extensions/fr3/src/hw/FrankaHand.h | 2 +- extensions/fr3/src/hw/main.cpp | 2 +- extensions/fr3/src/pybind/rcs.cpp | 12 ++--- {src/common => include/rcs}/Camera.h | 0 {src/common => include/rcs}/IK.h | 0 .../rcs}/LinearPoseTrajInterpolator.h | 0 {src/common => include/rcs}/Pose.h | 0 {src/common => include/rcs}/Robot.h | 0 {src/common => include/rcs}/utils.h | 0 src/CMakeLists.txt | 2 +- src/pybind/CMakeLists.txt | 5 +++ src/pybind/rcs.cpp | 8 ++-- src/{common => rcs}/CMakeLists.txt | 1 + src/{common => rcs}/IK.cpp | 2 +- src/{common => rcs}/Pose.cpp | 2 +- src/{common => rcs}/Robot.cpp | 2 +- src/{common => rcs}/utils.cpp | 2 +- src/sim/CMakeLists.txt | 2 +- src/sim/SimGripper.h | 2 +- src/sim/SimRobot.cpp | 2 +- src/sim/SimRobot.h | 8 ++-- src/sim/camera.cpp | 2 +- src/sim/camera.h | 6 +-- src/sim/renderer.cpp | 2 +- src/sim/test.cpp | 2 +- 31 files changed, 93 insertions(+), 45 deletions(-) create mode 100644 extensions/fr3/cmake/Findpinocchio.cmake rename {src/common => include/rcs}/Camera.h (100%) rename {src/common => include/rcs}/IK.h (100%) rename {src/common => include/rcs}/LinearPoseTrajInterpolator.h (100%) rename {src/common => include/rcs}/Pose.h (100%) rename {src/common => include/rcs}/Robot.h (100%) rename {src/common => include/rcs}/utils.h (100%) rename src/{common => rcs}/CMakeLists.txt (67%) rename src/{common => rcs}/IK.cpp (98%) rename src/{common => rcs}/Pose.cpp (99%) rename src/{common => rcs}/Robot.cpp (95%) rename src/{common => rcs}/utils.cpp (96%) diff --git a/extensions/fr3/CMakeLists.txt b/extensions/fr3/CMakeLists.txt index 32220287..f0ef83c0 100644 --- a/extensions/fr3/CMakeLists.txt +++ b/extensions/fr3/CMakeLists.txt @@ -45,7 +45,7 @@ include(FetchContent) find_package(Eigen3 REQUIRED) find_package(Python3 COMPONENTS Interpreter Development REQUIRED) -find_package(rcs REQUIRED) +# find_package(rcs REQUIRED) FetchContent_Declare( libfranka diff --git a/extensions/fr3/cmake/Findpinocchio.cmake b/extensions/fr3/cmake/Findpinocchio.cmake new file mode 100644 index 00000000..d1da5ba9 --- /dev/null +++ b/extensions/fr3/cmake/Findpinocchio.cmake @@ -0,0 +1,45 @@ +if (NOT pinocchio_FOUND) + if (NOT Python3_FOUND) + set(pinocchio_FOUND FALSE) + if (pinocchio_FIND_REQUIRED) + message(FATAL_ERROR "Could not find pinocchio. Please install pinocchio using pip.") + endif() + return() + endif() + + # Check if the include directory exists + cmake_path(APPEND Python3_SITELIB cmeel.prefix include OUTPUT_VARIABLE pinocchio_INCLUDE_DIRS) + if (NOT EXISTS ${pinocchio_INCLUDE_DIRS}) + set(pinocchio_FOUND FALSE) + if (pinocchio_FIND_REQUIRED) + message(FATAL_ERROR "Could not find pinocchio. Please install pinocchio using pip.") + endif() + return() + endif() + + # Check if the library file exists + cmake_path(APPEND Python3_SITELIB cmeel.prefix lib libpinocchio.so OUTPUT_VARIABLE pinocchio_library_path) + if (NOT EXISTS ${pinocchio_library_path}) + set(pinocchio_FOUND FALSE) + if (pinocchio_FIND_REQUIRED) + message(FATAL_ERROR "Could not find pinocchio. Please install pinocchio using pip.") + endif() + return() + endif() + + # Extract version from the library filename + file(GLOB pinocchio_dist_info "${Python3_SITELIB}/pin-*.dist-info") + cmake_path(GET pinocchio_dist_info FILENAME pinocchio_library_filename) + string(REPLACE "pin-" "" pinocchio_VERSION "${pinocchio_library_filename}") + string(REPLACE ".dist-info" "" pinocchio_VERSION "${pinocchio_VERSION}") + + # Create the imported target + add_library(pinocchio::pinocchio SHARED IMPORTED) + target_include_directories(pinocchio::pinocchio INTERFACE ${pinocchio_INCLUDE_DIRS}) + set_target_properties( + pinocchio::pinocchio + PROPERTIES + IMPORTED_LOCATION "${pinocchio_library_path}" + ) + set(pinocchio_FOUND TRUE) +endif() diff --git a/extensions/fr3/cmake/Findrcs.cmake b/extensions/fr3/cmake/Findrcs.cmake index cb67dab5..19fedcd7 100644 --- a/extensions/fr3/cmake/Findrcs.cmake +++ b/extensions/fr3/cmake/Findrcs.cmake @@ -2,7 +2,7 @@ if (NOT rcs_FOUND) if (NOT Python3_FOUND) set(rcs_FOUND FALSE) if (rcs_FIND_REQUIRED) - message(FATAL_ERROR "Could not find rcs. Please install rcs using pip.") + message(FATAL_ERROR "Could not find rcs. Please install rcs using pip 1.") endif() return() endif() @@ -12,17 +12,18 @@ if (NOT rcs_FOUND) if (NOT EXISTS ${rcs_INCLUDE_DIRS}) set(rcs_FOUND FALSE) if (rcs_FIND_REQUIRED) - message(FATAL_ERROR "Could not find rcs. Please install rcs using pip.") + message(FATAL_ERROR "Could not find rcs. Please install rcs using pip 2.") endif() return() endif() # Check if the library file exists - cmake_path(APPEND Python3_SITELIB rcs lib librcs.so OUTPUT_VARIABLE rcs_library_path) + cmake_path(APPEND Python3_SITELIB rcs OUTPUT_VARIABLE rcs_library_path) + file(GLOB rcs_library_path "${rcs_library_path}/librcs.so") if (NOT EXISTS ${rcs_library_path}) set(rcs_FOUND FALSE) if (rcs_FIND_REQUIRED) - message(FATAL_ERROR "Could not find rcs. Please install rcs using pip.") + message(FATAL_ERROR "Could not find rcs. Please install rcs using pip 3.") endif() return() endif() diff --git a/extensions/fr3/src/hw/FR3.cpp b/extensions/fr3/src/hw/FR3.cpp index e836ff9a..256d7dbf 100644 --- a/extensions/fr3/src/hw/FR3.cpp +++ b/extensions/fr3/src/hw/FR3.cpp @@ -15,7 +15,7 @@ #include #include "FR3MotionGenerator.h" -#include "common/Pose.h" +#include "rcs/Pose.h" namespace rcs { namespace hw { diff --git a/extensions/fr3/src/hw/FR3.h b/extensions/fr3/src/hw/FR3.h index 249235f4..7fdae8f9 100644 --- a/extensions/fr3/src/hw/FR3.h +++ b/extensions/fr3/src/hw/FR3.h @@ -1,11 +1,11 @@ #ifndef RCS_FR3_H #define RCS_FR3_H -#include -#include -#include -#include -#include +#include "rcs/IK.h" +#include "rcs/LinearPoseTrajInterpolator.h" +#include "rcs/Pose.h" +#include "rcs/Robot.h" +#include "rcs/utils.h" #include #include diff --git a/extensions/fr3/src/hw/FR3MotionGenerator.h b/extensions/fr3/src/hw/FR3MotionGenerator.h index 9777aabe..096e1291 100644 --- a/extensions/fr3/src/hw/FR3MotionGenerator.h +++ b/extensions/fr3/src/hw/FR3MotionGenerator.h @@ -1,8 +1,8 @@ #ifndef RCS_MOTION_GENERATOR_H #define RCS_MOTION_GENERATOR_H -#include -#include +#include "rcs/Robot.h" +#include "rcs/utils.h" #include #include #include diff --git a/extensions/fr3/src/hw/FrankaHand.h b/extensions/fr3/src/hw/FrankaHand.h index 95e24bd2..dd9d1943 100644 --- a/extensions/fr3/src/hw/FrankaHand.h +++ b/extensions/fr3/src/hw/FrankaHand.h @@ -1,7 +1,7 @@ #ifndef RCS_FRANKA_HAND_H #define RCS_FRANKA_HAND_H -#include +#include "rcs/Robot.h" #include #include diff --git a/extensions/fr3/src/hw/main.cpp b/extensions/fr3/src/hw/main.cpp index 6c815142..05fd5867 100644 --- a/extensions/fr3/src/hw/main.cpp +++ b/extensions/fr3/src/hw/main.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include #include diff --git a/extensions/fr3/src/pybind/rcs.cpp b/extensions/fr3/src/pybind/rcs.cpp index f1fe862e..9a4c772c 100644 --- a/extensions/fr3/src/pybind/rcs.cpp +++ b/extensions/fr3/src/pybind/rcs.cpp @@ -1,7 +1,7 @@ -#include -#include -#include -#include +#include "rcs/IK.h" +#include "rcs/Pose.h" +#include "rcs/Robot.h" +#include "rcs/utils.h" #include #include #include @@ -10,10 +10,6 @@ #include #include #include -#include -#include -#include -#include #include diff --git a/src/common/Camera.h b/include/rcs/Camera.h similarity index 100% rename from src/common/Camera.h rename to include/rcs/Camera.h diff --git a/src/common/IK.h b/include/rcs/IK.h similarity index 100% rename from src/common/IK.h rename to include/rcs/IK.h diff --git a/src/common/LinearPoseTrajInterpolator.h b/include/rcs/LinearPoseTrajInterpolator.h similarity index 100% rename from src/common/LinearPoseTrajInterpolator.h rename to include/rcs/LinearPoseTrajInterpolator.h diff --git a/src/common/Pose.h b/include/rcs/Pose.h similarity index 100% rename from src/common/Pose.h rename to include/rcs/Pose.h diff --git a/src/common/Robot.h b/include/rcs/Robot.h similarity index 100% rename from src/common/Robot.h rename to include/rcs/Robot.h diff --git a/src/common/utils.h b/include/rcs/utils.h similarity index 100% rename from src/common/utils.h rename to include/rcs/utils.h diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index f661c5a4..38c112ab 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,4 +1,4 @@ -add_subdirectory(common) +add_subdirectory(rcs) target_include_directories(rcs INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) add_subdirectory(sim) add_subdirectory(pybind) diff --git a/src/pybind/CMakeLists.txt b/src/pybind/CMakeLists.txt index 5a1678ad..c0758b20 100644 --- a/src/pybind/CMakeLists.txt +++ b/src/pybind/CMakeLists.txt @@ -8,3 +8,8 @@ set_target_properties(_core PROPERTIES ) # in pip install(TARGETS _core rcs DESTINATION rcs COMPONENT python_package) +install( + DIRECTORY ${CMAKE_SOURCE_DIR}/include/rcs + DESTINATION rcs/include + COMPONENT python_package +) diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index daefbff1..802d1848 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -1,7 +1,7 @@ -#include -#include -#include -#include +#include "rcs/IK.h" +#include "rcs/Pose.h" +#include "rcs/Robot.h" +#include "rcs/utils.h" #include #include #include diff --git a/src/common/CMakeLists.txt b/src/rcs/CMakeLists.txt similarity index 67% rename from src/common/CMakeLists.txt rename to src/rcs/CMakeLists.txt index 58a0c2ad..831e8a5b 100644 --- a/src/common/CMakeLists.txt +++ b/src/rcs/CMakeLists.txt @@ -1,3 +1,4 @@ add_library(rcs SHARED) +target_include_directories(rcs PUBLIC ${CMAKE_SOURCE_DIR}/include) target_sources(rcs PRIVATE Pose.cpp Robot.cpp IK.cpp utils.cpp) target_link_libraries(rcs PUBLIC Eigen3::Eigen mdl) diff --git a/src/common/IK.cpp b/src/rcs/IK.cpp similarity index 98% rename from src/common/IK.cpp rename to src/rcs/IK.cpp index e100024d..f67a1a37 100644 --- a/src/common/IK.cpp +++ b/src/rcs/IK.cpp @@ -1,4 +1,4 @@ -#include "IK.h" +#include "rcs/IK.h" #include #include diff --git a/src/common/Pose.cpp b/src/rcs/Pose.cpp similarity index 99% rename from src/common/Pose.cpp rename to src/rcs/Pose.cpp index 9175670c..4a5176aa 100644 --- a/src/common/Pose.cpp +++ b/src/rcs/Pose.cpp @@ -1,4 +1,4 @@ -#include "Pose.h" +#include "rcs/Pose.h" namespace rcs { namespace common { diff --git a/src/common/Robot.cpp b/src/rcs/Robot.cpp similarity index 95% rename from src/common/Robot.cpp rename to src/rcs/Robot.cpp index 33f9bed9..ec628e0e 100644 --- a/src/common/Robot.cpp +++ b/src/rcs/Robot.cpp @@ -1,4 +1,4 @@ -#include "Robot.h" +#include "rcs/Robot.h" namespace rcs { namespace common { diff --git a/src/common/utils.cpp b/src/rcs/utils.cpp similarity index 96% rename from src/common/utils.cpp rename to src/rcs/utils.cpp index 408735fc..d348375b 100644 --- a/src/common/utils.cpp +++ b/src/rcs/utils.cpp @@ -1,4 +1,4 @@ -#include "utils.h" +#include "rcs/utils.h" #include diff --git a/src/sim/CMakeLists.txt b/src/sim/CMakeLists.txt index b6b197f7..7a6129c8 100644 --- a/src/sim/CMakeLists.txt +++ b/src/sim/CMakeLists.txt @@ -5,4 +5,4 @@ target_sources(sim target_link_libraries(sim PUBLIC rcs MuJoCo::MuJoCo) #add_executable(test_sim test.cpp sim.cpp) -#target_link_libraries(test_sim PRIVATE sim MuJoCo::MuJoCo common egl) +#target_link_libraries(test_sim PRIVATE sim MuJoCo::MuJoCo rcs egl) diff --git a/src/sim/SimGripper.h b/src/sim/SimGripper.h index 9d4ceb54..ff25394c 100644 --- a/src/sim/SimGripper.h +++ b/src/sim/SimGripper.h @@ -1,7 +1,7 @@ #ifndef RCS_FRANKA_HAND_SIM_H #define RCS_FRANKA_HAND_SIM_H -#include +#include "rcs/Robot.h" #include #include diff --git a/src/sim/SimRobot.cpp b/src/sim/SimRobot.cpp index 50ac7817..cd8dc901 100644 --- a/src/sim/SimRobot.cpp +++ b/src/sim/SimRobot.cpp @@ -14,7 +14,7 @@ #include #include -#include "common/Robot.h" +#include "rcs/Robot.h" #include "mujoco/mjdata.h" #include "mujoco/mjmodel.h" #include "mujoco/mujoco.h" diff --git a/src/sim/SimRobot.h b/src/sim/SimRobot.h index 1c286a09..32fd894f 100644 --- a/src/sim/SimRobot.h +++ b/src/sim/SimRobot.h @@ -1,9 +1,9 @@ #ifndef RCS_FR3SIM_H #define RCS_FR3SIM_H -#include -#include -#include -#include +#include +#include +#include +#include #include #include "sim/sim.h" diff --git a/src/sim/camera.cpp b/src/sim/camera.cpp index d7de4ed3..8dde996e 100644 --- a/src/sim/camera.cpp +++ b/src/sim/camera.cpp @@ -13,7 +13,7 @@ #include #include -#include "common/Robot.h" +#include "rcs/Robot.h" #include "mujoco/mjdata.h" #include "mujoco/mjmodel.h" #include "mujoco/mujoco.h" diff --git a/src/sim/camera.h b/src/sim/camera.h index 0863e599..d1e8214d 100644 --- a/src/sim/camera.h +++ b/src/sim/camera.h @@ -1,7 +1,7 @@ #ifndef RCS_CAMSIM_H #define RCS_CAMSIM_H -#include -#include +#include +#include #include #include @@ -10,7 +10,7 @@ #include #include -#include "common/Camera.h" +#include "rcs/Camera.h" #include "sim/sim.h" namespace rcs { diff --git a/src/sim/renderer.cpp b/src/sim/renderer.cpp index c37ad256..8b5e9e96 100644 --- a/src/sim/renderer.cpp +++ b/src/sim/renderer.cpp @@ -1,4 +1,4 @@ -#include +#include "rcs/utils.h" #include #include diff --git a/src/sim/test.cpp b/src/sim/test.cpp index 5e7995af..9add48c2 100644 --- a/src/sim/test.cpp +++ b/src/sim/test.cpp @@ -8,7 +8,7 @@ #include #include -#include "common/Pose.h" +#include "rcs/Pose.h" #include "sim.h" #include "sim/SimRobot.h" From 0996783cbfb01bb69017139e8b8b3555c0f387e5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 23 Jun 2025 21:28:04 +0200 Subject: [PATCH 13/32] fix: rcs and rcs_fr3 compile cmake magic... --- extensions/fr3/CMakeLists.txt | 10 ++++++++-- extensions/fr3/cmake/Findrcs.cmake | 20 ++++++++++---------- extensions/fr3/src/CMakeLists.txt | 2 +- extensions/fr3/src/pybind/CMakeLists.txt | 6 +++++- extensions/fr3/src/rcs_fr3/envs.py | 2 +- extensions/fr3/src/rcs_fr3/utils.py | 3 ++- 6 files changed, 27 insertions(+), 16 deletions(-) diff --git a/extensions/fr3/CMakeLists.txt b/extensions/fr3/CMakeLists.txt index f0ef83c0..62bb2ad4 100644 --- a/extensions/fr3/CMakeLists.txt +++ b/extensions/fr3/CMakeLists.txt @@ -45,8 +45,14 @@ include(FetchContent) find_package(Eigen3 REQUIRED) find_package(Python3 COMPONENTS Interpreter Development REQUIRED) -# find_package(rcs REQUIRED) +find_package(rcs REQUIRED) +FetchContent_Declare(rl + GIT_REPOSITORY https://github.com/roboticslibrary/rl.git + GIT_TAG 0b3797215345a1d37903634095361233d190b2e6 + GIT_PROGRESS TRUE + EXCLUDE_FROM_ALL +) FetchContent_Declare( libfranka GIT_REPOSITORY https://github.com/frankaemika/libfranka.git @@ -61,6 +67,6 @@ FetchContent_Declare(pybind11 EXCLUDE_FROM_ALL ) -FetchContent_MakeAvailable(libfranka pybind11) +FetchContent_MakeAvailable(rl libfranka pybind11) add_subdirectory(src) diff --git a/extensions/fr3/cmake/Findrcs.cmake b/extensions/fr3/cmake/Findrcs.cmake index 19fedcd7..08456ccc 100644 --- a/extensions/fr3/cmake/Findrcs.cmake +++ b/extensions/fr3/cmake/Findrcs.cmake @@ -2,7 +2,7 @@ if (NOT rcs_FOUND) if (NOT Python3_FOUND) set(rcs_FOUND FALSE) if (rcs_FIND_REQUIRED) - message(FATAL_ERROR "Could not find rcs. Please install rcs using pip 1.") + message(FATAL_ERROR "Could not find rcs. Please install rcs using pip.") endif() return() endif() @@ -12,7 +12,7 @@ if (NOT rcs_FOUND) if (NOT EXISTS ${rcs_INCLUDE_DIRS}) set(rcs_FOUND FALSE) if (rcs_FIND_REQUIRED) - message(FATAL_ERROR "Could not find rcs. Please install rcs using pip 2.") + message(FATAL_ERROR "Could not find rcs. Please install rcs using pip.") endif() return() endif() @@ -23,22 +23,22 @@ if (NOT rcs_FOUND) if (NOT EXISTS ${rcs_library_path}) set(rcs_FOUND FALSE) if (rcs_FIND_REQUIRED) - message(FATAL_ERROR "Could not find rcs. Please install rcs using pip 3.") + message(FATAL_ERROR "Could not find rcs. Please install rcs using pip.") endif() return() endif() # Extract version from the library filename - file(GLOB rcs_dist_info "${Python3_SITELIB}/rcs-*.dist-info") - cmake_path(GET rcs_dist_info FILENAME rcs_library_filename) - string(REPLACE "rcs-" "" rcs_VERSION "${rcs_library_filename}") - string(REPLACE ".dist-info" "" rcs_VERSION "${rcs_VERSION}") + # file(GLOB rcs_dist_info "${Python3_SITELIB}/rcs-*.dist-info") + # cmake_path(GET rcs_dist_info FILENAME rcs_library_filename) + # string(REPLACE "rcs-" "" rcs_VERSION "${rcs_library_filename}") + # string(REPLACE ".dist-info" "" rcs_VERSION "${rcs_VERSION}") # Create the imported target - add_library(rcs::rcs SHARED IMPORTED) - target_include_directories(rcs::rcs INTERFACE ${rcs_INCLUDE_DIRS}) + add_library(rcs SHARED IMPORTED) + target_include_directories(rcs INTERFACE ${rcs_INCLUDE_DIRS}) set_target_properties( - rcs::rcs + rcs PROPERTIES IMPORTED_LOCATION "${rcs_library_path}" ) diff --git a/extensions/fr3/src/CMakeLists.txt b/extensions/fr3/src/CMakeLists.txt index 1bbcc258..9f24288c 100644 --- a/extensions/fr3/src/CMakeLists.txt +++ b/extensions/fr3/src/CMakeLists.txt @@ -1,3 +1,3 @@ -# target_include_directories(rcs INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) +target_include_directories(rcs INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) add_subdirectory(hw) add_subdirectory(pybind) diff --git a/extensions/fr3/src/pybind/CMakeLists.txt b/extensions/fr3/src/pybind/CMakeLists.txt index 01556292..f91fd2b7 100644 --- a/extensions/fr3/src/pybind/CMakeLists.txt +++ b/extensions/fr3/src/pybind/CMakeLists.txt @@ -1,10 +1,14 @@ pybind11_add_module(_core MODULE rcs.cpp) -target_link_libraries(_core PRIVATE hw rcs) +target_link_libraries(_core PRIVATE hw rcs mdl) target_compile_definitions(_core PRIVATE VERSION_INFO=${PROJECT_VERSION}) set_target_properties(franka PROPERTIES INSTALL_RPATH "$ORIGIN/../cmeel.prefix/lib" INTERPROCEDURAL_OPTIMIZATION TRUE ) +set_target_properties(_core PROPERTIES + INSTALL_RPATH "$ORIGIN;$ORIGIN/../rcs_fr3" + INTERPROCEDURAL_OPTIMIZATION TRUE +) # in pip install(TARGETS _core franka DESTINATION rcs_fr3 COMPONENT python_package) diff --git a/extensions/fr3/src/rcs_fr3/envs.py b/extensions/fr3/src/rcs_fr3/envs.py index 12ea76b3..3f1472bb 100644 --- a/extensions/fr3/src/rcs_fr3/envs.py +++ b/extensions/fr3/src/rcs_fr3/envs.py @@ -2,7 +2,7 @@ from typing import Any, SupportsFloat, cast import gymnasium as gym -from rcs import hw +from rcs_fr3._core import hw from rcs.envs.base import RobotEnv _logger = logging.getLogger(__name__) diff --git a/extensions/fr3/src/rcs_fr3/utils.py b/extensions/fr3/src/rcs_fr3/utils.py index 6c8fa7dc..6fb5a47c 100644 --- a/extensions/fr3/src/rcs_fr3/utils.py +++ b/extensions/fr3/src/rcs_fr3/utils.py @@ -1,4 +1,5 @@ -from rcs import common, hw +from rcs import common +from rcs_fr3._core import hw def default_fr3_hw_robot_cfg(async_control: bool = False) -> hw.FR3Config: From f22dcd201c2094d5a62d31db5018367893d1d6eb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 23 Jun 2025 21:47:26 +0200 Subject: [PATCH 14/32] refactor: rename extensions with rcs prefix --- extensions/{fr3 => rcs_fr3}/CMakeLists.txt | 0 extensions/{fr3 => rcs_fr3}/cmake/Findpinocchio.cmake | 0 extensions/{fr3 => rcs_fr3}/cmake/Findrcs.cmake | 0 extensions/{fr3 => rcs_fr3}/pyproject.toml | 0 extensions/{fr3 => rcs_fr3}/src/CMakeLists.txt | 0 extensions/{fr3 => rcs_fr3}/src/hw/CMakeLists.txt | 0 extensions/{fr3 => rcs_fr3}/src/hw/FR3.cpp | 0 extensions/{fr3 => rcs_fr3}/src/hw/FR3.h | 0 extensions/{fr3 => rcs_fr3}/src/hw/FR3MotionGenerator.cpp | 0 extensions/{fr3 => rcs_fr3}/src/hw/FR3MotionGenerator.h | 0 extensions/{fr3 => rcs_fr3}/src/hw/FrankaHand.cpp | 0 extensions/{fr3 => rcs_fr3}/src/hw/FrankaHand.h | 0 extensions/{fr3 => rcs_fr3}/src/hw/main.cpp | 0 extensions/{fr3 => rcs_fr3}/src/pybind/CMakeLists.txt | 0 extensions/{fr3 => rcs_fr3}/src/pybind/rcs.cpp | 0 extensions/{fr3 => rcs_fr3}/src/rcs_fr3/__init__.py | 0 extensions/{fr3 => rcs_fr3}/src/rcs_fr3/__main__.py | 0 extensions/{fr3 => rcs_fr3}/src/rcs_fr3/creators.py | 0 extensions/{fr3 => rcs_fr3}/src/rcs_fr3/desk.py | 0 extensions/{fr3 => rcs_fr3}/src/rcs_fr3/envs.py | 0 extensions/{fr3 => rcs_fr3}/src/rcs_fr3/utils.py | 0 extensions/{realsense => rcs_realsense}/pyproject.toml | 0 .../{realsense => rcs_realsense}/src/rcs_realsense/__init__.py | 0 .../{realsense => rcs_realsense}/src/rcs_realsense/__main__.py | 0 .../{realsense => rcs_realsense}/src/rcs_realsense/camera.py | 0 .../{realsense => rcs_realsense}/src/rcs_realsense/utils.py | 0 extensions/{so101 => rcs_so101}/pyproject.toml | 0 extensions/{so101 => rcs_so101}/src/rcs_so101/__init__.py | 0 extensions/{so101 => rcs_so101}/src/rcs_so101/creators.py | 0 .../{so101 => rcs_so101}/src/rcs_so101/env_joint_control.py | 0 extensions/{so101 => rcs_so101}/src/rcs_so101/hw.py | 0 31 files changed, 0 insertions(+), 0 deletions(-) rename extensions/{fr3 => rcs_fr3}/CMakeLists.txt (100%) rename extensions/{fr3 => rcs_fr3}/cmake/Findpinocchio.cmake (100%) rename extensions/{fr3 => rcs_fr3}/cmake/Findrcs.cmake (100%) rename extensions/{fr3 => rcs_fr3}/pyproject.toml (100%) rename extensions/{fr3 => rcs_fr3}/src/CMakeLists.txt (100%) rename extensions/{fr3 => rcs_fr3}/src/hw/CMakeLists.txt (100%) rename extensions/{fr3 => rcs_fr3}/src/hw/FR3.cpp (100%) rename extensions/{fr3 => rcs_fr3}/src/hw/FR3.h (100%) rename extensions/{fr3 => rcs_fr3}/src/hw/FR3MotionGenerator.cpp (100%) rename extensions/{fr3 => rcs_fr3}/src/hw/FR3MotionGenerator.h (100%) rename extensions/{fr3 => rcs_fr3}/src/hw/FrankaHand.cpp (100%) rename extensions/{fr3 => rcs_fr3}/src/hw/FrankaHand.h (100%) rename extensions/{fr3 => rcs_fr3}/src/hw/main.cpp (100%) rename extensions/{fr3 => rcs_fr3}/src/pybind/CMakeLists.txt (100%) rename extensions/{fr3 => rcs_fr3}/src/pybind/rcs.cpp (100%) rename extensions/{fr3 => rcs_fr3}/src/rcs_fr3/__init__.py (100%) rename extensions/{fr3 => rcs_fr3}/src/rcs_fr3/__main__.py (100%) rename extensions/{fr3 => rcs_fr3}/src/rcs_fr3/creators.py (100%) rename extensions/{fr3 => rcs_fr3}/src/rcs_fr3/desk.py (100%) rename extensions/{fr3 => rcs_fr3}/src/rcs_fr3/envs.py (100%) rename extensions/{fr3 => rcs_fr3}/src/rcs_fr3/utils.py (100%) rename extensions/{realsense => rcs_realsense}/pyproject.toml (100%) rename extensions/{realsense => rcs_realsense}/src/rcs_realsense/__init__.py (100%) rename extensions/{realsense => rcs_realsense}/src/rcs_realsense/__main__.py (100%) rename extensions/{realsense => rcs_realsense}/src/rcs_realsense/camera.py (100%) rename extensions/{realsense => rcs_realsense}/src/rcs_realsense/utils.py (100%) rename extensions/{so101 => rcs_so101}/pyproject.toml (100%) rename extensions/{so101 => rcs_so101}/src/rcs_so101/__init__.py (100%) rename extensions/{so101 => rcs_so101}/src/rcs_so101/creators.py (100%) rename extensions/{so101 => rcs_so101}/src/rcs_so101/env_joint_control.py (100%) rename extensions/{so101 => rcs_so101}/src/rcs_so101/hw.py (100%) diff --git a/extensions/fr3/CMakeLists.txt b/extensions/rcs_fr3/CMakeLists.txt similarity index 100% rename from extensions/fr3/CMakeLists.txt rename to extensions/rcs_fr3/CMakeLists.txt diff --git a/extensions/fr3/cmake/Findpinocchio.cmake b/extensions/rcs_fr3/cmake/Findpinocchio.cmake similarity index 100% rename from extensions/fr3/cmake/Findpinocchio.cmake rename to extensions/rcs_fr3/cmake/Findpinocchio.cmake diff --git a/extensions/fr3/cmake/Findrcs.cmake b/extensions/rcs_fr3/cmake/Findrcs.cmake similarity index 100% rename from extensions/fr3/cmake/Findrcs.cmake rename to extensions/rcs_fr3/cmake/Findrcs.cmake diff --git a/extensions/fr3/pyproject.toml b/extensions/rcs_fr3/pyproject.toml similarity index 100% rename from extensions/fr3/pyproject.toml rename to extensions/rcs_fr3/pyproject.toml diff --git a/extensions/fr3/src/CMakeLists.txt b/extensions/rcs_fr3/src/CMakeLists.txt similarity index 100% rename from extensions/fr3/src/CMakeLists.txt rename to extensions/rcs_fr3/src/CMakeLists.txt diff --git a/extensions/fr3/src/hw/CMakeLists.txt b/extensions/rcs_fr3/src/hw/CMakeLists.txt similarity index 100% rename from extensions/fr3/src/hw/CMakeLists.txt rename to extensions/rcs_fr3/src/hw/CMakeLists.txt diff --git a/extensions/fr3/src/hw/FR3.cpp b/extensions/rcs_fr3/src/hw/FR3.cpp similarity index 100% rename from extensions/fr3/src/hw/FR3.cpp rename to extensions/rcs_fr3/src/hw/FR3.cpp diff --git a/extensions/fr3/src/hw/FR3.h b/extensions/rcs_fr3/src/hw/FR3.h similarity index 100% rename from extensions/fr3/src/hw/FR3.h rename to extensions/rcs_fr3/src/hw/FR3.h diff --git a/extensions/fr3/src/hw/FR3MotionGenerator.cpp b/extensions/rcs_fr3/src/hw/FR3MotionGenerator.cpp similarity index 100% rename from extensions/fr3/src/hw/FR3MotionGenerator.cpp rename to extensions/rcs_fr3/src/hw/FR3MotionGenerator.cpp diff --git a/extensions/fr3/src/hw/FR3MotionGenerator.h b/extensions/rcs_fr3/src/hw/FR3MotionGenerator.h similarity index 100% rename from extensions/fr3/src/hw/FR3MotionGenerator.h rename to extensions/rcs_fr3/src/hw/FR3MotionGenerator.h diff --git a/extensions/fr3/src/hw/FrankaHand.cpp b/extensions/rcs_fr3/src/hw/FrankaHand.cpp similarity index 100% rename from extensions/fr3/src/hw/FrankaHand.cpp rename to extensions/rcs_fr3/src/hw/FrankaHand.cpp diff --git a/extensions/fr3/src/hw/FrankaHand.h b/extensions/rcs_fr3/src/hw/FrankaHand.h similarity index 100% rename from extensions/fr3/src/hw/FrankaHand.h rename to extensions/rcs_fr3/src/hw/FrankaHand.h diff --git a/extensions/fr3/src/hw/main.cpp b/extensions/rcs_fr3/src/hw/main.cpp similarity index 100% rename from extensions/fr3/src/hw/main.cpp rename to extensions/rcs_fr3/src/hw/main.cpp diff --git a/extensions/fr3/src/pybind/CMakeLists.txt b/extensions/rcs_fr3/src/pybind/CMakeLists.txt similarity index 100% rename from extensions/fr3/src/pybind/CMakeLists.txt rename to extensions/rcs_fr3/src/pybind/CMakeLists.txt diff --git a/extensions/fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp similarity index 100% rename from extensions/fr3/src/pybind/rcs.cpp rename to extensions/rcs_fr3/src/pybind/rcs.cpp diff --git a/extensions/fr3/src/rcs_fr3/__init__.py b/extensions/rcs_fr3/src/rcs_fr3/__init__.py similarity index 100% rename from extensions/fr3/src/rcs_fr3/__init__.py rename to extensions/rcs_fr3/src/rcs_fr3/__init__.py diff --git a/extensions/fr3/src/rcs_fr3/__main__.py b/extensions/rcs_fr3/src/rcs_fr3/__main__.py similarity index 100% rename from extensions/fr3/src/rcs_fr3/__main__.py rename to extensions/rcs_fr3/src/rcs_fr3/__main__.py diff --git a/extensions/fr3/src/rcs_fr3/creators.py b/extensions/rcs_fr3/src/rcs_fr3/creators.py similarity index 100% rename from extensions/fr3/src/rcs_fr3/creators.py rename to extensions/rcs_fr3/src/rcs_fr3/creators.py diff --git a/extensions/fr3/src/rcs_fr3/desk.py b/extensions/rcs_fr3/src/rcs_fr3/desk.py similarity index 100% rename from extensions/fr3/src/rcs_fr3/desk.py rename to extensions/rcs_fr3/src/rcs_fr3/desk.py diff --git a/extensions/fr3/src/rcs_fr3/envs.py b/extensions/rcs_fr3/src/rcs_fr3/envs.py similarity index 100% rename from extensions/fr3/src/rcs_fr3/envs.py rename to extensions/rcs_fr3/src/rcs_fr3/envs.py diff --git a/extensions/fr3/src/rcs_fr3/utils.py b/extensions/rcs_fr3/src/rcs_fr3/utils.py similarity index 100% rename from extensions/fr3/src/rcs_fr3/utils.py rename to extensions/rcs_fr3/src/rcs_fr3/utils.py diff --git a/extensions/realsense/pyproject.toml b/extensions/rcs_realsense/pyproject.toml similarity index 100% rename from extensions/realsense/pyproject.toml rename to extensions/rcs_realsense/pyproject.toml diff --git a/extensions/realsense/src/rcs_realsense/__init__.py b/extensions/rcs_realsense/src/rcs_realsense/__init__.py similarity index 100% rename from extensions/realsense/src/rcs_realsense/__init__.py rename to extensions/rcs_realsense/src/rcs_realsense/__init__.py diff --git a/extensions/realsense/src/rcs_realsense/__main__.py b/extensions/rcs_realsense/src/rcs_realsense/__main__.py similarity index 100% rename from extensions/realsense/src/rcs_realsense/__main__.py rename to extensions/rcs_realsense/src/rcs_realsense/__main__.py diff --git a/extensions/realsense/src/rcs_realsense/camera.py b/extensions/rcs_realsense/src/rcs_realsense/camera.py similarity index 100% rename from extensions/realsense/src/rcs_realsense/camera.py rename to extensions/rcs_realsense/src/rcs_realsense/camera.py diff --git a/extensions/realsense/src/rcs_realsense/utils.py b/extensions/rcs_realsense/src/rcs_realsense/utils.py similarity index 100% rename from extensions/realsense/src/rcs_realsense/utils.py rename to extensions/rcs_realsense/src/rcs_realsense/utils.py diff --git a/extensions/so101/pyproject.toml b/extensions/rcs_so101/pyproject.toml similarity index 100% rename from extensions/so101/pyproject.toml rename to extensions/rcs_so101/pyproject.toml diff --git a/extensions/so101/src/rcs_so101/__init__.py b/extensions/rcs_so101/src/rcs_so101/__init__.py similarity index 100% rename from extensions/so101/src/rcs_so101/__init__.py rename to extensions/rcs_so101/src/rcs_so101/__init__.py diff --git a/extensions/so101/src/rcs_so101/creators.py b/extensions/rcs_so101/src/rcs_so101/creators.py similarity index 100% rename from extensions/so101/src/rcs_so101/creators.py rename to extensions/rcs_so101/src/rcs_so101/creators.py diff --git a/extensions/so101/src/rcs_so101/env_joint_control.py b/extensions/rcs_so101/src/rcs_so101/env_joint_control.py similarity index 100% rename from extensions/so101/src/rcs_so101/env_joint_control.py rename to extensions/rcs_so101/src/rcs_so101/env_joint_control.py diff --git a/extensions/so101/src/rcs_so101/hw.py b/extensions/rcs_so101/src/rcs_so101/hw.py similarity index 100% rename from extensions/so101/src/rcs_so101/hw.py rename to extensions/rcs_so101/src/rcs_so101/hw.py From 22188239aadcbdbb628765ef5e06460a6bb8d0d0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 23 Jun 2025 21:48:49 +0200 Subject: [PATCH 15/32] feat: added stub files for rcs_fr3 --- Makefile | 8 ++ extensions/rcs_fr3/_core/__init__.pyi | 19 +++ extensions/rcs_fr3/_core/hw/__init__.pyi | 151 +++++++++++++++++++++ extensions/rcs_fr3/_core/hw/exceptions.pyi | 44 ++++++ 4 files changed, 222 insertions(+) create mode 100644 extensions/rcs_fr3/_core/__init__.pyi create mode 100644 extensions/rcs_fr3/_core/hw/__init__.pyi create mode 100644 extensions/rcs_fr3/_core/hw/exceptions.pyi diff --git a/Makefile b/Makefile index b2f6c9a7..0b0de5a2 100644 --- a/Makefile +++ b/Makefile @@ -32,6 +32,14 @@ stubgen: ruff check --fix python/rcs/_core isort python/rcs/_core black python/rcs/_core + pybind11-stubgen -o extensions --numpy-array-use-type-var rcs_fr3 + find ./extensions/rcs_fr3 -not -path "./extensions/rcs_fr3/_core/*" -name '*.pyi' -delete + find ./extensions/rcs_fr3 -name '*.pyi' -print | xargs sed -i '1s/^/# ATTENTION: auto generated from C++ code, use `make stubgen` to update!\n/' + find ./extensions/rcs_fr3/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/typing\.Literal[\1]/g' + find ./extensions/rcs_fr3/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/\1/g' + ruff check --fix extensions/rcs_fr3/_core + isort extensions/rcs_fr3/_core + black extensions/rcs_fr3/_core # Python pycheckformat: diff --git a/extensions/rcs_fr3/_core/__init__.pyi b/extensions/rcs_fr3/_core/__init__.pyi new file mode 100644 index 00000000..b6c1624d --- /dev/null +++ b/extensions/rcs_fr3/_core/__init__.pyi @@ -0,0 +1,19 @@ +# ATTENTION: auto generated from C++ code, use `make stubgen` to update! +""" + + Robot Control Stack Python Bindings + ----------------------- + + .. currentmodule:: _core + + .. autosummary:: + :toctree: _generate + + +""" +from __future__ import annotations + +from . import hw + +__all__ = ["hw"] +__version__: str = "0.4.0" diff --git a/extensions/rcs_fr3/_core/hw/__init__.pyi b/extensions/rcs_fr3/_core/hw/__init__.pyi new file mode 100644 index 00000000..e28f3e26 --- /dev/null +++ b/extensions/rcs_fr3/_core/hw/__init__.pyi @@ -0,0 +1,151 @@ +# ATTENTION: auto generated from C++ code, use `make stubgen` to update! +""" +rcs fr3 module +""" +from __future__ import annotations + +import typing + +import numpy +import rcs._core.common + +from . import exceptions + +__all__ = [ + "FHConfig", + "FHState", + "FR3", + "FR3Config", + "FR3Load", + "FR3State", + "FrankaHand", + "IKSolver", + "exceptions", + "franka_ik", + "rcs_ik", +] + +class FHConfig(rcs._core.common.GripperConfig): + async_control: bool + epsilon_inner: float + epsilon_outer: float + force: float + grasping_width: float + speed: float + def __init__(self) -> None: ... + +class FHState(rcs._core.common.GripperState): + def __init__(self) -> None: ... + @property + def bool_state(self) -> bool: ... + @property + def is_grasped(self) -> bool: ... + @property + def is_moving(self) -> bool: ... + @property + def last_commanded_width(self) -> float: ... + @property + def max_unnormalized_width(self) -> float: ... + @property + def temperature(self) -> int: ... + @property + def width(self) -> float: ... + +class FR3(rcs._core.common.Robot): + def __init__(self, ip: str, ik: rcs._core.common.IK | None = None) -> None: ... + def automatic_error_recovery(self) -> None: ... + def controller_set_joint_position( + self, desired_q: numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]] + ) -> None: ... + def double_tap_robot_to_continue(self) -> None: ... + def get_parameters(self) -> FR3Config: ... + def get_state(self) -> FR3State: ... + def osc_set_cartesian_position( + self, desired_pos_EE_in_base_frame: rcs._core.common.Pose + ) -> None: ... + def set_cartesian_position_ik( + self, + pose: rcs._core.common.Pose, + max_time: float, + elbow: float | None, + max_force: float | None = 5, + ) -> None: ... + def set_cartesian_position_internal(self, pose: rcs._core.common.Pose) -> None: ... + def set_default_robot_behavior(self) -> None: ... + def set_guiding_mode( + self, + x: bool = True, + y: bool = True, + z: bool = True, + roll: bool = True, + pitch: bool = True, + yaw: bool = True, + elbow: bool = True, + ) -> None: ... + def set_parameters(self, cfg: FR3Config) -> bool: ... + def stop_control_thread(self) -> None: ... + def zero_torque_guiding(self) -> None: ... + +class FR3Config(rcs._core.common.RobotConfig): + async_control: bool + ik_solver: IKSolver + load_parameters: FR3Load | None + nominal_end_effector_frame: rcs._core.common.Pose | None + speed_factor: float + tcp_offset: rcs._core.common.Pose + world_to_robot: rcs._core.common.Pose | None + def __init__(self) -> None: ... + +class FR3Load: + f_x_cload: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]] | None + load_inertia: ( + numpy.ndarray[ + tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64] + ] + | None + ) + load_mass: float + def __init__(self) -> None: ... + +class FR3State(rcs._core.common.RobotState): + def __init__(self) -> None: ... + +class FrankaHand(rcs._core.common.Gripper): + def __init__(self, ip: str, cfg: FHConfig) -> None: ... + def get_parameters(self) -> FHConfig: ... + def get_state(self) -> FHState: ... + def homing(self) -> bool: ... + def is_grasped(self) -> bool: ... + def set_parameters(self, cfg: FHConfig) -> bool: ... + +class IKSolver: + """ + Members: + + franka_ik + + rcs_ik + """ + + __members__: typing.ClassVar[ + dict[str, IKSolver] + ] # value = {'franka_ik': , 'rcs_ik': } + franka_ik: typing.ClassVar[IKSolver] # value = + rcs_ik: typing.ClassVar[IKSolver] # value = + def __eq__(self, other: typing.Any) -> bool: ... + def __getstate__(self) -> int: ... + def __hash__(self) -> int: ... + def __index__(self) -> int: ... + def __init__(self, value: int) -> None: ... + def __int__(self) -> int: ... + def __ne__(self, other: typing.Any) -> bool: ... + def __repr__(self) -> str: ... + def __setstate__(self, state: int) -> None: ... + def __str__(self) -> str: ... + @property + def name(self) -> str: ... + @property + def value(self) -> int: ... + +franka_ik: IKSolver # value = +rcs_ik: IKSolver # value = diff --git a/extensions/rcs_fr3/_core/hw/exceptions.pyi b/extensions/rcs_fr3/_core/hw/exceptions.pyi new file mode 100644 index 00000000..3576a88d --- /dev/null +++ b/extensions/rcs_fr3/_core/hw/exceptions.pyi @@ -0,0 +1,44 @@ +# ATTENTION: auto generated from C++ code, use `make stubgen` to update! +""" +exceptions from the hardware module +""" +from __future__ import annotations + +__all__ = [ + "FrankaCommandException", + "FrankaControlException", + "FrankaException", + "FrankaIncompatibleVersionException", + "FrankaInvalidOperationException", + "FrankaModelException", + "FrankaNetworkException", + "FrankaProtocolException", + "FrankaRealtimeException", +] + +class FrankaCommandException(RuntimeError): + pass + +class FrankaControlException(RuntimeError): + pass + +class FrankaException(RuntimeError): + pass + +class FrankaIncompatibleVersionException(RuntimeError): + pass + +class FrankaInvalidOperationException(RuntimeError): + pass + +class FrankaModelException(RuntimeError): + pass + +class FrankaNetworkException(RuntimeError): + pass + +class FrankaProtocolException(RuntimeError): + pass + +class FrankaRealtimeException(RuntimeError): + pass From 1071a1ff8713bbac8cf42a7ee68fba80e06dfee2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 23 Jun 2025 21:59:20 +0200 Subject: [PATCH 16/32] style: import sorting and formatting adapted make file to include extension --- Makefile | 12 ++++++------ extensions/rcs_fr3/_core/hw/__init__.pyi | 11 ++--------- extensions/rcs_fr3/src/rcs_fr3/creators.py | 3 ++- extensions/rcs_fr3/src/rcs_fr3/desk.py | 3 ++- extensions/rcs_fr3/src/rcs_fr3/envs.py | 2 +- extensions/rcs_fr3/src/rcs_fr3/utils.py | 3 ++- extensions/rcs_realsense/src/rcs_realsense/camera.py | 3 ++- extensions/rcs_realsense/src/rcs_realsense/utils.py | 3 ++- extensions/rcs_so101/src/rcs_so101/creators.py | 5 +++-- .../rcs_so101/src/rcs_so101/env_joint_control.py | 5 +++-- extensions/rcs_so101/src/rcs_so101/hw.py | 1 + python/examples/fr3.py | 5 +++-- python/rcs/__init__.py | 3 ++- python/rcs/camera/sim.py | 3 ++- python/rcs/envs/base.py | 3 ++- python/rcs/envs/creators.py | 5 +++-- python/rcs/envs/sim.py | 5 +++-- python/rcs/envs/utils.py | 5 +++-- python/tests/test_common.py | 1 + python/tests/test_sim_envs.py | 3 ++- 20 files changed, 47 insertions(+), 37 deletions(-) diff --git a/Makefile b/Makefile index 0b0de5a2..c7793471 100644 --- a/Makefile +++ b/Makefile @@ -43,20 +43,20 @@ stubgen: # Python pycheckformat: - isort --check-only ${PYSRC} - black --check ${PYSRC} + isort --check-only ${PYSRC} extensions + black --check ${PYSRC} extensions pyformat: - isort ${PYSRC} - black ${PYSRC} + isort ${PYSRC} extensions + black ${PYSRC} extensions pylint: ruff mypy ruff: - ruff check ${PYSRC} + ruff check ${PYSRC} extensions mypy: - mypy ${PYSRC} --install-types --non-interactive --no-namespace-packages + mypy ${PYSRC} extensions --install-types --non-interactive --no-namespace-packages --exclude 'build' pytest: pytest -vv diff --git a/extensions/rcs_fr3/_core/hw/__init__.pyi b/extensions/rcs_fr3/_core/hw/__init__.pyi index e28f3e26..bca1aeb6 100644 --- a/extensions/rcs_fr3/_core/hw/__init__.pyi +++ b/extensions/rcs_fr3/_core/hw/__init__.pyi @@ -60,9 +60,7 @@ class FR3(rcs._core.common.Robot): def double_tap_robot_to_continue(self) -> None: ... def get_parameters(self) -> FR3Config: ... def get_state(self) -> FR3State: ... - def osc_set_cartesian_position( - self, desired_pos_EE_in_base_frame: rcs._core.common.Pose - ) -> None: ... + def osc_set_cartesian_position(self, desired_pos_EE_in_base_frame: rcs._core.common.Pose) -> None: ... def set_cartesian_position_ik( self, pose: rcs._core.common.Pose, @@ -98,12 +96,7 @@ class FR3Config(rcs._core.common.RobotConfig): class FR3Load: f_x_cload: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]] | None - load_inertia: ( - numpy.ndarray[ - tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64] - ] - | None - ) + load_inertia: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]] | None load_mass: float def __init__(self) -> None: ... diff --git a/extensions/rcs_fr3/src/rcs_fr3/creators.py b/extensions/rcs_fr3/src/rcs_fr3/creators.py index 53591c09..57784326 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/creators.py +++ b/extensions/rcs_fr3/src/rcs_fr3/creators.py @@ -3,7 +3,6 @@ import gymnasium as gym import numpy as np -import rcs import rcs.hand.tilburg_hand from rcs.camera.hw import HardwareCameraSet from rcs.envs.base import ( @@ -22,6 +21,8 @@ from rcs_fr3.envs import FR3HW from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg +import rcs + logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) diff --git a/extensions/rcs_fr3/src/rcs_fr3/desk.py b/extensions/rcs_fr3/src/rcs_fr3/desk.py index 140ab14d..48042e17 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/desk.py +++ b/extensions/rcs_fr3/src/rcs_fr3/desk.py @@ -10,13 +10,14 @@ from typing import Callable, Literal from urllib import parse -import rcs import requests from dotenv import load_dotenv from rcs_fr3.utils import default_fr3_hw_gripper_cfg from requests.packages import urllib3 # type: ignore[attr-defined] from websockets.sync.client import connect +import rcs + _logger = logging.getLogger("desk") TOKEN_PATH = "~/.rcs/token.conf" diff --git a/extensions/rcs_fr3/src/rcs_fr3/envs.py b/extensions/rcs_fr3/src/rcs_fr3/envs.py index 3f1472bb..303c1d13 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/envs.py +++ b/extensions/rcs_fr3/src/rcs_fr3/envs.py @@ -2,8 +2,8 @@ from typing import Any, SupportsFloat, cast import gymnasium as gym -from rcs_fr3._core import hw from rcs.envs.base import RobotEnv +from rcs_fr3._core import hw _logger = logging.getLogger(__name__) diff --git a/extensions/rcs_fr3/src/rcs_fr3/utils.py b/extensions/rcs_fr3/src/rcs_fr3/utils.py index 6fb5a47c..0c368bd7 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/utils.py +++ b/extensions/rcs_fr3/src/rcs_fr3/utils.py @@ -1,6 +1,7 @@ -from rcs import common from rcs_fr3._core import hw +from rcs import common + def default_fr3_hw_robot_cfg(async_control: bool = False) -> hw.FR3Config: robot_cfg = hw.FR3Config() diff --git a/extensions/rcs_realsense/src/rcs_realsense/camera.py b/extensions/rcs_realsense/src/rcs_realsense/camera.py index f0ddb65c..4cd55c85 100644 --- a/extensions/rcs_realsense/src/rcs_realsense/camera.py +++ b/extensions/rcs_realsense/src/rcs_realsense/camera.py @@ -3,10 +3,11 @@ import numpy as np import pyrealsense2 as rs -from rcs import common from rcs.camera.hw import HardwareCamera from rcs.camera.interface import CameraFrame, DataFrame, Frame, IMUFrame +from rcs import common + @dataclass class RealSenseDeviceInfo: diff --git a/extensions/rcs_realsense/src/rcs_realsense/utils.py b/extensions/rcs_realsense/src/rcs_realsense/utils.py index 645f3595..372dc04d 100644 --- a/extensions/rcs_realsense/src/rcs_realsense/utils.py +++ b/extensions/rcs_realsense/src/rcs_realsense/utils.py @@ -1,6 +1,7 @@ -from rcs import common from rcs_realsense.camera import RealSenseCameraSet +from rcs import common + def default_realsense(name2id: dict[str, str] | None) -> RealSenseCameraSet | None: if name2id is None: diff --git a/extensions/rcs_so101/src/rcs_so101/creators.py b/extensions/rcs_so101/src/rcs_so101/creators.py index 32d1132c..c950c1ed 100644 --- a/extensions/rcs_so101/src/rcs_so101/creators.py +++ b/extensions/rcs_so101/src/rcs_so101/creators.py @@ -4,7 +4,6 @@ from typing import Type import gymnasium as gym -import rcs from gymnasium.envs.registration import EnvCreator from lerobot.common.robots import make_robot_from_config from lerobot.common.robots.so101_follower.config_so101_follower import ( @@ -15,7 +14,6 @@ ) from lerobot.common.teleoperators.so101_leader.so101_leader import SO101Leader from lerobot.common.teleoperators.utils import make_teleoperator_from_config -from rcs import common, sim from rcs.camera.hw import HardwareCameraSet from rcs.camera.sim import SimCameraSet from rcs.envs.base import ( @@ -31,6 +29,9 @@ from rcs.sim import SimCameraConfig, SimGripperConfig, SimRobotConfig from rcs_so101.hw import SO101, S0101Gripper +import rcs +from rcs import common, sim + logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) diff --git a/extensions/rcs_so101/src/rcs_so101/env_joint_control.py b/extensions/rcs_so101/src/rcs_so101/env_joint_control.py index 0862e7b9..d7cbbb80 100644 --- a/extensions/rcs_so101/src/rcs_so101/env_joint_control.py +++ b/extensions/rcs_so101/src/rcs_so101/env_joint_control.py @@ -1,12 +1,13 @@ import logging from time import sleep -import rcs -from rcs import sim from rcs._core.common import RobotPlatform from rcs.envs.base import ControlMode, RelativeTo from rcs_so101.creators import RCSSO101EnvCreator, SO101SimEnvCreator +import rcs +from rcs import sim + logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) diff --git a/extensions/rcs_so101/src/rcs_so101/hw.py b/extensions/rcs_so101/src/rcs_so101/hw.py index ee4e9ec7..947e9991 100644 --- a/extensions/rcs_so101/src/rcs_so101/hw.py +++ b/extensions/rcs_so101/src/rcs_so101/hw.py @@ -2,6 +2,7 @@ import numpy as np from lerobot.common.robots.so101_follower.so101_follower import SO101Follower + from rcs import common diff --git a/python/examples/fr3.py b/python/examples/fr3.py index 7a22d014..daa75698 100644 --- a/python/examples/fr3.py +++ b/python/examples/fr3.py @@ -1,14 +1,15 @@ import logging import numpy as np -import rcs -from rcs import sim from rcs._core.common import RobotPlatform from rcs._core.hw import FR3Config, IKSolver from rcs._core.sim import CameraType from rcs.camera.sim import SimCameraConfig, SimCameraSet from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_fr3_desk +import rcs +from rcs import sim + ROBOT_IP = "192.168.101.1" ROBOT_INSTANCE = RobotPlatform.SIMULATION diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py index 72270331..413ddfa5 100644 --- a/python/rcs/__init__.py +++ b/python/rcs/__init__.py @@ -4,10 +4,11 @@ import site from gymnasium import register -from rcs import camera, envs, hand, sim from rcs._core import __version__, common from rcs.envs.creators import FR3SimplePickUpSimEnvCreator +from rcs import camera, envs, hand, sim + # available mujoco scenes scenes: dict[str, dict[str, pathlib.Path]] = { path.stem: { diff --git a/python/rcs/camera/sim.py b/python/rcs/camera/sim.py index 8ea38c67..ba5eaf67 100644 --- a/python/rcs/camera/sim.py +++ b/python/rcs/camera/sim.py @@ -2,7 +2,6 @@ from datetime import datetime import numpy as np -import rcs # from rcs._core.common import BaseCameraConfig from rcs._core.sim import FrameSet as _FrameSet @@ -10,6 +9,8 @@ from rcs._core.sim import SimCameraSet as _SimCameraSet from rcs.camera.interface import CameraFrame, DataFrame, Frame, FrameSet +import rcs + class SimCameraSet(_SimCameraSet): """Represents a set of cameras in a mujoco simulation. diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index 18451da2..11370c03 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -7,7 +7,6 @@ import gymnasium as gym import numpy as np -from rcs import common from rcs.camera.interface import BaseCameraSet from rcs.envs.space_utils import ( ActObsInfoWrapper, @@ -21,6 +20,8 @@ ) from rcs.hand.interface import BaseHand +from rcs import common + _logger = logging.getLogger(__name__) diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index 37c76be1..76dcf8d4 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -4,9 +4,7 @@ import gymnasium as gym import numpy as np -import rcs from gymnasium.envs.registration import EnvCreator -from rcs import sim from rcs._core.sim import CameraType from rcs.camera.sim import SimCameraConfig, SimCameraSet from rcs.envs.base import ( @@ -28,6 +26,9 @@ from rcs.envs.space_utils import VecType from rcs.envs.utils import default_sim_gripper_cfg, default_sim_robot_cfg +import rcs +from rcs import sim + logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index 9c3ec169..76eca2e2 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -3,12 +3,13 @@ import gymnasium as gym import numpy as np -import rcs -from rcs import sim from rcs.envs.base import ControlMode, GripperWrapper, MultiRobotWrapper, RobotEnv from rcs.envs.space_utils import ActObsInfoWrapper, VecType from rcs.envs.utils import default_sim_robot_cfg +import rcs +from rcs import sim + logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py index 9d644386..781391a3 100644 --- a/python/rcs/envs/utils.py +++ b/python/rcs/envs/utils.py @@ -4,14 +4,15 @@ import mujoco as mj import numpy as np -import rcs from digit_interface import Digit -from rcs import sim from rcs._core.common import BaseCameraConfig from rcs._core.sim import CameraType, SimCameraConfig from rcs.camera.digit_cam import DigitCam from rcs.hand.tilburg_hand import THConfig +import rcs +from rcs import sim + logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) diff --git a/python/tests/test_common.py b/python/tests/test_common.py index 03e3fdd0..52996ff3 100644 --- a/python/tests/test_common.py +++ b/python/tests/test_common.py @@ -2,6 +2,7 @@ import numpy as np import pytest + from rcs import common diff --git a/python/tests/test_sim_envs.py b/python/tests/test_sim_envs.py index c5e2fd5c..e2495f8e 100644 --- a/python/tests/test_sim_envs.py +++ b/python/tests/test_sim_envs.py @@ -2,7 +2,6 @@ import numpy as np import pytest -import rcs from rcs.envs.base import ( ControlMode, GripperDictType, @@ -18,6 +17,8 @@ default_sim_robot_cfg, ) +import rcs + @pytest.fixture() def cfg(): From 739d31693672f8f3bca08f536b67dc8ab3bad49b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 23 Jun 2025 22:35:57 +0200 Subject: [PATCH 17/32] style: fix imports from rebase --- python/rcs/envs/creators.py | 1 - python/rcs/envs/sim.py | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index 76dcf8d4..ac5b242f 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -23,7 +23,6 @@ RobotSimWrapper, SimWrapper, ) -from rcs.envs.space_utils import VecType from rcs.envs.utils import default_sim_gripper_cfg, default_sim_robot_cfg import rcs diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index 76eca2e2..da797787 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -4,7 +4,7 @@ import gymnasium as gym import numpy as np from rcs.envs.base import ControlMode, GripperWrapper, MultiRobotWrapper, RobotEnv -from rcs.envs.space_utils import ActObsInfoWrapper, VecType +from rcs.envs.space_utils import ActObsInfoWrapper from rcs.envs.utils import default_sim_robot_cfg import rcs From f527368a66831a553af3eb1d35ffcf2c7ceeb9b7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 23 Jun 2025 22:49:54 +0200 Subject: [PATCH 18/32] style: fix types --- extensions/rcs_fr3/src/rcs_fr3/__init__.py | 2 ++ extensions/rcs_fr3/src/rcs_fr3/creators.py | 21 +++++++++++---------- extensions/rcs_fr3/src/rcs_fr3/desk.py | 18 +++++++++--------- python/examples/fr3.py | 9 +++++---- 4 files changed, 27 insertions(+), 23 deletions(-) diff --git a/extensions/rcs_fr3/src/rcs_fr3/__init__.py b/extensions/rcs_fr3/src/rcs_fr3/__init__.py index cd4f6f15..29e879ce 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/__init__.py +++ b/extensions/rcs_fr3/src/rcs_fr3/__init__.py @@ -1,6 +1,8 @@ from rcs_fr3 import desk, envs +from rcs_fr3._core import hw __all__ = [ "desk", "envs", + "hw", ] diff --git a/extensions/rcs_fr3/src/rcs_fr3/creators.py b/extensions/rcs_fr3/src/rcs_fr3/creators.py index 57784326..11ae9a19 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/creators.py +++ b/extensions/rcs_fr3/src/rcs_fr3/creators.py @@ -20,6 +20,7 @@ from rcs.hand.tilburg_hand import TilburgHand from rcs_fr3.envs import FR3HW from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg +from rcs_fr3 import hw import rcs @@ -32,9 +33,9 @@ def __call__( # type: ignore self, ip: str, control_mode: ControlMode, - robot_cfg: rcs.hw.FR3Config, + robot_cfg: hw.FR3Config, collision_guard: str | PathLike | None = None, - gripper_cfg: rcs.hw.FHConfig | rcs.hand.tilburg_hand.THConfig | None = None, + gripper_cfg: hw.FHConfig | rcs.hand.tilburg_hand.THConfig | None = None, camera_set: HardwareCameraSet | None = None, max_relative_movement: float | tuple[float, float] | None = None, relative_to: RelativeTo = RelativeTo.LAST_STEP, @@ -65,14 +66,14 @@ def __call__( # type: ignore if urdf_path is None: urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] ik = rcs.common.IK(str(urdf_path)) if urdf_path is not None else None - robot = rcs.hw.FR3(ip, ik) + robot = hw.FR3(ip, ik) robot.set_parameters(robot_cfg) env: gym.Env = RobotEnv(robot, ControlMode.JOINTS if collision_guard is not None else control_mode) env = FR3HW(env) - if isinstance(gripper_cfg, rcs.hw.FHConfig): - gripper = rcs.hw.FrankaHand(ip, gripper_cfg) + if isinstance(gripper_cfg, hw.FHConfig): + gripper = hw.FrankaHand(ip, gripper_cfg) env = GripperWrapper(env, gripper, binary=True) elif isinstance(gripper_cfg, rcs.hand.tilburg_hand.THConfig): hand = TilburgHand(gripper_cfg) @@ -107,8 +108,8 @@ class RCSFR3MultiEnvCreator(RCSHardwareEnvCreator): def __call__( # type: ignore ips: list[str], control_mode: ControlMode, - robot_cfg: rcs.hw.FR3Config, - gripper_cfg: rcs.hw.FHConfig | None = None, + robot_cfg: hw.FR3Config, + gripper_cfg: hw.FHConfig | None = None, camera_set: HardwareCameraSet | None = None, max_relative_movement: float | tuple[float, float] | None = None, relative_to: RelativeTo = RelativeTo.LAST_STEP, @@ -117,9 +118,9 @@ def __call__( # type: ignore urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] ik = rcs.common.IK(str(urdf_path)) if urdf_path is not None else None - robots: dict[str, rcs.hw.FR3] = {} + robots: dict[str, hw.FR3] = {} for ip in ips: - robots[ip] = rcs.hw.FR3(ip, ik) + robots[ip] = hw.FR3(ip, ik) robots[ip].set_parameters(robot_cfg) envs = {} @@ -127,7 +128,7 @@ def __call__( # type: ignore env: gym.Env = RobotEnv(robots[ip], control_mode) env = FR3HW(env) if gripper_cfg is not None: - gripper = rcs.hw.FrankaHand(ip, gripper_cfg) + gripper = hw.FrankaHand(ip, gripper_cfg) env = GripperWrapper(env, gripper, binary=True) if max_relative_movement is not None: diff --git a/extensions/rcs_fr3/src/rcs_fr3/desk.py b/extensions/rcs_fr3/src/rcs_fr3/desk.py index 48042e17..78b74768 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/desk.py +++ b/extensions/rcs_fr3/src/rcs_fr3/desk.py @@ -13,10 +13,10 @@ import requests from dotenv import load_dotenv from rcs_fr3.utils import default_fr3_hw_gripper_cfg +import rcs_fr3 from requests.packages import urllib3 # type: ignore[attr-defined] from websockets.sync.client import connect -import rcs _logger = logging.getLogger("desk") @@ -38,13 +38,13 @@ def load_creds_fr3_desk() -> tuple[str, str]: def home(ip: str, username: str, password: str, shut: bool, unlock: bool = False): with Desk.fci(ip, username, password, unlock=unlock): - f = rcs.hw.FR3(ip) - config = rcs.hw.FR3Config() + f = rcs_fr3.hw.FR3(ip) + config = rcs_fr3.hw.FR3Config() config.speed_factor = 0.2 - config.ik_solver = rcs.hw.IKSolver.franka_ik + config.ik_solver = rcs_fr3.hw.IKSolver.franka_ik f.set_parameters(config) - config_hand = rcs.hw.FHConfig() - g = rcs.hw.FrankaHand(ip, config_hand) + config_hand = rcs_fr3.hw.FHConfig() + g = rcs_fr3.hw.FrankaHand(ip, config_hand) if shut: g.shut() else: @@ -54,8 +54,8 @@ def home(ip: str, username: str, password: str, shut: bool, unlock: bool = False def info(ip: str, username: str, password: str, include_hand: bool = False): with Desk.fci(ip, username, password): - f = rcs.hw.FR3(ip) - config = rcs.hw.FR3Config() + f = rcs_fr3.hw.FR3(ip) + config = rcs_fr3.hw.FR3Config() f.set_parameters(config) print("Robot info:") print("Current cartesian position:") @@ -64,7 +64,7 @@ def info(ip: str, username: str, password: str, include_hand: bool = False): print(f.get_joint_position()) if include_hand: config_hand = default_fr3_hw_gripper_cfg() - g = rcs.hw.FrankaHand(ip, config_hand) + g = rcs_fr3.hw.FrankaHand(ip, config_hand) print("Gripper info:") print("Current normalized width:") print(g.get_normalized_width()) diff --git a/python/examples/fr3.py b/python/examples/fr3.py index daa75698..f84ba74a 100644 --- a/python/examples/fr3.py +++ b/python/examples/fr3.py @@ -6,6 +6,7 @@ from rcs._core.sim import CameraType from rcs.camera.sim import SimCameraConfig, SimCameraSet from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_fr3_desk +from rcs_fr3._core import hw import rcs from rcs import sim @@ -94,17 +95,17 @@ def main(): else: urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] ik = rcs.common.IK(str(urdf_path)) - robot = rcs.hw.FR3(ROBOT_IP, ik) + robot = hw.FR3(ROBOT_IP, ik) robot_cfg = FR3Config() robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) robot_cfg.ik_solver = IKSolver.rcs_ik - robot.set_parameters(robot_cfg) + robot.set_parameters(robot_cfg) # type: ignore - gripper_cfg_hw = rcs.hw.FHConfig() + gripper_cfg_hw = hw.FHConfig() gripper_cfg_hw.epsilon_inner = gripper_cfg_hw.epsilon_outer = 0.1 gripper_cfg_hw.speed = 0.1 gripper_cfg_hw.force = 30 - gripper = rcs.hw.FrankaHand(ROBOT_IP, gripper_cfg_hw) + gripper = hw.FrankaHand(ROBOT_IP, gripper_cfg_hw) input("the robot is going to move, press enter whenever you are ready") # move to home position and open gripper From 97d2069442e932c6687cb7aeaeaa3122a12f99c2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 23 Jun 2025 23:03:58 +0200 Subject: [PATCH 19/32] style: cpp format --- Makefile | 9 +++++++-- extensions/rcs_fr3/src/hw/FR3.h | 11 ++++++----- extensions/rcs_fr3/src/hw/FR3MotionGenerator.h | 5 +++-- extensions/rcs_fr3/src/hw/FrankaHand.h | 3 ++- extensions/rcs_fr3/src/hw/main.cpp | 2 +- extensions/rcs_fr3/src/pybind/rcs.cpp | 9 ++++----- extensions/rcs_fr3/src/rcs_fr3/creators.py | 2 +- extensions/rcs_fr3/src/rcs_fr3/desk.py | 3 +-- python/examples/fr3.py | 2 +- src/pybind/rcs.cpp | 9 ++++----- src/sim/SimGripper.h | 3 +-- src/sim/SimRobot.cpp | 2 +- src/sim/SimRobot.h | 2 +- src/sim/camera.cpp | 2 +- src/sim/camera.h | 2 +- src/sim/renderer.cpp | 2 +- 16 files changed, 36 insertions(+), 32 deletions(-) diff --git a/Makefile b/Makefile index c7793471..52860ca1 100644 --- a/Makefile +++ b/Makefile @@ -5,22 +5,27 @@ COMPILE_MODE = Release # CPP cppcheckformat: clang-format --dry-run -Werror -i $(shell find ${CPPSRC} -name '*.cpp' -o -name '*.cc' -o -name '*.h') + clang-format --dry-run -Werror -i $(shell find extensions/rcs_fr3/src -name '*.cpp' -o -name '*.cc' -o -name '*.h') cppformat: clang-format -Werror -i $(shell find ${CPPSRC} -name '*.cpp' -o -name '*.cc' -o -name '*.h') + clang-format -Werror -i $(shell find extensions/rcs_fr3/src -name '*.cpp' -o -name '*.cc' -o -name '*.h') cpplint: clang-tidy -p=build --warnings-as-errors='*' $(shell find ${CPPSRC} -name '*.cpp' -o -name '*.cc' -name '*.h') +# import errors +# clang-tidy -p=build --warnings-as-errors='*' $(shell find extensions/rcs_fr3/src -name '*.cpp' -o -name '*.cc' -name '*.h') + gcccompile: pip install --upgrade --requirement requirements_dev.txt cmake -DCMAKE_BUILD_TYPE=${COMPILE_MODE} -DCMAKE_C_COMPILER=gcc -DCMAKE_CXX_COMPILER=g++ -B build -G Ninja - cmake --build build --target _core franka + cmake --build build --target _core clangcompile: pip install --upgrade --requirement requirements_dev.txt cmake -DCMAKE_BUILD_TYPE=${COMPILE_MODE} -DCMAKE_C_COMPILER=clang -DCMAKE_CXX_COMPILER=clang++ -B build -G Ninja - cmake --build build --target _core franka + cmake --build build --target _core # Auto generation of CPP binding stub files stubgen: diff --git a/extensions/rcs_fr3/src/hw/FR3.h b/extensions/rcs_fr3/src/hw/FR3.h index 7fdae8f9..3cc2a799 100644 --- a/extensions/rcs_fr3/src/hw/FR3.h +++ b/extensions/rcs_fr3/src/hw/FR3.h @@ -1,11 +1,6 @@ #ifndef RCS_FR3_H #define RCS_FR3_H -#include "rcs/IK.h" -#include "rcs/LinearPoseTrajInterpolator.h" -#include "rcs/Pose.h" -#include "rcs/Robot.h" -#include "rcs/utils.h" #include #include @@ -15,6 +10,12 @@ #include #include +#include "rcs/IK.h" +#include "rcs/LinearPoseTrajInterpolator.h" +#include "rcs/Pose.h" +#include "rcs/Robot.h" +#include "rcs/utils.h" + namespace rcs { namespace hw { diff --git a/extensions/rcs_fr3/src/hw/FR3MotionGenerator.h b/extensions/rcs_fr3/src/hw/FR3MotionGenerator.h index 096e1291..5f8bfdb5 100644 --- a/extensions/rcs_fr3/src/hw/FR3MotionGenerator.h +++ b/extensions/rcs_fr3/src/hw/FR3MotionGenerator.h @@ -1,8 +1,6 @@ #ifndef RCS_MOTION_GENERATOR_H #define RCS_MOTION_GENERATOR_H -#include "rcs/Robot.h" -#include "rcs/utils.h" #include #include #include @@ -12,6 +10,9 @@ #include #include +#include "rcs/Robot.h" +#include "rcs/utils.h" + namespace rcs { namespace hw { diff --git a/extensions/rcs_fr3/src/hw/FrankaHand.h b/extensions/rcs_fr3/src/hw/FrankaHand.h index dd9d1943..40895160 100644 --- a/extensions/rcs_fr3/src/hw/FrankaHand.h +++ b/extensions/rcs_fr3/src/hw/FrankaHand.h @@ -1,7 +1,6 @@ #ifndef RCS_FRANKA_HAND_H #define RCS_FRANKA_HAND_H -#include "rcs/Robot.h" #include #include @@ -9,6 +8,8 @@ #include #include +#include "rcs/Robot.h" + // TODO: we need a common interface for the gripper, maybe we do use the hal // we need to create a robot class that has both the robot and the gripper diff --git a/extensions/rcs_fr3/src/hw/main.cpp b/extensions/rcs_fr3/src/hw/main.cpp index 05fd5867..3f4afbc0 100644 --- a/extensions/rcs_fr3/src/hw/main.cpp +++ b/extensions/rcs_fr3/src/hw/main.cpp @@ -1,7 +1,7 @@ -#include #include #include #include +#include #include #include diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp index 9a4c772c..4af63f5a 100644 --- a/extensions/rcs_fr3/src/pybind/rcs.cpp +++ b/extensions/rcs_fr3/src/pybind/rcs.cpp @@ -1,7 +1,3 @@ -#include "rcs/IK.h" -#include "rcs/Pose.h" -#include "rcs/Robot.h" -#include "rcs/utils.h" #include #include #include @@ -13,6 +9,10 @@ #include +#include "rcs/IK.h" +#include "rcs/Pose.h" +#include "rcs/Robot.h" +#include "rcs/utils.h" #include "rl/mdl/UrdfFactory.h" // TODO: define exceptions @@ -148,5 +148,4 @@ PYBIND11_MODULE(_core, m) { hw_except, "FrankaRealtimeException", PyExc_RuntimeError); py::register_exception( hw_except, "FrankaInvalidOperationException", PyExc_RuntimeError); - } diff --git a/extensions/rcs_fr3/src/rcs_fr3/creators.py b/extensions/rcs_fr3/src/rcs_fr3/creators.py index 11ae9a19..a50b4085 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/creators.py +++ b/extensions/rcs_fr3/src/rcs_fr3/creators.py @@ -18,9 +18,9 @@ from rcs.envs.creators import RCSHardwareEnvCreator from rcs.envs.sim import CollisionGuard from rcs.hand.tilburg_hand import TilburgHand +from rcs_fr3 import hw from rcs_fr3.envs import FR3HW from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg -from rcs_fr3 import hw import rcs diff --git a/extensions/rcs_fr3/src/rcs_fr3/desk.py b/extensions/rcs_fr3/src/rcs_fr3/desk.py index 78b74768..ab5076f6 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/desk.py +++ b/extensions/rcs_fr3/src/rcs_fr3/desk.py @@ -10,14 +10,13 @@ from typing import Callable, Literal from urllib import parse +import rcs_fr3 import requests from dotenv import load_dotenv from rcs_fr3.utils import default_fr3_hw_gripper_cfg -import rcs_fr3 from requests.packages import urllib3 # type: ignore[attr-defined] from websockets.sync.client import connect - _logger = logging.getLogger("desk") TOKEN_PATH = "~/.rcs/token.conf" diff --git a/python/examples/fr3.py b/python/examples/fr3.py index f84ba74a..1f37e2df 100644 --- a/python/examples/fr3.py +++ b/python/examples/fr3.py @@ -5,8 +5,8 @@ from rcs._core.hw import FR3Config, IKSolver from rcs._core.sim import CameraType from rcs.camera.sim import SimCameraConfig, SimCameraSet -from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_fr3_desk from rcs_fr3._core import hw +from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_fr3_desk import rcs from rcs import sim diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 802d1848..57301d3e 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -1,7 +1,3 @@ -#include "rcs/IK.h" -#include "rcs/Pose.h" -#include "rcs/Robot.h" -#include "rcs/utils.h" #include #include #include @@ -14,6 +10,10 @@ #include +#include "rcs/IK.h" +#include "rcs/Pose.h" +#include "rcs/Robot.h" +#include "rcs/utils.h" #include "rl/mdl/UrdfFactory.h" // TODO: define exceptions @@ -305,7 +305,6 @@ PYBIND11_MODULE(_core, m) { .def("reset", &rcs::common::Gripper::reset, py::call_guard()); - // SIM MODULE auto sim = m.def_submodule("sim", "sim module"); py::class_( diff --git a/src/sim/SimGripper.h b/src/sim/SimGripper.h index ff25394c..f48acbb8 100644 --- a/src/sim/SimGripper.h +++ b/src/sim/SimGripper.h @@ -1,13 +1,12 @@ #ifndef RCS_FRANKA_HAND_SIM_H #define RCS_FRANKA_HAND_SIM_H -#include "rcs/Robot.h" - #include #include #include #include +#include "rcs/Robot.h" #include "sim/sim.h" namespace rcs { diff --git a/src/sim/SimRobot.cpp b/src/sim/SimRobot.cpp index cd8dc901..f1b8dccd 100644 --- a/src/sim/SimRobot.cpp +++ b/src/sim/SimRobot.cpp @@ -14,10 +14,10 @@ #include #include -#include "rcs/Robot.h" #include "mujoco/mjdata.h" #include "mujoco/mjmodel.h" #include "mujoco/mujoco.h" +#include "rcs/Robot.h" namespace rcs { namespace sim { diff --git a/src/sim/SimRobot.h b/src/sim/SimRobot.h index 32fd894f..04cef8aa 100644 --- a/src/sim/SimRobot.h +++ b/src/sim/SimRobot.h @@ -1,10 +1,10 @@ #ifndef RCS_FR3SIM_H #define RCS_FR3SIM_H +#include #include #include #include #include -#include #include "sim/sim.h" diff --git a/src/sim/camera.cpp b/src/sim/camera.cpp index 8dde996e..7323d6c4 100644 --- a/src/sim/camera.cpp +++ b/src/sim/camera.cpp @@ -13,10 +13,10 @@ #include #include -#include "rcs/Robot.h" #include "mujoco/mjdata.h" #include "mujoco/mjmodel.h" #include "mujoco/mujoco.h" +#include "rcs/Robot.h" namespace rcs { namespace sim { diff --git a/src/sim/camera.h b/src/sim/camera.h index d1e8214d..160cd01e 100644 --- a/src/sim/camera.h +++ b/src/sim/camera.h @@ -1,8 +1,8 @@ #ifndef RCS_CAMSIM_H #define RCS_CAMSIM_H +#include #include #include -#include #include #include diff --git a/src/sim/renderer.cpp b/src/sim/renderer.cpp index 8b5e9e96..95efeece 100644 --- a/src/sim/renderer.cpp +++ b/src/sim/renderer.cpp @@ -1,10 +1,10 @@ -#include "rcs/utils.h" #include #include #include #include +#include "rcs/utils.h" #include "sim/sim.h" namespace rcs { namespace sim { From fa9763f8502db09677aefb49f7f12bcde6e8da49 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Wed, 25 Jun 2025 10:08:42 +0200 Subject: [PATCH 20/32] fix: cycle import problem --- python/rcs/camera/sim.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/python/rcs/camera/sim.py b/python/rcs/camera/sim.py index ba5eaf67..9513df54 100644 --- a/python/rcs/camera/sim.py +++ b/python/rcs/camera/sim.py @@ -8,8 +8,8 @@ from rcs._core.sim import SimCameraConfig from rcs._core.sim import SimCameraSet as _SimCameraSet from rcs.camera.interface import CameraFrame, DataFrame, Frame, FrameSet +from rcs import sim -import rcs class SimCameraSet(_SimCameraSet): @@ -19,7 +19,7 @@ class SimCameraSet(_SimCameraSet): def __init__( self, - sim: rcs.sim.Sim, + sim: sim.Sim, cameras: dict[str, SimCameraConfig], physical_units: bool = False, render_on_demand: bool = True, @@ -29,7 +29,7 @@ def __init__( self.physical_units = physical_units super().__init__(sim, cameras, render_on_demand=render_on_demand) - self._sim: rcs.sim.Sim + self._sim: sim.Sim def get_latest_frames(self) -> FrameSet | None: """Should return the latest frame from the camera with the given name.""" From ec256d5aad1a11e7765f6a3d025f9542501a88b6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Wed, 25 Jun 2025 10:23:37 +0200 Subject: [PATCH 21/32] fix(fr3): pybind rcs module refs --- extensions/rcs_fr3/src/pybind/CMakeLists.txt | 2 +- extensions/rcs_fr3/src/pybind/rcs.cpp | 22 +++++++++++++------- extensions/rcs_fr3/src/rcs_fr3/creators.py | 6 +++--- 3 files changed, 18 insertions(+), 12 deletions(-) diff --git a/extensions/rcs_fr3/src/pybind/CMakeLists.txt b/extensions/rcs_fr3/src/pybind/CMakeLists.txt index f91fd2b7..91d134c0 100644 --- a/extensions/rcs_fr3/src/pybind/CMakeLists.txt +++ b/extensions/rcs_fr3/src/pybind/CMakeLists.txt @@ -7,7 +7,7 @@ set_target_properties(franka PROPERTIES INTERPROCEDURAL_OPTIMIZATION TRUE ) set_target_properties(_core PROPERTIES - INSTALL_RPATH "$ORIGIN;$ORIGIN/../rcs_fr3" + INSTALL_RPATH "$ORIGIN;$ORIGIN/../rcs" INTERPROCEDURAL_OPTIMIZATION TRUE ) # in pip diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp index 4af63f5a..d1ff5bc8 100644 --- a/extensions/rcs_fr3/src/pybind/rcs.cpp +++ b/extensions/rcs_fr3/src/pybind/rcs.cpp @@ -42,7 +42,8 @@ PYBIND11_MODULE(_core, m) { // HARDWARE MODULE auto hw = m.def_submodule("hw", "rcs fr3 module"); - py::class_(hw, "FR3State") + py::object robot_state = (py::object) py::module_::import("rcs").attr("common").attr("RobotState"); + py::class_(hw, "FR3State", robot_state) .def(py::init<>()); py::class_(hw, "FR3Load") .def(py::init<>()) @@ -55,7 +56,8 @@ PYBIND11_MODULE(_core, m) { .value("rcs_ik", rcs::hw::IKSolver::rcs_ik) .export_values(); - py::class_(hw, "FR3Config") + py::object robot_config = (py::object) py::module_::import("rcs").attr("common").attr("RobotConfig"); + py::class_(hw, "FR3Config", robot_config) .def(py::init<>()) .def_readwrite("ik_solver", &rcs::hw::FR3Config::ik_solver) .def_readwrite("speed_factor", &rcs::hw::FR3Config::speed_factor) @@ -66,7 +68,8 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("tcp_offset", &rcs::hw::FR3Config::tcp_offset) .def_readwrite("async_control", &rcs::hw::FR3Config::async_control); - py::class_(hw, "FHConfig") + py::object gripper_config = (py::object) py::module_::import("rcs").attr("common").attr("GripperConfig"); + py::class_(hw, "FHConfig", gripper_config) .def(py::init<>()) .def_readwrite("grasping_width", &rcs::hw::FHConfig::grasping_width) .def_readwrite("speed", &rcs::hw::FHConfig::speed) @@ -75,7 +78,8 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("epsilon_outer", &rcs::hw::FHConfig::epsilon_outer) .def_readwrite("async_control", &rcs::hw::FHConfig::async_control); - py::class_(hw, "FHState") + py::object gripper_state = (py::object) py::module_::import("rcs").attr("common").attr("GripperState"); + py::class_(hw, "FHState", gripper_state) .def(py::init<>()) .def_readonly("width", &rcs::hw::FHState::width) .def_readonly("is_grasped", &rcs::hw::FHState::is_grasped) @@ -87,8 +91,9 @@ PYBIND11_MODULE(_core, m) { &rcs::hw::FHState::max_unnormalized_width) .def_readonly("temperature", &rcs::hw::FHState::temperature); - py::class_>( - hw, "FR3") + py::object robot = (py::object) py::module_::import("rcs").attr("common").attr("Robot"); + py::class_>( + hw, "FR3", robot) .def(py::init>>(), py::arg("ip"), py::arg("ik") = std::nullopt) @@ -117,8 +122,9 @@ PYBIND11_MODULE(_core, m) { &rcs::hw::FR3::set_cartesian_position_internal, py::arg("pose"), py::arg("max_time"), py::arg("elbow"), py::arg("max_force") = 5); - py::class_>(hw, "FrankaHand") + py::object gripper = (py::object) py::module_::import("rcs").attr("common").attr("Gripper"); + py::class_>(hw, "FrankaHand", gripper) .def(py::init(), py::arg("ip"), py::arg("cfg")) .def("get_parameters", &rcs::hw::FrankaHand::get_parameters) diff --git a/extensions/rcs_fr3/src/rcs_fr3/creators.py b/extensions/rcs_fr3/src/rcs_fr3/creators.py index a50b4085..ab5d54a2 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/creators.py +++ b/extensions/rcs_fr3/src/rcs_fr3/creators.py @@ -47,12 +47,12 @@ def __call__( # type: ignore Args: ip (str): IP address of the robot. control_mode (ControlMode): Control mode for the robot. - robot_cfg (rcs.hw.FR3Config): Configuration for the FR3 robot. + robot_cfg (hw.FR3Config): Configuration for the FR3 robot. collision_guard (str | PathLike | None): Key to a built-in scene - robot_cfg (rcs.hw.FR3Config): Configuration for the FR3 robot. + robot_cfg (hw.FR3Config): Configuration for the FR3 robot. collision_guard (str | PathLike | None): Key to a scene (requires UTN compatible scene package to be present) or the path to a mujoco scene for collision guarding. If None, collision guarding is not used. - gripper_cfg (rcs.hw.FHConfig | None): Configuration for the gripper. If None, no gripper is used. + gripper_cfg (hw.FHConfig | None): Configuration for the gripper. If None, no gripper is used. camera_set (BaseHardwareCameraSet | None): Camera set to be used. If None, no cameras are used. max_relative_movement (float | tuple[float, float] | None): Maximum allowed movement. If float, it restricts translational movement in meters. If tuple, it restricts both translational (in meters) and rotational From e0e2e8e618b5fa9e72aa51c9b52f209a0cfb207e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Wed, 25 Jun 2025 10:25:30 +0200 Subject: [PATCH 22/32] style: linting and formatting --- python/rcs/camera/sim.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/python/rcs/camera/sim.py b/python/rcs/camera/sim.py index 9513df54..2400bf5c 100644 --- a/python/rcs/camera/sim.py +++ b/python/rcs/camera/sim.py @@ -8,8 +8,8 @@ from rcs._core.sim import SimCameraConfig from rcs._core.sim import SimCameraSet as _SimCameraSet from rcs.camera.interface import CameraFrame, DataFrame, Frame, FrameSet -from rcs import sim +from rcs import sim class SimCameraSet(_SimCameraSet): @@ -19,7 +19,7 @@ class SimCameraSet(_SimCameraSet): def __init__( self, - sim: sim.Sim, + simulation: sim.Sim, cameras: dict[str, SimCameraConfig], physical_units: bool = False, render_on_demand: bool = True, @@ -28,7 +28,7 @@ def __init__( self.cameras = cameras self.physical_units = physical_units - super().__init__(sim, cameras, render_on_demand=render_on_demand) + super().__init__(simulation, cameras, render_on_demand=render_on_demand) self._sim: sim.Sim def get_latest_frames(self) -> FrameSet | None: From ce526faebc306574656eee748ed53ff9bfbee6c1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Wed, 25 Jun 2025 10:31:09 +0200 Subject: [PATCH 23/32] pipeline: added fr3 compile check --- .github/workflows/py.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/py.yaml b/.github/workflows/py.yaml index 15fb1241..00973b74 100644 --- a/.github/workflows/py.yaml +++ b/.github/workflows/py.yaml @@ -48,6 +48,8 @@ jobs: run: python -m pip install wheel - name: Install the package run: python -m pip install --no-build-isolation . + - name: Install the fr3 package + run: python -m pip install --no-build-isolation extensions/rcs_fr3 - name: Code linting run: make pylint - name: Check that stub files are up-to-date From 168376f4a3c0f804eb8e25e212fd3bcb2a2725f1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Wed, 25 Jun 2025 17:40:55 +0200 Subject: [PATCH 24/32] style(fr3): cpp format and stub files --- Makefile | 6 ++--- extensions/rcs_fr3/_core/hw/__init__.pyi | 6 +---- extensions/rcs_fr3/src/pybind/rcs.cpp | 30 ++++++++++++++---------- 3 files changed, 21 insertions(+), 21 deletions(-) diff --git a/Makefile b/Makefile index 52860ca1..db415535 100644 --- a/Makefile +++ b/Makefile @@ -35,16 +35,14 @@ stubgen: find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/typing\.Literal[\1]/g' find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/\1/g' ruff check --fix python/rcs/_core - isort python/rcs/_core - black python/rcs/_core pybind11-stubgen -o extensions --numpy-array-use-type-var rcs_fr3 find ./extensions/rcs_fr3 -not -path "./extensions/rcs_fr3/_core/*" -name '*.pyi' -delete find ./extensions/rcs_fr3 -name '*.pyi' -print | xargs sed -i '1s/^/# ATTENTION: auto generated from C++ code, use `make stubgen` to update!\n/' find ./extensions/rcs_fr3/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/typing\.Literal[\1]/g' find ./extensions/rcs_fr3/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/\1/g' ruff check --fix extensions/rcs_fr3/_core - isort extensions/rcs_fr3/_core - black extensions/rcs_fr3/_core + isort python/rcs/_core extensions/rcs_fr3/_core + black python/rcs/_core extensions/rcs_fr3/_core # Python pycheckformat: diff --git a/extensions/rcs_fr3/_core/hw/__init__.pyi b/extensions/rcs_fr3/_core/hw/__init__.pyi index bca1aeb6..e9e59105 100644 --- a/extensions/rcs_fr3/_core/hw/__init__.pyi +++ b/extensions/rcs_fr3/_core/hw/__init__.pyi @@ -62,11 +62,7 @@ class FR3(rcs._core.common.Robot): def get_state(self) -> FR3State: ... def osc_set_cartesian_position(self, desired_pos_EE_in_base_frame: rcs._core.common.Pose) -> None: ... def set_cartesian_position_ik( - self, - pose: rcs._core.common.Pose, - max_time: float, - elbow: float | None, - max_force: float | None = 5, + self, pose: rcs._core.common.Pose, max_time: float, elbow: float | None, max_force: float | None = 5 ) -> None: ... def set_cartesian_position_internal(self, pose: rcs._core.common.Pose) -> None: ... def set_default_robot_behavior(self) -> None: ... diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp index d1ff5bc8..5fcb37c7 100644 --- a/extensions/rcs_fr3/src/pybind/rcs.cpp +++ b/extensions/rcs_fr3/src/pybind/rcs.cpp @@ -42,9 +42,9 @@ PYBIND11_MODULE(_core, m) { // HARDWARE MODULE auto hw = m.def_submodule("hw", "rcs fr3 module"); - py::object robot_state = (py::object) py::module_::import("rcs").attr("common").attr("RobotState"); - py::class_(hw, "FR3State", robot_state) - .def(py::init<>()); + py::object robot_state = + (py::object)py::module_::import("rcs").attr("common").attr("RobotState"); + py::class_(hw, "FR3State", robot_state).def(py::init<>()); py::class_(hw, "FR3Load") .def(py::init<>()) .def_readwrite("load_mass", &rcs::hw::FR3Load::load_mass) @@ -56,7 +56,8 @@ PYBIND11_MODULE(_core, m) { .value("rcs_ik", rcs::hw::IKSolver::rcs_ik) .export_values(); - py::object robot_config = (py::object) py::module_::import("rcs").attr("common").attr("RobotConfig"); + py::object robot_config = + (py::object)py::module_::import("rcs").attr("common").attr("RobotConfig"); py::class_(hw, "FR3Config", robot_config) .def(py::init<>()) .def_readwrite("ik_solver", &rcs::hw::FR3Config::ik_solver) @@ -68,7 +69,9 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("tcp_offset", &rcs::hw::FR3Config::tcp_offset) .def_readwrite("async_control", &rcs::hw::FR3Config::async_control); - py::object gripper_config = (py::object) py::module_::import("rcs").attr("common").attr("GripperConfig"); + py::object gripper_config = + (py::object)py::module_::import("rcs").attr("common").attr( + "GripperConfig"); py::class_(hw, "FHConfig", gripper_config) .def(py::init<>()) .def_readwrite("grasping_width", &rcs::hw::FHConfig::grasping_width) @@ -78,7 +81,9 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("epsilon_outer", &rcs::hw::FHConfig::epsilon_outer) .def_readwrite("async_control", &rcs::hw::FHConfig::async_control); - py::object gripper_state = (py::object) py::module_::import("rcs").attr("common").attr("GripperState"); + py::object gripper_state = + (py::object)py::module_::import("rcs").attr("common").attr( + "GripperState"); py::class_(hw, "FHState", gripper_state) .def(py::init<>()) .def_readonly("width", &rcs::hw::FHState::width) @@ -91,9 +96,9 @@ PYBIND11_MODULE(_core, m) { &rcs::hw::FHState::max_unnormalized_width) .def_readonly("temperature", &rcs::hw::FHState::temperature); - py::object robot = (py::object) py::module_::import("rcs").attr("common").attr("Robot"); - py::class_>( - hw, "FR3", robot) + py::object robot = + (py::object)py::module_::import("rcs").attr("common").attr("Robot"); + py::class_>(hw, "FR3", robot) .def(py::init>>(), py::arg("ip"), py::arg("ik") = std::nullopt) @@ -122,9 +127,10 @@ PYBIND11_MODULE(_core, m) { &rcs::hw::FR3::set_cartesian_position_internal, py::arg("pose"), py::arg("max_time"), py::arg("elbow"), py::arg("max_force") = 5); - py::object gripper = (py::object) py::module_::import("rcs").attr("common").attr("Gripper"); - py::class_>(hw, "FrankaHand", gripper) + py::object gripper = + (py::object)py::module_::import("rcs").attr("common").attr("Gripper"); + py::class_>( + hw, "FrankaHand", gripper) .def(py::init(), py::arg("ip"), py::arg("cfg")) .def("get_parameters", &rcs::hw::FrankaHand::get_parameters) From 46210cdccbc19ad0985cddef69938deb595956eb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 26 Jun 2025 13:32:42 +0200 Subject: [PATCH 25/32] refactor(fr3): moved stubs to correct location --- extensions/rcs_fr3/{ => src/rcs_fr3}/_core/__init__.pyi | 0 extensions/rcs_fr3/{ => src/rcs_fr3}/_core/hw/__init__.pyi | 0 extensions/rcs_fr3/{ => src/rcs_fr3}/_core/hw/exceptions.pyi | 0 3 files changed, 0 insertions(+), 0 deletions(-) rename extensions/rcs_fr3/{ => src/rcs_fr3}/_core/__init__.pyi (100%) rename extensions/rcs_fr3/{ => src/rcs_fr3}/_core/hw/__init__.pyi (100%) rename extensions/rcs_fr3/{ => src/rcs_fr3}/_core/hw/exceptions.pyi (100%) diff --git a/extensions/rcs_fr3/_core/__init__.pyi b/extensions/rcs_fr3/src/rcs_fr3/_core/__init__.pyi similarity index 100% rename from extensions/rcs_fr3/_core/__init__.pyi rename to extensions/rcs_fr3/src/rcs_fr3/_core/__init__.pyi diff --git a/extensions/rcs_fr3/_core/hw/__init__.pyi b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi similarity index 100% rename from extensions/rcs_fr3/_core/hw/__init__.pyi rename to extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi diff --git a/extensions/rcs_fr3/_core/hw/exceptions.pyi b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/exceptions.pyi similarity index 100% rename from extensions/rcs_fr3/_core/hw/exceptions.pyi rename to extensions/rcs_fr3/src/rcs_fr3/_core/hw/exceptions.pyi From 3146654ebebab8fe8546c8dd6dd74de9223047f8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 26 Jun 2025 13:41:30 +0200 Subject: [PATCH 26/32] pipeline: adapted makefile to match new fr3 _core location --- Makefile | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/Makefile b/Makefile index db415535..17a38542 100644 --- a/Makefile +++ b/Makefile @@ -38,11 +38,13 @@ stubgen: pybind11-stubgen -o extensions --numpy-array-use-type-var rcs_fr3 find ./extensions/rcs_fr3 -not -path "./extensions/rcs_fr3/_core/*" -name '*.pyi' -delete find ./extensions/rcs_fr3 -name '*.pyi' -print | xargs sed -i '1s/^/# ATTENTION: auto generated from C++ code, use `make stubgen` to update!\n/' - find ./extensions/rcs_fr3/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/typing\.Literal[\1]/g' - find ./extensions/rcs_fr3/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/\1/g' - ruff check --fix extensions/rcs_fr3/_core - isort python/rcs/_core extensions/rcs_fr3/_core - black python/rcs/_core extensions/rcs_fr3/_core + find ./extensions/rcs_fr3 -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/typing\.Literal[\1]/g' + find ./extensions/rcs_fr3 -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/\1/g' + rm -r extensions/rcs_fr3/src/rcs_fr3/_core + mv extensions/rcs_fr3/_core/ extensions/rcs_fr3/src/rcs_fr3/ + ruff check --fix extensions/rcs_fr3/src/rcs_fr3/_core + isort python/rcs/_core extensions/rcs_fr3/src/rcs_fr3/_core + black python/rcs/_core extensions/rcs_fr3/src/rcs_fr3/_core # Python pycheckformat: From c0e99a3695653ee224ed529365516afa2648e113 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 27 Jun 2025 10:39:28 +0200 Subject: [PATCH 27/32] fix(cmake): added pinocchio link dep --- extensions/rcs_fr3/CMakeLists.txt | 1 + extensions/rcs_fr3/cmake/Findpinocchio.cmake | 23 ++++++++++++++++++-- extensions/rcs_fr3/pyproject.toml | 2 +- extensions/rcs_fr3/src/hw/CMakeLists.txt | 2 +- extensions/rcs_fr3/src/pybind/CMakeLists.txt | 4 ++-- src/rcs/CMakeLists.txt | 2 +- 6 files changed, 27 insertions(+), 7 deletions(-) diff --git a/extensions/rcs_fr3/CMakeLists.txt b/extensions/rcs_fr3/CMakeLists.txt index 62bb2ad4..89fa86cf 100644 --- a/extensions/rcs_fr3/CMakeLists.txt +++ b/extensions/rcs_fr3/CMakeLists.txt @@ -45,6 +45,7 @@ include(FetchContent) find_package(Eigen3 REQUIRED) find_package(Python3 COMPONENTS Interpreter Development REQUIRED) +find_package(pinocchio REQUIRED) find_package(rcs REQUIRED) FetchContent_Declare(rl diff --git a/extensions/rcs_fr3/cmake/Findpinocchio.cmake b/extensions/rcs_fr3/cmake/Findpinocchio.cmake index d1da5ba9..ef049517 100644 --- a/extensions/rcs_fr3/cmake/Findpinocchio.cmake +++ b/extensions/rcs_fr3/cmake/Findpinocchio.cmake @@ -18,7 +18,7 @@ if (NOT pinocchio_FOUND) endif() # Check if the library file exists - cmake_path(APPEND Python3_SITELIB cmeel.prefix lib libpinocchio.so OUTPUT_VARIABLE pinocchio_library_path) + cmake_path(APPEND Python3_SITELIB cmeel.prefix lib libpinocchio_default.so OUTPUT_VARIABLE pinocchio_library_path) if (NOT EXISTS ${pinocchio_library_path}) set(pinocchio_FOUND FALSE) if (pinocchio_FIND_REQUIRED) @@ -27,6 +27,16 @@ if (NOT pinocchio_FOUND) return() endif() + # Check if the library file exists + cmake_path(APPEND Python3_SITELIB cmeel.prefix lib libpinocchio_parsers.so OUTPUT_VARIABLE pinocchio_parsers_path) + if (NOT EXISTS ${pinocchio_parsers_path}) + set(pinocchio_FOUND FALSE) + if (pinocchio_FIND_REQUIRED) + message(FATAL_ERROR "Could not find pinocchio parsers path. Please install pinocchio using pip.") + endif() + return() + endif() + # Extract version from the library filename file(GLOB pinocchio_dist_info "${Python3_SITELIB}/pin-*.dist-info") cmake_path(GET pinocchio_dist_info FILENAME pinocchio_library_filename) @@ -38,8 +48,17 @@ if (NOT pinocchio_FOUND) target_include_directories(pinocchio::pinocchio INTERFACE ${pinocchio_INCLUDE_DIRS}) set_target_properties( pinocchio::pinocchio - PROPERTIES + PROPERTIES IMPORTED_LOCATION "${pinocchio_library_path}" ) + + add_library(pinocchio_parsers SHARED IMPORTED) + target_include_directories(pinocchio_parsers INTERFACE ${pinocchio_INCLUDE_DIRS}) + set_target_properties( + pinocchio_parsers + PROPERTIES + IMPORTED_LOCATION "${pinocchio_parsers_path}" + ) set(pinocchio_FOUND TRUE) + endif() diff --git a/extensions/rcs_fr3/pyproject.toml b/extensions/rcs_fr3/pyproject.toml index 3eae2aa4..f80948d3 100644 --- a/extensions/rcs_fr3/pyproject.toml +++ b/extensions/rcs_fr3/pyproject.toml @@ -8,7 +8,7 @@ version = "0.4.0" description="RCS libfranka integration" dependencies = [ # NOTE: when changing the pin version, also change it in requirements_dev.txt - "pin==2.7.0", + "pin==3.7.0", ] readme = "../../README.md" maintainers = [ diff --git a/extensions/rcs_fr3/src/hw/CMakeLists.txt b/extensions/rcs_fr3/src/hw/CMakeLists.txt index aef503cc..8755cd6e 100644 --- a/extensions/rcs_fr3/src/hw/CMakeLists.txt +++ b/extensions/rcs_fr3/src/hw/CMakeLists.txt @@ -2,7 +2,7 @@ add_library(hw) target_sources(hw PRIVATE FR3.cpp FrankaHand.cpp FR3MotionGenerator.cpp) target_link_libraries( hw - PUBLIC franka rcs + PUBLIC franka rcs pinocchio::pinocchio pinocchio_parsers PRIVATE math ) diff --git a/extensions/rcs_fr3/src/pybind/CMakeLists.txt b/extensions/rcs_fr3/src/pybind/CMakeLists.txt index 91d134c0..cfeb5c12 100644 --- a/extensions/rcs_fr3/src/pybind/CMakeLists.txt +++ b/extensions/rcs_fr3/src/pybind/CMakeLists.txt @@ -1,5 +1,5 @@ pybind11_add_module(_core MODULE rcs.cpp) -target_link_libraries(_core PRIVATE hw rcs mdl) +target_link_libraries(_core PRIVATE hw rcs mdl pinocchio::pinocchio pinocchio_parsers) target_compile_definitions(_core PRIVATE VERSION_INFO=${PROJECT_VERSION}) set_target_properties(franka PROPERTIES @@ -7,7 +7,7 @@ set_target_properties(franka PROPERTIES INTERPROCEDURAL_OPTIMIZATION TRUE ) set_target_properties(_core PROPERTIES - INSTALL_RPATH "$ORIGIN;$ORIGIN/../rcs" + INSTALL_RPATH "$ORIGIN;$ORIGIN/../rcs;$ORIGIN/../cmeel.prefix/lib" INTERPROCEDURAL_OPTIMIZATION TRUE ) # in pip diff --git a/src/rcs/CMakeLists.txt b/src/rcs/CMakeLists.txt index 831e8a5b..49ab913a 100644 --- a/src/rcs/CMakeLists.txt +++ b/src/rcs/CMakeLists.txt @@ -1,4 +1,4 @@ add_library(rcs SHARED) target_include_directories(rcs PUBLIC ${CMAKE_SOURCE_DIR}/include) target_sources(rcs PRIVATE Pose.cpp Robot.cpp IK.cpp utils.cpp) -target_link_libraries(rcs PUBLIC Eigen3::Eigen mdl) +target_link_libraries(rcs PUBLIC Eigen3::Eigen mdl pinocchio::pinocchio pinocchio_parsers) From 14cd417989b4710632b8789bcca65be4ebecae54 Mon Sep 17 00:00:00 2001 From: Pierre Krack Date: Fri, 27 Jun 2025 11:24:19 +0200 Subject: [PATCH 28/32] Fix librcs RPATH --- cmake/Findpinocchio.cmake | 22 +++++++++++++--------- src/rcs/CMakeLists.txt | 5 ++++- 2 files changed, 17 insertions(+), 10 deletions(-) diff --git a/cmake/Findpinocchio.cmake b/cmake/Findpinocchio.cmake index 3cf7d3f1..fcefcf5c 100644 --- a/cmake/Findpinocchio.cmake +++ b/cmake/Findpinocchio.cmake @@ -46,19 +46,23 @@ if (NOT pinocchio_FOUND) # Create the imported target add_library(pinocchio::pinocchio SHARED IMPORTED) target_include_directories(pinocchio::pinocchio INTERFACE ${pinocchio_INCLUDE_DIRS}) - set_target_properties( - pinocchio::pinocchio - PROPERTIES + set_target_properties(pinocchio::pinocchio + PROPERTIES IMPORTED_LOCATION "${pinocchio_library_path}" ) - - add_library(pinocchio_parsers SHARED IMPORTED) - target_include_directories(pinocchio_parsers INTERFACE ${pinocchio_INCLUDE_DIRS}) - set_target_properties( - pinocchio_parsers - PROPERTIES + + add_library(pinocchio::parsers SHARED IMPORTED) + target_include_directories(pinocchio::parsers INTERFACE ${pinocchio_INCLUDE_DIRS}) + set_target_properties(pinocchio::parsers + PROPERTIES IMPORTED_LOCATION "${pinocchio_parsers_path}" ) + + add_library(pinocchio::all INTERFACE IMPORTED) + set_target_properties(pinocchio::all + PROPERTIES + INTERFACE_LINK_LIBRARIES "pinocchio::pinocchio;pinocchio::parsers" + ) set(pinocchio_FOUND TRUE) endif() diff --git a/src/rcs/CMakeLists.txt b/src/rcs/CMakeLists.txt index 49ab913a..cd8b937c 100644 --- a/src/rcs/CMakeLists.txt +++ b/src/rcs/CMakeLists.txt @@ -1,4 +1,7 @@ add_library(rcs SHARED) target_include_directories(rcs PUBLIC ${CMAKE_SOURCE_DIR}/include) target_sources(rcs PRIVATE Pose.cpp Robot.cpp IK.cpp utils.cpp) -target_link_libraries(rcs PUBLIC Eigen3::Eigen mdl pinocchio::pinocchio pinocchio_parsers) +target_link_libraries(rcs PUBLIC Eigen3::Eigen mdl pinocchio::all) +set_target_properties(rcs PROPERTIES + INSTALL_RPATH "$ORIGIN;$ORIGIN/../cmeel.prefix/lib" +) From 22940455996d550f4e2f3e4559b9991102a79c62 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 27 Jun 2025 14:20:03 +0200 Subject: [PATCH 29/32] build(fr3): fix pinocchio linking, fix stubfiles --- Makefile | 4 ++-- extensions/rcs_fr3/cmake/Findpinocchio.cmake | 20 ++++++++++++-------- extensions/rcs_fr3/src/hw/CMakeLists.txt | 2 +- extensions/rcs_fr3/src/pybind/CMakeLists.txt | 2 +- extensions/rcs_fr3/src/rcs_fr3/creators.py | 4 ++-- python/examples/fr3.py | 2 +- 6 files changed, 19 insertions(+), 15 deletions(-) diff --git a/Makefile b/Makefile index e19d0e2f..206b81e6 100644 --- a/Makefile +++ b/Makefile @@ -38,8 +38,8 @@ stubgen: pybind11-stubgen -o extensions --numpy-array-use-type-var rcs_fr3 find ./extensions/rcs_fr3 -not -path "./extensions/rcs_fr3/_core/*" -name '*.pyi' -delete find ./extensions/rcs_fr3 -name '*.pyi' -print | xargs sed -i '1s/^/# ATTENTION: auto generated from C++ code, use `make stubgen` to update!\n/' - find ./extensions/rcs_fr3 -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/typing\.Literal[\1]/g' - find ./extensions/rcs_fr3 -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/\1/g' + find ./extensions/rcs_fr3 -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/tuple\[typing\.Literal[\1]\]/g' + find ./extensions/rcs_fr3 -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/tuple\[\1\]/g' rm -r extensions/rcs_fr3/src/rcs_fr3/_core mv extensions/rcs_fr3/_core/ extensions/rcs_fr3/src/rcs_fr3/ ruff check --fix extensions/rcs_fr3/src/rcs_fr3/_core diff --git a/extensions/rcs_fr3/cmake/Findpinocchio.cmake b/extensions/rcs_fr3/cmake/Findpinocchio.cmake index ef049517..fcefcf5c 100644 --- a/extensions/rcs_fr3/cmake/Findpinocchio.cmake +++ b/extensions/rcs_fr3/cmake/Findpinocchio.cmake @@ -46,19 +46,23 @@ if (NOT pinocchio_FOUND) # Create the imported target add_library(pinocchio::pinocchio SHARED IMPORTED) target_include_directories(pinocchio::pinocchio INTERFACE ${pinocchio_INCLUDE_DIRS}) - set_target_properties( - pinocchio::pinocchio - PROPERTIES + set_target_properties(pinocchio::pinocchio + PROPERTIES IMPORTED_LOCATION "${pinocchio_library_path}" ) - - add_library(pinocchio_parsers SHARED IMPORTED) - target_include_directories(pinocchio_parsers INTERFACE ${pinocchio_INCLUDE_DIRS}) - set_target_properties( - pinocchio_parsers + + add_library(pinocchio::parsers SHARED IMPORTED) + target_include_directories(pinocchio::parsers INTERFACE ${pinocchio_INCLUDE_DIRS}) + set_target_properties(pinocchio::parsers PROPERTIES IMPORTED_LOCATION "${pinocchio_parsers_path}" ) + + add_library(pinocchio::all INTERFACE IMPORTED) + set_target_properties(pinocchio::all + PROPERTIES + INTERFACE_LINK_LIBRARIES "pinocchio::pinocchio;pinocchio::parsers" + ) set(pinocchio_FOUND TRUE) endif() diff --git a/extensions/rcs_fr3/src/hw/CMakeLists.txt b/extensions/rcs_fr3/src/hw/CMakeLists.txt index 8755cd6e..51b0a173 100644 --- a/extensions/rcs_fr3/src/hw/CMakeLists.txt +++ b/extensions/rcs_fr3/src/hw/CMakeLists.txt @@ -2,7 +2,7 @@ add_library(hw) target_sources(hw PRIVATE FR3.cpp FrankaHand.cpp FR3MotionGenerator.cpp) target_link_libraries( hw - PUBLIC franka rcs pinocchio::pinocchio pinocchio_parsers + PUBLIC franka rcs pinocchio::all PRIVATE math ) diff --git a/extensions/rcs_fr3/src/pybind/CMakeLists.txt b/extensions/rcs_fr3/src/pybind/CMakeLists.txt index cfeb5c12..3a409fec 100644 --- a/extensions/rcs_fr3/src/pybind/CMakeLists.txt +++ b/extensions/rcs_fr3/src/pybind/CMakeLists.txt @@ -1,5 +1,5 @@ pybind11_add_module(_core MODULE rcs.cpp) -target_link_libraries(_core PRIVATE hw rcs mdl pinocchio::pinocchio pinocchio_parsers) +target_link_libraries(_core PRIVATE hw rcs mdl pinocchio::all) target_compile_definitions(_core PRIVATE VERSION_INFO=${PROJECT_VERSION}) set_target_properties(franka PROPERTIES diff --git a/extensions/rcs_fr3/src/rcs_fr3/creators.py b/extensions/rcs_fr3/src/rcs_fr3/creators.py index ab5d54a2..68df13db 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/creators.py +++ b/extensions/rcs_fr3/src/rcs_fr3/creators.py @@ -65,7 +65,7 @@ def __call__( # type: ignore """ if urdf_path is None: urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] - ik = rcs.common.IK(str(urdf_path)) if urdf_path is not None else None + ik = rcs.common.RL(str(urdf_path)) if urdf_path is not None else None robot = hw.FR3(ip, ik) robot.set_parameters(robot_cfg) @@ -117,7 +117,7 @@ def __call__( # type: ignore ) -> gym.Env: urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] - ik = rcs.common.IK(str(urdf_path)) if urdf_path is not None else None + ik = rcs.common.RL(str(urdf_path)) if urdf_path is not None else None robots: dict[str, hw.FR3] = {} for ip in ips: robots[ip] = hw.FR3(ip, ik) diff --git a/python/examples/fr3.py b/python/examples/fr3.py index 42a815c3..0995c951 100644 --- a/python/examples/fr3.py +++ b/python/examples/fr3.py @@ -94,7 +94,7 @@ def main(): else: urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] - ik = rcs.common.IK(str(urdf_path)) + ik = rcs.common.RL(str(urdf_path)) robot = hw.FR3(ROBOT_IP, ik) robot_cfg = FR3Config() robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) From c2c59e30d989e6a1f38abb37697e4951e81f1cab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 27 Jun 2025 19:37:19 +0200 Subject: [PATCH 30/32] refactor: move examples into top level --- Makefile | 12 ++++++------ README.md | 2 +- {python/examples => examples}/__init__.py | 0 .../examples => examples}/env_cartesian_control.py | 0 .../env_cartesian_control_digit.py | 0 .../env_cartesian_control_tilburg.py | 0 {python/examples => examples}/env_joint_control.py | 0 {python/examples => examples}/fr3.py | 0 {python/examples => examples}/grasp_demo.py | 0 {python/examples => examples}/grasp_demo_lab.py | 0 10 files changed, 7 insertions(+), 7 deletions(-) rename {python/examples => examples}/__init__.py (100%) rename {python/examples => examples}/env_cartesian_control.py (100%) rename {python/examples => examples}/env_cartesian_control_digit.py (100%) rename {python/examples => examples}/env_cartesian_control_tilburg.py (100%) rename {python/examples => examples}/env_joint_control.py (100%) rename {python/examples => examples}/fr3.py (100%) rename {python/examples => examples}/grasp_demo.py (100%) rename {python/examples => examples}/grasp_demo_lab.py (100%) diff --git a/Makefile b/Makefile index 206b81e6..c6510394 100644 --- a/Makefile +++ b/Makefile @@ -48,20 +48,20 @@ stubgen: # Python pycheckformat: - isort --check-only ${PYSRC} extensions - black --check ${PYSRC} extensions + isort --check-only ${PYSRC} extensions examples + black --check ${PYSRC} extensions examples pyformat: - isort ${PYSRC} extensions - black ${PYSRC} extensions + isort ${PYSRC} extensions examples + black ${PYSRC} extensions examples pylint: ruff mypy ruff: - ruff check ${PYSRC} extensions + ruff check ${PYSRC} extensions examples mypy: - mypy ${PYSRC} extensions --install-types --non-interactive --no-namespace-packages --exclude 'build' + mypy ${PYSRC} extensions examples --install-types --non-interactive --no-namespace-packages --exclude 'build' pytest: pytest -vv diff --git a/README.md b/README.md index 2a3c7ec8..ee3b5d61 100644 --- a/README.md +++ b/README.md @@ -26,7 +26,7 @@ Import the library in python: ```python import rcs ``` -Checkout the python examples that we provide in [python/examples](python/examples): +Checkout the python examples that we provide in [examples](examples): - [fr3.py](python/examples/fr3.py) shows direct robot control with RCS's python bindings - [env_joint_control.py](python/examples/env_joint_control.py) and [env_cartesian_control.py](python/examples/env_cartesian_control.py) demonstrates RCS's high level [gymnasium](https://gymnasium.farama.org/) interface both for joint- and end effector space control All of these examples work both in the MuJoCo simulation as well as on your hardware FR3. diff --git a/python/examples/__init__.py b/examples/__init__.py similarity index 100% rename from python/examples/__init__.py rename to examples/__init__.py diff --git a/python/examples/env_cartesian_control.py b/examples/env_cartesian_control.py similarity index 100% rename from python/examples/env_cartesian_control.py rename to examples/env_cartesian_control.py diff --git a/python/examples/env_cartesian_control_digit.py b/examples/env_cartesian_control_digit.py similarity index 100% rename from python/examples/env_cartesian_control_digit.py rename to examples/env_cartesian_control_digit.py diff --git a/python/examples/env_cartesian_control_tilburg.py b/examples/env_cartesian_control_tilburg.py similarity index 100% rename from python/examples/env_cartesian_control_tilburg.py rename to examples/env_cartesian_control_tilburg.py diff --git a/python/examples/env_joint_control.py b/examples/env_joint_control.py similarity index 100% rename from python/examples/env_joint_control.py rename to examples/env_joint_control.py diff --git a/python/examples/fr3.py b/examples/fr3.py similarity index 100% rename from python/examples/fr3.py rename to examples/fr3.py diff --git a/python/examples/grasp_demo.py b/examples/grasp_demo.py similarity index 100% rename from python/examples/grasp_demo.py rename to examples/grasp_demo.py diff --git a/python/examples/grasp_demo_lab.py b/examples/grasp_demo_lab.py similarity index 100% rename from python/examples/grasp_demo_lab.py rename to examples/grasp_demo_lab.py From 6e75f2e4e2968ca5ee930227563d7f9353222aee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 27 Jun 2025 20:29:24 +0200 Subject: [PATCH 31/32] chore: clean up examples --- examples/env_cartesian_control_digit.py | 102 ------------------ examples/env_cartesian_control_tilburg.py | 102 ------------------ examples/{fr3.py => fr3_direct_control.py} | 15 ++- ...ontrol.py => fr3_env_cartesian_control.py} | 10 +- ...nt_control.py => fr3_env_joint_control.py} | 13 ++- examples/grasp_demo.py | 12 +-- examples/grasp_demo_lab.py | 96 ----------------- 7 files changed, 23 insertions(+), 327 deletions(-) delete mode 100644 examples/env_cartesian_control_digit.py delete mode 100644 examples/env_cartesian_control_tilburg.py rename examples/{fr3.py => fr3_direct_control.py} (96%) rename examples/{env_cartesian_control.py => fr3_env_cartesian_control.py} (94%) rename examples/{env_joint_control.py => fr3_env_joint_control.py} (91%) delete mode 100644 examples/grasp_demo_lab.py diff --git a/examples/env_cartesian_control_digit.py b/examples/env_cartesian_control_digit.py deleted file mode 100644 index e624b121..00000000 --- a/examples/env_cartesian_control_digit.py +++ /dev/null @@ -1,102 +0,0 @@ -import logging -import typing - -from rcs._core.common import RobotPlatform -from rcs.camera.hw import HardwareCamera, HardwareCameraSet -from rcs.envs.base import ControlMode, RelativeTo -from rcs.envs.creators import SimEnvCreator -from rcs.envs.utils import ( - default_digit, - default_mujoco_cameraset_cfg, - default_sim_gripper_cfg, - default_sim_robot_cfg, -) -from rcs_fr3.creators import RCSFR3EnvCreator -from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_fr3_desk -from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg - -logger = logging.getLogger(__name__) -logger.setLevel(logging.INFO) - -ROBOT_IP = "192.168.101.1" -ROBOT_INSTANCE = RobotPlatform.SIMULATION - - -""" -Create a .env file in the same directory as this file with the following content: -FR3_USERNAME= -FR3_PASSWORD= - -When you use a real FR3 you first need to unlock its joints using the following cli script: - -python -m rcs.fr3 unlock - -or put it into guiding mode using: - -python -m rcs.fr3 guiding-mode - -When you are done you lock it again using: - -python -m rcs.fr3 lock - -or even shut it down using: - -python -m rcs.fr3 shutdown -""" - - -def main(): - resource_manger: FCI | ContextManager - if ROBOT_INSTANCE == RobotPlatform.HARDWARE: - user, pw = load_creds_fr3_desk() - resource_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False) - else: - resource_manger = ContextManager() - with resource_manger: - if ROBOT_INSTANCE == RobotPlatform.HARDWARE: - camera_set = typing.cast(HardwareCamera, default_digit({"digit_0": "D21182"})) - env_rel = RCSFR3EnvCreator()( - ip=ROBOT_IP, - control_mode=ControlMode.CARTESIAN_TQuat, - robot_cfg=default_fr3_hw_robot_cfg(), - collision_guard="lab", - camera_set=HardwareCameraSet([camera_set]), - gripper_cfg=default_fr3_hw_gripper_cfg(), - max_relative_movement=0.5, - relative_to=RelativeTo.LAST_STEP, - ) - else: - env_rel = SimEnvCreator()( - control_mode=ControlMode.CARTESIAN_TQuat, - robot_cfg=default_sim_robot_cfg(), - collision_guard=False, - gripper_cfg=default_sim_gripper_cfg(), - cameras=default_mujoco_cameraset_cfg(), - max_relative_movement=0.5, - relative_to=RelativeTo.LAST_STEP, - ) - env_rel.get_wrapper_attr("sim").open_gui() - - env_rel.reset() - print(env_rel.unwrapped.robot.get_cartesian_position()) # type: ignore - - for _ in range(10): - for _ in range(10): - # move 1cm in x direction (forward) and close gripper - act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 0} - obs, reward, terminated, truncated, info = env_rel.step(act) - if truncated or terminated: - logger.info("Truncated or terminated!") - return - for _ in range(10): - # move 1cm in negative x direction (backward) and open gripper - act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": 1} - obs, reward, terminated, truncated, info = env_rel.step(act) - if truncated or terminated: - logger.info("Truncated or terminated!") - return - env_rel.close() - - -if __name__ == "__main__": - main() diff --git a/examples/env_cartesian_control_tilburg.py b/examples/env_cartesian_control_tilburg.py deleted file mode 100644 index 4aac2359..00000000 --- a/examples/env_cartesian_control_tilburg.py +++ /dev/null @@ -1,102 +0,0 @@ -import logging - -from rcs._core.common import RobotPlatform -from rcs.envs.base import ControlMode, RelativeTo -from rcs.envs.creators import SimEnvCreator -from rcs.envs.utils import ( - default_mujoco_cameraset_cfg, - default_sim_gripper_cfg, - default_sim_robot_cfg, - default_tilburg_hw_hand_cfg, -) -from rcs_fr3.creators import RCSFR3EnvCreator -from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_fr3_desk -from rcs_fr3.utils import default_fr3_hw_robot_cfg - -logger = logging.getLogger(__name__) -logger.setLevel(logging.INFO) - -ROBOT_IP = "192.168.101.1" -ROBOT_INSTANCE = RobotPlatform.SIMULATION - - -""" -Create a .env file in the same directory as this file with the following content: -FR3_USERNAME= -FR3_PASSWORD= - -When you use a real FR3 you first need to unlock its joints using the following cli script: - -python -m rcs.fr3 unlock - -or put it into guiding mode using: - -python -m rcs.fr3 guiding-mode - -When you are done you lock it again using: - -python -m rcs.fr3 lock - -or even shut it down using: - -python -m rcs.fr3 shutdown -""" - - -def main(): - resource_manger: FCI | ContextManager - if ROBOT_INSTANCE == RobotPlatform.HARDWARE: - user, pw = load_creds_fr3_desk() - resource_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False) - else: - resource_manger = ContextManager() - with resource_manger: - if ROBOT_INSTANCE == RobotPlatform.HARDWARE: - env_rel = RCSFR3EnvCreator()( - ip=ROBOT_IP, - control_mode=ControlMode.CARTESIAN_TQuat, - robot_cfg=default_fr3_hw_robot_cfg(), - collision_guard="lab", - gripper_cfg=default_tilburg_hw_hand_cfg(), - max_relative_movement=0.5, - relative_to=RelativeTo.LAST_STEP, - ) - else: - env_rel = SimEnvCreator()( - control_mode=ControlMode.CARTESIAN_TQuat, - robot_cfg=default_sim_robot_cfg(), - collision_guard=False, - gripper_cfg=default_sim_gripper_cfg(), - cameras=default_mujoco_cameraset_cfg(), - max_relative_movement=0.5, - relative_to=RelativeTo.LAST_STEP, - ) - env_rel.get_wrapper_attr("sim").open_gui() - - env_rel.reset() - print(env_rel.unwrapped.robot.get_cartesian_position()) # type: ignore - close_action = 0 - open_action = 1 - - for _ in range(10): - for _ in range(10): - # move 1cm in x direction (forward) and close gripper - act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "hand": close_action} - obs, reward, terminated, truncated, info = env_rel.step(act) - if truncated or terminated: - logger.info("Truncated or terminated!") - return - from time import sleep - - sleep(5) - for _ in range(10): - # move 1cm in negative x direction (backward) and open gripper - act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1], "hand": open_action} - obs, reward, terminated, truncated, info = env_rel.step(act) - if truncated or terminated: - logger.info("Truncated or terminated!") - return - - -if __name__ == "__main__": - main() diff --git a/examples/fr3.py b/examples/fr3_direct_control.py similarity index 96% rename from examples/fr3.py rename to examples/fr3_direct_control.py index 0995c951..a428e5c8 100644 --- a/examples/fr3.py +++ b/examples/fr3_direct_control.py @@ -2,7 +2,6 @@ import numpy as np from rcs._core.common import RobotPlatform -from rcs._core.hw import FR3Config, IKSolver from rcs._core.sim import CameraType from rcs.camera.sim import SimCameraConfig, SimCameraSet from rcs_fr3._core import hw @@ -32,19 +31,19 @@ When you use a real FR3 you first need to unlock its joints using the following cli script: -python -m rcs.fr3 unlock +python -m rcs_fr3 unlock or put it into guiding mode using: -python -m rcs.fr3 guiding-mode +python -m rcs_fr3 guiding-mode When you are done you lock it again using: -python -m rcs.fr3 lock +python -m rcs_fr3 lock or even shut it down using: -python -m rcs.fr3 shutdown +python -m rcs_fr3 shutdown """ @@ -74,7 +73,7 @@ def main(): # add camera to have a rendering gui cameras = { - "default_free": SimCameraConfig( + "default_free": sim.SimCameraConfig( identifier="", type=CameraType.default_free, resolution_width=1280, @@ -96,9 +95,9 @@ def main(): urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] ik = rcs.common.RL(str(urdf_path)) robot = hw.FR3(ROBOT_IP, ik) - robot_cfg = FR3Config() + robot_cfg = hw.FR3Config() robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) - robot_cfg.ik_solver = IKSolver.rcs_ik + robot_cfg.ik_solver = hw.IKSolver.rcs_ik robot.set_parameters(robot_cfg) # type: ignore gripper_cfg_hw = hw.FHConfig() diff --git a/examples/env_cartesian_control.py b/examples/fr3_env_cartesian_control.py similarity index 94% rename from examples/env_cartesian_control.py rename to examples/fr3_env_cartesian_control.py index 83809d1a..9a460923 100644 --- a/examples/env_cartesian_control.py +++ b/examples/fr3_env_cartesian_control.py @@ -26,19 +26,19 @@ When you use a real FR3 you first need to unlock its joints using the following cli script: -python -m rcs.fr3 unlock +python -m rcs_fr3 unlock or put it into guiding mode using: -python -m rcs.fr3 guiding-mode +python -m rcs_fr3 guiding-mode When you are done you lock it again using: -python -m rcs.fr3 lock +python -m rcs_fr3 lock or even shut it down using: -python -m rcs.fr3 shutdown +python -m rcs_fr3 shutdown """ @@ -56,7 +56,7 @@ def main(): ip=ROBOT_IP, control_mode=ControlMode.CARTESIAN_TQuat, robot_cfg=default_fr3_hw_robot_cfg(), - collision_guard="lab", + collision_guard=None, gripper_cfg=default_fr3_hw_gripper_cfg(), max_relative_movement=0.5, relative_to=RelativeTo.LAST_STEP, diff --git a/examples/env_joint_control.py b/examples/fr3_env_joint_control.py similarity index 91% rename from examples/env_joint_control.py rename to examples/fr3_env_joint_control.py index 7bb45e10..a1dd2cce 100644 --- a/examples/env_joint_control.py +++ b/examples/fr3_env_joint_control.py @@ -27,19 +27,19 @@ When you use a real FR3 you first need to unlock its joints using the following cli script: -python -m rcs.fr3 unlock +python -m rcs_fr3 unlock or put it into guiding mode using: -python -m rcs.fr3 guiding-mode +python -m rcs_fr3 guiding-mode When you are done you lock it again using: -python -m rcs.fr3 lock +python -m rcs_fr3 lock or even shut it down using: -python -m rcs.fr3 shutdown +python -m rcs_fr3 shutdown """ @@ -57,7 +57,7 @@ def main(): ip=ROBOT_IP, control_mode=ControlMode.JOINTS, robot_cfg=default_fr3_hw_robot_cfg(), - collision_guard="lab", + collision_guard=None, gripper_cfg=default_fr3_hw_gripper_cfg(), max_relative_movement=np.deg2rad(5), relative_to=RelativeTo.LAST_STEP, @@ -76,14 +76,13 @@ def main(): for _ in range(10): obs, info = env_rel.reset() - for _ in range(3): + for _ in range(10): # sample random relative action and execute it act = env_rel.action_space.sample() obs, reward, terminated, truncated, info = env_rel.step(act) if truncated or terminated: logger.info("Truncated or terminated!") return - logger.info(act["gripper"], obs["gripper"]) if __name__ == "__main__": diff --git a/examples/grasp_demo.py b/examples/grasp_demo.py index 03b2bb14..113d2f80 100644 --- a/examples/grasp_demo.py +++ b/examples/grasp_demo.py @@ -50,10 +50,10 @@ def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int return self.generate_waypoints(end_eff_pose, goal_pose, num_waypoints=num_waypoints) def execute_motion(self, waypoints: list[Pose], gripper: float = GripperWrapper.BINARY_GRIPPER_OPEN) -> dict: - for i in range(1, len(waypoints)): + for i in range(len(waypoints)): # calculate delta action - delta_action = waypoints[i] * waypoints[i - 1].inverse() - obs = self.step(self._action(delta_action, gripper)) + # delta_action = waypoints[i] * waypoints[i - 1].inverse() + obs = self.step(self._action(waypoints[i], gripper)) return obs def approach(self, geom_name: str): @@ -82,15 +82,13 @@ def pickup(self, geom_name: str): def main(): - # compatilbe with rcs/SimplePickUpSimDigitHand-v0 and rcs/SimplePickUpSim-v0 env = gym.make( - "rcs/SimplePickUpSimDigitHand-v0", + "rcs/FR3SimplePickUpSim-v0", render_mode="human", - delta_actions=True, + delta_actions=False, ) env.reset() print(env.unwrapped.robot.get_cartesian_position().translation()) # type: ignore - # assert False controller = PickUpDemo(env) controller.pickup("box_geom") diff --git a/examples/grasp_demo_lab.py b/examples/grasp_demo_lab.py deleted file mode 100644 index 3abcc18b..00000000 --- a/examples/grasp_demo_lab.py +++ /dev/null @@ -1,96 +0,0 @@ -import logging -from typing import Any, cast - -import gymnasium as gym -import mujoco -import numpy as np -from rcs._core.common import Pose -from rcs.envs.base import GripperWrapper, RobotEnv - -logger = logging.getLogger(__name__) -logger.setLevel(logging.INFO) - - -class PickUpDemo: - def __init__(self, env: gym.Env): - self.env = env - self.unwrapped: RobotEnv = cast(RobotEnv, self.env.unwrapped) - self.home_pose = self.unwrapped.robot.get_cartesian_position() - - def _action(self, pose: Pose, gripper: float) -> dict[str, Any]: - return {"xyzrpy": pose.xyzrpy(), "gripper": gripper} - - def get_object_pose(self, geom_name) -> Pose: - model = self.env.get_wrapper_attr("sim").model - data = self.env.get_wrapper_attr("sim").data - - geom_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, geom_name) - obj_pose_world_coordinates = Pose( - translation=data.geom_xpos[geom_id], rotation=data.geom_xmat[geom_id].reshape(3, 3) - ) - return self.unwrapped.robot.to_pose_in_robot_coordinates(obj_pose_world_coordinates) - - def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: int) -> list[Pose]: - waypoints = [] - for i in range(num_waypoints + 1): - t = i / (num_waypoints + 1) - waypoints.append(start_pose.interpolate(end_pose, t)) - return waypoints - - def step(self, action: dict) -> dict: - return self.env.step(action)[0] - - def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 200) -> list[Pose]: - end_eff_pose = self.unwrapped.robot.get_cartesian_position() - goal_pose = self.get_object_pose(geom_name=geom_name) - goal_pose *= Pose(translation=np.array([0, 0, delta_up]), quaternion=np.array([1, 0, 0, 0])) - return self.generate_waypoints(end_eff_pose, goal_pose, num_waypoints=num_waypoints) - - def execute_motion(self, waypoints: list[Pose], gripper: float = GripperWrapper.BINARY_GRIPPER_OPEN) -> dict: - for i in range(1, len(waypoints)): - # calculate delta action - delta_action = waypoints[i] * waypoints[i - 1].inverse() - obs = self.step(self._action(delta_action, gripper)) - return obs - - def approach(self, geom_name: str): - waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=50) - self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_OPEN) - - def grasp(self, geom_name: str): - - waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0, num_waypoints=50) - self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_OPEN) - - self.step(self._action(Pose(), GripperWrapper.BINARY_GRIPPER_CLOSED)) - - waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=50) - self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_CLOSED) - - def move_home(self): - end_eff_pose = self.unwrapped.robot.get_cartesian_position() - waypoints = self.generate_waypoints(end_eff_pose, self.home_pose, num_waypoints=50) - self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_CLOSED) - - def pickup(self, geom_name: str): - self.approach(geom_name) - self.grasp(geom_name) - self.move_home() - - -def main(): - env = gym.make( - "rcs/LabPickUpSimDigitHand-v0", - render_mode="human", - delta_actions=True, - cam_robot_joints=np.array( - [-0.78452318, -1.18096017, 1.75158399, -1.0718541, -0.11207275, 1.01050546, 2.47343638] - ), - ) - env.reset() - controller = PickUpDemo(env) - controller.pickup("box_geom") - - -if __name__ == "__main__": - main() From a82269fd59745898eb60c715055a169c2211ebc4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 27 Jun 2025 20:32:12 +0200 Subject: [PATCH 32/32] docs: improved readme content - added readme for hardware extensions - added concrete python examples to readme --- README.md | 105 ++++++++++++++++++++++++++++++----- extensions/README.md | 8 ++- extensions/rcs_fr3/README.md | 53 ++++++++++++++++++ 3 files changed, 151 insertions(+), 15 deletions(-) create mode 100644 extensions/rcs_fr3/README.md diff --git a/README.md b/README.md index ee3b5d61..20122dcf 100644 --- a/README.md +++ b/README.md @@ -22,34 +22,108 @@ pip install -ve . ## Usage The python package is called `rcs`. -Import the library in python: + +### Direct Robot Control +Simple direct robot control: ```python import rcs +from rcs import sim +from rcs._core.sim import CameraType +from rcs.camera.sim import SimCameraConfig, SimCameraSet +simulation = sim.Sim(rcs.scenes["fr3_empty_world"]["mjb"]) +urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] +ik = rcs.common.RL(str(urdf_path)) +cfg = sim.SimRobotConfig() +cfg.add_id("0") +cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) +robot = rcs.sim.SimRobot(simulation, ik, cfg) + +gripper_cfg_sim = sim.SimGripperConfig() +gripper_cfg_sim.add_id("0") +gripper = sim.SimGripper(simulation, gripper_cfg_sim) + +# add camera to have a rendering gui +cameras = { + "wrist": SimCameraConfig( + identifier="wrist_0", + type=CameraType.fixed, + resolution_width=640, + resolution_height=480, + frame_rate=30, + ), +} +camera_set = SimCameraSet(simulation, cameras) +simulation.open_gui() +robot.set_cartesian_position( + robot.get_cartesian_position() * rcs.common.Pose(translation=np.array([0.05, 0, 0])) +) +gripper.grasp() +simulation.step_until_convergence() +``` +### Gym Env Interface +```python +from rcs.envs.creators import SimEnvCreator +from rcs.envs.utils import ( + default_mujoco_cameraset_cfg, + default_sim_gripper_cfg, + default_sim_robot_cfg, +) +from rcs.envs.base import ControlMode, RelativeTo +env_rel = SimEnvCreator()( + control_mode=ControlMode.JOINTS, + collision_guard=False, + robot_cfg=default_sim_robot_cfg(), + gripper_cfg=default_sim_gripper_cfg(), + cameras=default_mujoco_cameraset_cfg(), + max_relative_movement=np.deg2rad(5), + relative_to=RelativeTo.LAST_STEP, +) +env_rel.get_wrapper_attr("sim").open_gui() + +for _ in range(10): + obs, info = env_rel.reset() + for _ in range(10): + # sample random relative action and execute it + act = env_rel.action_space.sample() + print(act) + obs, reward, terminated, truncated, info = env_rel.step(act) + print(obs) + if truncated or terminated: + logger.info("Truncated or terminated!") + return ``` -Checkout the python examples that we provide in [examples](examples): -- [fr3.py](python/examples/fr3.py) shows direct robot control with RCS's python bindings -- [env_joint_control.py](python/examples/env_joint_control.py) and [env_cartesian_control.py](python/examples/env_cartesian_control.py) demonstrates RCS's high level [gymnasium](https://gymnasium.farama.org/) interface both for joint- and end effector space control +### Examples +Checkout the python examples in the [examples](examples) folder: +- [fr3_direct_control.py](examples/fr3.py) shows direct robot control with RCS's python bindings +- [fr3_env_joint_control.py](examples/env_joint_control.py) and [fr3_env_cartesian_control.py](examples/env_cartesian_control.py) demonstrates RCS's high level [gymnasium](https://gymnasium.farama.org/) interface both for joint- and end effector space control All of these examples work both in the MuJoCo simulation as well as on your hardware FR3. -Just switch between the following settings in the example script + + +### Hardware Extensions +To enable hardware usage in RCS, install the needed hardware extensions via pip. RCS itself comes with a couple of supported extensions e.g. control of the FR3 via the [`rcs_fr3`](extensions/rcs_fr3) extension. All native supported extension are located in [extensions](extensions). +To install extensions: +```shell +pip install -ve extensions/rcs_fr3 +``` +For more details real the readme file of the respective extension. + +After the required hardware extensions are installed the examples also above work on real hardware: +Switch to hardware by setting the following flag: ```python ROBOT_INSTANCE = RobotPlatform.SIMULATION # ROBOT_INSTANCE = RobotPlatform.HARDWARE ``` -and add your robot credentials to a `.env` file like this: -```env -DESK_USERNAME=... -DESK_PASSWORD=... -``` -### Command Line Interface +#### Command Line Interface Some modules include command line interfaces, e.g. rcs_fr3 defines useful commands to handle the FR3 robot without the need to use the Desk Website. You can see the available subcommands as follows: ```shell -python -m rcs.fr3 --help -python -m rcs.realsense --help +python -m rcs_fr3 --help +python -m rcs_realsense --help ``` ## Development +### Formatting and Linting ```shell # check for c++ formatting errors make cppcheckformat @@ -66,10 +140,13 @@ make pylint # Testing make pytest ``` - ### Stub Files for Python Bindings We use autogenerated python stub files (`.pyi`) in the [`_core`](python/rcs/_core/) folder to show our linters the expected types of the C++ Python bindings. If the python bindings in the C++ code have changed you might need to regenerate them by using: ```shell make stubgen ``` + +### Develop Your Own Hardware Extension +TODO + diff --git a/extensions/README.md b/extensions/README.md index 5692de4c..5734183c 100644 --- a/extensions/README.md +++ b/extensions/README.md @@ -1 +1,7 @@ -# Hardware Extensions \ No newline at end of file +# Hardware Extensions + +To install hardware exentsion use +```shell +pip install -ve # e.g. rcs_fr3 +``` +Some extensions need further dependencies or further setups. Please to check out their readme pages for details. \ No newline at end of file diff --git a/extensions/rcs_fr3/README.md b/extensions/rcs_fr3/README.md new file mode 100644 index 00000000..a3172da8 --- /dev/null +++ b/extensions/rcs_fr3/README.md @@ -0,0 +1,53 @@ +# RCS FR3 Hardware Extension +Extension to control the fr3 with rcs. + +## Installation +```shell +pip install -ve . +``` + +Add your FR3 credentials to a `.env` file like this: +```env +DESK_USERNAME=... +DESK_PASSWORD=... +``` + +## Usage +```python +import rcs_fr3 +from rcs_fr3._core import hw +from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_fr3_desk +user, pw = load_creds_fr3_desk() +with FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False): + urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] + ik = rcs.common.RL(str(urdf_path)) + robot = hw.FR3(ROBOT_IP, ik) + robot_cfg = FR3Config() + robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) + robot_cfg.ik_solver = IKSolver.rcs_ik + robot.set_parameters(robot_cfg) + + gripper_cfg_hw = hw.FHConfig() + gripper_cfg_hw.epsilon_inner = gripper_cfg_hw.epsilon_outer = 0.1 + gripper_cfg_hw.speed = 0.1 + gripper_cfg_hw.force = 30 + gripper = hw.FrankaHand(ROBOT_IP, gripper_cfg_hw) + robot.set_cartesian_position( + robot.get_cartesian_position() * rcs.common.Pose(translation=np.array([0.05, 0, 0])) + ) + gripper.grasp() +``` +For more examples see the [examples](../../examples/) folder. +You can switch to hardware by setting the following flag: +```python +ROBOT_INSTANCE = RobotPlatform.HARDWARE +# ROBOT_INSTANCE = RobotPlatform.SIMULATION +``` + + +## CLI +Defines useful commands to handle the FR3 robot without the need to use the Desk Website. +You can see the available subcommands as follows: +```shell +python -m rcs_fr3 --help +``` \ No newline at end of file