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 diff --git a/CMakeLists.txt b/CMakeLists.txt index 15a91fcc..9850e896 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,14 @@ 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) find_package(pinocchio 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 +52,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/Makefile b/Makefile index 49fc50cc..c6510394 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: @@ -30,25 +35,33 @@ stubgen: find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/tuple\[typing\.Literal[\1]\]/g' find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/tuple\[\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 -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 + isort python/rcs/_core extensions/rcs_fr3/src/rcs_fr3/_core + black python/rcs/_core extensions/rcs_fr3/src/rcs_fr3/_core # Python pycheckformat: - isort --check-only ${PYSRC} - black --check ${PYSRC} + isort --check-only ${PYSRC} extensions examples + black --check ${PYSRC} extensions examples pyformat: - isort ${PYSRC} - black ${PYSRC} + isort ${PYSRC} extensions examples + black ${PYSRC} extensions examples pylint: ruff mypy ruff: - ruff check ${PYSRC} + ruff check ${PYSRC} extensions examples mypy: - mypy ${PYSRC} --install-types --non-interactive --no-namespace-packages + 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 97b7b83c..20122dcf 100644 --- a/README.md +++ b/README.md @@ -22,33 +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 [python/examples](python/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 -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: +#### 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 --help +python -m rcs_fr3 --help +python -m rcs_realsense --help ``` ## Development +### Formatting and Linting ```shell # check for c++ formatting errors make cppcheckformat @@ -65,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/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/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/fr3.py b/examples/fr3_direct_control.py similarity index 92% rename from python/examples/fr3.py rename to examples/fr3_direct_control.py index 981a5c88..a428e5c8 100644 --- a/python/examples/fr3.py +++ b/examples/fr3_direct_control.py @@ -1,13 +1,14 @@ 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.control.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 ROBOT_IP = "192.168.101.1" ROBOT_INSTANCE = RobotPlatform.SIMULATION @@ -30,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 """ @@ -72,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, @@ -93,17 +94,17 @@ def main(): else: urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] ik = rcs.common.RL(str(urdf_path)) - robot = rcs.hw.FR3(ROBOT_IP, ik) - robot_cfg = FR3Config() + robot = hw.FR3(ROBOT_IP, ik) + robot_cfg = hw.FR3Config() robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) - robot_cfg.ik_solver = IKSolver.rcs_ik - robot.set_parameters(robot_cfg) + robot_cfg.ik_solver = hw.IKSolver.rcs_ik + 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 diff --git a/python/examples/env_cartesian_control.py b/examples/fr3_env_cartesian_control.py similarity index 81% rename from python/examples/env_cartesian_control.py rename to examples/fr3_env_cartesian_control.py index 62f374e8..9a460923 100644 --- a/python/examples/env_cartesian_control.py +++ b/examples/fr3_env_cartesian_control.py @@ -1,16 +1,16 @@ 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.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) @@ -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,17 +56,17 @@ 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, ) 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/examples/fr3_env_joint_control.py similarity index 76% rename from python/examples/env_joint_control.py rename to examples/fr3_env_joint_control.py index 4ee966a8..a1dd2cce 100644 --- a/python/examples/env_joint_control.py +++ b/examples/fr3_env_joint_control.py @@ -2,16 +2,16 @@ 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.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) @@ -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,17 +57,17 @@ 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, ) 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, @@ -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/python/examples/grasp_demo.py b/examples/grasp_demo.py similarity index 91% rename from python/examples/grasp_demo.py rename to examples/grasp_demo.py index 03b2bb14..113d2f80 100644 --- a/python/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/extensions/README.md b/extensions/README.md new file mode 100644 index 00000000..5734183c --- /dev/null +++ b/extensions/README.md @@ -0,0 +1,7 @@ +# 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/CMakeLists.txt b/extensions/rcs_fr3/CMakeLists.txt new file mode 100644 index 00000000..89fa86cf --- /dev/null +++ b/extensions/rcs_fr3/CMakeLists.txt @@ -0,0 +1,73 @@ +cmake_minimum_required(VERSION 3.19) + +project( + rcs_fr3 + 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(pinocchio 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 + 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(rl libfranka pybind11) + +add_subdirectory(src) 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 diff --git a/extensions/rcs_fr3/cmake/Findpinocchio.cmake b/extensions/rcs_fr3/cmake/Findpinocchio.cmake new file mode 100644 index 00000000..fcefcf5c --- /dev/null +++ b/extensions/rcs_fr3/cmake/Findpinocchio.cmake @@ -0,0 +1,68 @@ +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_default.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() + + # 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) + 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}" + ) + + 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/cmake/Findrcs.cmake b/extensions/rcs_fr3/cmake/Findrcs.cmake new file mode 100644 index 00000000..08456ccc --- /dev/null +++ b/extensions/rcs_fr3/cmake/Findrcs.cmake @@ -0,0 +1,46 @@ +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 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.") + 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 SHARED IMPORTED) + target_include_directories(rcs INTERFACE ${rcs_INCLUDE_DIRS}) + set_target_properties( + rcs + PROPERTIES + IMPORTED_LOCATION "${rcs_library_path}" + ) + set(rcs_FOUND TRUE) +endif() \ No newline at end of file diff --git a/extensions/rcs_fr3/pyproject.toml b/extensions/rcs_fr3/pyproject.toml new file mode 100644 index 00000000..f80948d3 --- /dev/null +++ b/extensions/rcs_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==3.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/rcs_fr3/src/CMakeLists.txt b/extensions/rcs_fr3/src/CMakeLists.txt new file mode 100644 index 00000000..9f24288c --- /dev/null +++ b/extensions/rcs_fr3/src/CMakeLists.txt @@ -0,0 +1,3 @@ +target_include_directories(rcs INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) +add_subdirectory(hw) +add_subdirectory(pybind) diff --git a/src/hw/CMakeLists.txt b/extensions/rcs_fr3/src/hw/CMakeLists.txt similarity index 85% rename from src/hw/CMakeLists.txt rename to extensions/rcs_fr3/src/hw/CMakeLists.txt index a0edd471..51b0a173 100644 --- a/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 common + PUBLIC franka rcs pinocchio::all PRIVATE math ) diff --git a/src/hw/FR3.cpp b/extensions/rcs_fr3/src/hw/FR3.cpp similarity index 99% rename from src/hw/FR3.cpp rename to extensions/rcs_fr3/src/hw/FR3.cpp index e836ff9a..256d7dbf 100644 --- a/src/hw/FR3.cpp +++ b/extensions/rcs_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/src/hw/FR3.h b/extensions/rcs_fr3/src/hw/FR3.h similarity index 95% rename from src/hw/FR3.h rename to extensions/rcs_fr3/src/hw/FR3.h index 249235f4..3cc2a799 100644 --- a/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 -#include -#include -#include -#include #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/src/hw/FR3MotionGenerator.cpp b/extensions/rcs_fr3/src/hw/FR3MotionGenerator.cpp similarity index 100% rename from src/hw/FR3MotionGenerator.cpp rename to extensions/rcs_fr3/src/hw/FR3MotionGenerator.cpp diff --git a/src/hw/FR3MotionGenerator.h b/extensions/rcs_fr3/src/hw/FR3MotionGenerator.h similarity index 97% rename from src/hw/FR3MotionGenerator.h rename to extensions/rcs_fr3/src/hw/FR3MotionGenerator.h index 9777aabe..5f8bfdb5 100644 --- a/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 -#include #include #include #include @@ -12,6 +10,9 @@ #include #include +#include "rcs/Robot.h" +#include "rcs/utils.h" + namespace rcs { namespace hw { diff --git a/src/hw/FrankaHand.cpp b/extensions/rcs_fr3/src/hw/FrankaHand.cpp similarity index 100% rename from src/hw/FrankaHand.cpp rename to extensions/rcs_fr3/src/hw/FrankaHand.cpp diff --git a/src/hw/FrankaHand.h b/extensions/rcs_fr3/src/hw/FrankaHand.h similarity index 98% rename from src/hw/FrankaHand.h rename to extensions/rcs_fr3/src/hw/FrankaHand.h index 95e24bd2..40895160 100644 --- a/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 #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/src/hw/main.cpp b/extensions/rcs_fr3/src/hw/main.cpp similarity index 97% rename from src/hw/main.cpp rename to extensions/rcs_fr3/src/hw/main.cpp index 2e2c89b5..df624717 100644 --- a/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/CMakeLists.txt b/extensions/rcs_fr3/src/pybind/CMakeLists.txt new file mode 100644 index 00000000..3a409fec --- /dev/null +++ b/extensions/rcs_fr3/src/pybind/CMakeLists.txt @@ -0,0 +1,14 @@ +pybind11_add_module(_core MODULE rcs.cpp) +target_link_libraries(_core PRIVATE hw rcs mdl pinocchio::all) +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;$ORIGIN/../cmeel.prefix/lib" + INTERPROCEDURAL_OPTIMIZATION TRUE +) +# in pip +install(TARGETS _core franka DESTINATION rcs_fr3 COMPONENT python_package) diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp new file mode 100644 index 00000000..5fcb37c7 --- /dev/null +++ b/extensions/rcs_fr3/src/pybind/rcs.cpp @@ -0,0 +1,163 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#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 + +#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::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) + .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::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) + .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::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) + .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::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) + .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::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) + .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::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) + .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/rcs_fr3/src/rcs_fr3/__init__.py b/extensions/rcs_fr3/src/rcs_fr3/__init__.py new file mode 100644 index 00000000..29e879ce --- /dev/null +++ b/extensions/rcs_fr3/src/rcs_fr3/__init__.py @@ -0,0 +1,8 @@ +from rcs_fr3 import desk, envs +from rcs_fr3._core import hw + +__all__ = [ + "desk", + "envs", + "hw", +] diff --git a/python/rcs/cli/main.py b/extensions/rcs_fr3/src/rcs_fr3/__main__.py similarity index 58% rename from python/rcs/cli/main.py rename to extensions/rcs_fr3/src/rcs_fr3/__main__.py index e73c8f76..f77613b0 100644 --- a/python/rcs/cli/main.py +++ b/extensions/rcs_fr3/src/rcs_fr3/__main__.py @@ -2,51 +2,17 @@ from time import sleep from typing import Annotated -import pyrealsense2 as rs -import rcs -import rcs.control.fr3_desk +import rcs_fr3 import typer -from rcs.camera.realsense import RealSenseCameraSet -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).") - - -# 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) - # FR3 CLI -fr3_app = typer.Typer() -main_app.add_typer( - fr3_app, - name="fr3", +fr3_app = typer.Typer( 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." ), ) @@ -60,7 +26,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() @@ -70,7 +36,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() @@ -79,7 +45,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() @@ -88,8 +54,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() @@ -102,12 +68,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() @@ -118,7 +84,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() @@ -127,8 +93,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/extensions/rcs_fr3/src/rcs_fr3/_core/__init__.pyi b/extensions/rcs_fr3/src/rcs_fr3/_core/__init__.pyi new file mode 100644 index 00000000..b6c1624d --- /dev/null +++ b/extensions/rcs_fr3/src/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/python/rcs/_core/hw/__init__.pyi b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi similarity index 99% rename from python/rcs/_core/hw/__init__.pyi rename to extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi index 19d7fea6..8e8a8062 100644 --- a/python/rcs/_core/hw/__init__.pyi +++ b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi @@ -1,6 +1,6 @@ # ATTENTION: auto generated from C++ code, use `make stubgen` to update! """ -hardware module +rcs fr3 module """ from __future__ import annotations diff --git a/python/rcs/_core/hw/exceptions.pyi b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/exceptions.pyi similarity index 100% rename from python/rcs/_core/hw/exceptions.pyi rename to extensions/rcs_fr3/src/rcs_fr3/_core/hw/exceptions.pyi diff --git a/extensions/rcs_fr3/src/rcs_fr3/creators.py b/extensions/rcs_fr3/src/rcs_fr3/creators.py new file mode 100644 index 00000000..68df13db --- /dev/null +++ b/extensions/rcs_fr3/src/rcs_fr3/creators.py @@ -0,0 +1,165 @@ +import logging +from os import PathLike + +import gymnasium as gym +import numpy as np +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 import hw +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) + + +class RCSFR3EnvCreator(RCSHardwareEnvCreator): + def __call__( # type: ignore + self, + ip: str, + control_mode: ControlMode, + robot_cfg: hw.FR3Config, + collision_guard: str | PathLike | 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, + 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 (hw.FR3Config): Configuration for the FR3 robot. + collision_guard (str | PathLike | None): Key to a built-in scene + 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 (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.RL(str(urdf_path)) if urdf_path is not None else None + 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, 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) + 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: 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, + urdf_path: str | PathLike | None = None, + ) -> gym.Env: + + urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] + 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) + 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 = 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/control/fr3_desk.py b/extensions/rcs_fr3/src/rcs_fr3/desk.py similarity index 97% rename from python/rcs/control/fr3_desk.py rename to extensions/rcs_fr3/src/rcs_fr3/desk.py index 3cedad71..ab5076f6 100644 --- a/python/rcs/control/fr3_desk.py +++ b/extensions/rcs_fr3/src/rcs_fr3/desk.py @@ -10,10 +10,10 @@ from typing import Callable, Literal from urllib import parse -import rcs +import rcs_fr3 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 @@ -37,13 +37,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: @@ -53,8 +53,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:") @@ -63,7 +63,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/rcs/envs/hw.py b/extensions/rcs_fr3/src/rcs_fr3/envs.py similarity index 97% rename from python/rcs/envs/hw.py rename to extensions/rcs_fr3/src/rcs_fr3/envs.py index 12ea76b3..303c1d13 100644 --- a/python/rcs/envs/hw.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 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 new file mode 100644 index 00000000..0c368bd7 --- /dev/null +++ b/extensions/rcs_fr3/src/rcs_fr3/utils.py @@ -0,0 +1,21 @@ +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() + 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/extensions/rcs_realsense/pyproject.toml b/extensions/rcs_realsense/pyproject.toml new file mode 100644 index 00000000..ac3e8067 --- /dev/null +++ b/extensions/rcs_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/python/rcs/cli/__init__.py b/extensions/rcs_realsense/src/rcs_realsense/__init__.py similarity index 100% rename from python/rcs/cli/__init__.py rename to extensions/rcs_realsense/src/rcs_realsense/__init__.py diff --git a/extensions/rcs_realsense/src/rcs_realsense/__main__.py b/extensions/rcs_realsense/src/rcs_realsense/__main__.py new file mode 100644 index 00000000..ff342679 --- /dev/null +++ b/extensions/rcs_realsense/src/rcs_realsense/__main__.py @@ -0,0 +1,25 @@ +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.") + + +@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) + + +if __name__ == "__main__": + realsense_app() diff --git a/python/rcs/camera/realsense.py b/extensions/rcs_realsense/src/rcs_realsense/camera.py similarity index 98% rename from python/rcs/camera/realsense.py rename to extensions/rcs_realsense/src/rcs_realsense/camera.py index 28b89a1b..4cd55c85 100644 --- a/python/rcs/camera/realsense.py +++ b/extensions/rcs_realsense/src/rcs_realsense/camera.py @@ -3,10 +3,11 @@ import numpy as np import pyrealsense2 as rs -from rcs._core.common import BaseCameraConfig from rcs.camera.hw import HardwareCamera from rcs.camera.interface import CameraFrame, DataFrame, Frame, IMUFrame +from rcs import common + @dataclass class RealSenseDeviceInfo: @@ -26,7 +27,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 +97,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/extensions/rcs_realsense/src/rcs_realsense/utils.py b/extensions/rcs_realsense/src/rcs_realsense/utils.py new file mode 100644 index 00000000..372dc04d --- /dev/null +++ b/extensions/rcs_realsense/src/rcs_realsense/utils.py @@ -0,0 +1,13 @@ +from rcs_realsense.camera import RealSenseCameraSet + +from rcs import common + + +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) diff --git a/extensions/rcs_so101/pyproject.toml b/extensions/rcs_so101/pyproject.toml new file mode 100644 index 00000000..40ee8309 --- /dev/null +++ b/extensions/rcs_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/python/rcs/control/__init__.py b/extensions/rcs_so101/src/rcs_so101/__init__.py similarity index 100% rename from python/rcs/control/__init__.py rename to extensions/rcs_so101/src/rcs_so101/__init__.py diff --git a/python/rcs_so101/creators.py b/extensions/rcs_so101/src/rcs_so101/creators.py similarity index 99% rename from python/rcs_so101/creators.py rename to extensions/rcs_so101/src/rcs_so101/creators.py index 5d4b79cb..acdf1584 100644 --- a/python/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/python/rcs_so101/env_joint_control.py b/extensions/rcs_so101/src/rcs_so101/env_joint_control.py similarity index 99% rename from python/rcs_so101/env_joint_control.py rename to extensions/rcs_so101/src/rcs_so101/env_joint_control.py index 0862e7b9..d7cbbb80 100644 --- a/python/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/python/rcs_so101/hw.py b/extensions/rcs_so101/src/rcs_so101/hw.py similarity index 99% rename from python/rcs_so101/hw.py rename to extensions/rcs_so101/src/rcs_so101/hw.py index 54367003..1ca20b64 100644 --- a/python/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/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/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/pyproject.toml b/pyproject.toml index d106907c..e9906cd3 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) @@ -54,18 +52,13 @@ classifiers = [ # "mypy==1.4.1", # ] -[project.optional-dependencies] -so101 = [ - "lerobot @ git+https://github.com/huggingface/lerobot.git", -] - [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", "python/rcs_so101"] +wheel.packages = ["python/rcs"] install.components = ["python_package"] [tool.ruff] diff --git a/python/examples/env_cartesian_control_digit.py b/python/examples/env_cartesian_control_digit.py deleted file mode 100644 index 776c8fd7..00000000 --- a/python/examples/env_cartesian_control_digit.py +++ /dev/null @@ -1,101 +0,0 @@ -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 ( - 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, -) - -from python.rcs.camera.hw import HardwareCameraSet - -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", - camera_set=HardwareCameraSet(default_digit({"digit_0": "D21182"})), - gripper_cfg=default_fr3_hw_gripper_cfg(), - max_relative_movement=0.5, - relative_to=RelativeTo.LAST_STEP, - ) - else: - env_rel = FR3SimEnvCreator()( - control_mode=ControlMode.CARTESIAN_TQuat, - robot_cfg=default_fr3_sim_robot_cfg(), - collision_guard=False, - gripper_cfg=default_fr3_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/python/examples/env_cartesian_control_tilburg.py b/python/examples/env_cartesian_control_tilburg.py deleted file mode 100644 index 4d97b95b..00000000 --- a/python/examples/env_cartesian_control_tilburg.py +++ /dev/null @@ -1,101 +0,0 @@ -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 ( - default_fr3_hw_robot_cfg, - default_fr3_sim_gripper_cfg, - default_fr3_sim_robot_cfg, - default_mujoco_cameraset_cfg, - default_tilburg_hw_hand_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 = FR3SimEnvCreator()( - control_mode=ControlMode.CARTESIAN_TQuat, - robot_cfg=default_fr3_sim_robot_cfg(), - collision_guard=False, - gripper_cfg=default_fr3_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/python/examples/grasp_demo_lab.py b/python/examples/grasp_demo_lab.py deleted file mode 100644 index 3abcc18b..00000000 --- a/python/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() diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py index 856cb4d7..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, control, envs, hand, sim -from rcs._core import __version__, common, hw +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: { @@ -18,7 +19,7 @@ } # make submodules available -__all__ = ["__doc__", "__version__", "common", "hw", "sim", "camera", "scenes", "control", "envs", "hand"] +__all__ = ["__doc__", "__version__", "common", "sim", "camera", "scenes", "envs", "hand"] # register gymnasium environments register( @@ -30,5 +31,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/__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/_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/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 diff --git a/python/rcs/camera/sim.py b/python/rcs/camera/sim.py index 8ea38c67..2400bf5c 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 +from rcs import sim + class SimCameraSet(_SimCameraSet): """Represents a set of cameras in a mujoco simulation. @@ -18,7 +19,7 @@ class SimCameraSet(_SimCameraSet): def __init__( self, - sim: rcs.sim.Sim, + simulation: sim.Sim, cameras: dict[str, SimCameraConfig], physical_units: bool = False, render_on_demand: bool = True, @@ -27,8 +28,8 @@ def __init__( self.cameras = cameras self.physical_units = physical_units - super().__init__(sim, cameras, render_on_demand=render_on_demand) - self._sim: rcs.sim.Sim + super().__init__(simulation, cameras, render_on_demand=render_on_demand) + self._sim: sim.Sim def get_latest_frames(self) -> FrameSet | None: """Should return the latest frame from the camera with the given name.""" 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 ab48b788..b09eae75 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -4,24 +4,17 @@ 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.envs.hw import FR3HW from rcs.envs.sim import ( CollisionGuard, GripperWrapperSim, @@ -30,14 +23,10 @@ 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, - default_realsense, -) -from rcs.hand.tilburg_hand import TilburgHand +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) @@ -47,147 +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.RL(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.RL(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_config: dict[str, str] | 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, - 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, @@ -274,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 e3359608..283b246b 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -3,11 +3,12 @@ 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 -from rcs.envs.utils import default_fr3_sim_robot_cfg +from rcs.envs.utils import default_sim_robot_cfg + +import rcs +from rcs import sim logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -205,7 +206,7 @@ def env_from_xml_paths( assert isinstance(env.unwrapped, RobotEnv) simulation = sim.Sim(mjmld) ik = rcs.common.RL(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 e650c0eb..781391a3 100644 --- a/python/rcs/envs/utils.py +++ b/python/rcs/envs/utils.py @@ -4,21 +4,20 @@ 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.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 +import rcs +from rcs import sim + logger = logging.getLogger(__name__) 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 @@ -27,24 +26,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 @@ -52,17 +33,7 @@ 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: +def default_sim_gripper_cfg(idx: str = "0") -> sim.SimGripperConfig: cfg = sim.SimGripperConfig() cfg.add_id(idx) return cfg @@ -97,6 +68,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_so101/__init__.py b/python/rcs_so101/__init__.py deleted file mode 100644 index e69de29b..00000000 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 cd1bfb36..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, @@ -11,22 +10,24 @@ 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, ) +import rcs + @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 +65,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 +76,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 +89,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 +112,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 +125,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 +143,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 +160,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 +193,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 +209,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 +230,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 +241,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 +259,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 +280,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 +313,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 +325,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 +339,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 +354,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 +369,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 +395,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)) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index c01d7a98..38c112ab 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(rcs) +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 deleted file mode 100644 index abccdf63..00000000 --- a/src/common/CMakeLists.txt +++ /dev/null @@ -1,3 +0,0 @@ -add_library(common) -target_sources(common PRIVATE Pose.cpp Robot.cpp IK.cpp utils.cpp) -target_link_libraries(common PUBLIC Eigen3::Eigen mdl pinocchio::pinocchio pinocchio_parsers) diff --git a/src/pybind/CMakeLists.txt b/src/pybind/CMakeLists.txt index fc3e987a..3baf7822 100644 --- a/src/pybind/CMakeLists.txt +++ b/src/pybind/CMakeLists.txt @@ -1,13 +1,15 @@ pybind11_add_module(_core MODULE rcs.cpp) -target_link_libraries(_core PRIVATE hw sim common) +target_link_libraries(_core PRIVATE sim rcs) target_compile_definitions(_core PRIVATE VERSION_INFO=${PROJECT_VERSION}) set_target_properties(_core PROPERTIES INSTALL_RPATH "$ORIGIN;$ORIGIN/../mujoco;$ORIGIN/../cmeel.prefix/lib" INTERPROCEDURAL_OPTIMIZATION TRUE ) -set_target_properties(franka PROPERTIES - INSTALL_RPATH "$ORIGIN/../cmeel.prefix/lib" - INTERPROCEDURAL_OPTIMIZATION TRUE +# in pip +install(TARGETS _core rcs DESTINATION rcs COMPONENT python_package) +install( + DIRECTORY ${CMAKE_SOURCE_DIR}/include/rcs + DESTINATION rcs/include + COMPONENT python_package ) -install(TARGETS _core franka DESTINATION rcs COMPONENT python_package) diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index a473dd59..2c55ed72 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -1,10 +1,3 @@ -#include -#include -#include -#include -#include -#include -#include #include #include #include @@ -17,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 @@ -317,116 +314,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"); py::class_( diff --git a/src/rcs/CMakeLists.txt b/src/rcs/CMakeLists.txt new file mode 100644 index 00000000..cd8b937c --- /dev/null +++ b/src/rcs/CMakeLists.txt @@ -0,0 +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::all) +set_target_properties(rcs PROPERTIES + INSTALL_RPATH "$ORIGIN;$ORIGIN/../cmeel.prefix/lib" +) diff --git a/src/common/IK.cpp b/src/rcs/IK.cpp similarity index 99% rename from src/common/IK.cpp rename to src/rcs/IK.cpp index a413e5f9..98f466ab 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 91f4bebc..0768d92f 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 pinocchio::pinocchio) +#target_link_libraries(test_sim PRIVATE sim MuJoCo::MuJoCo rcs egl pinocchio::pinocchio) diff --git a/src/sim/SimGripper.h b/src/sim/SimGripper.h index 9d4ceb54..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 - #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 50ac7817..f1b8dccd 100644 --- a/src/sim/SimRobot.cpp +++ b/src/sim/SimRobot.cpp @@ -14,10 +14,10 @@ #include #include -#include "common/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 1c286a09..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 +#include +#include #include "sim/sim.h" diff --git a/src/sim/camera.cpp b/src/sim/camera.cpp index d7de4ed3..7323d6c4 100644 --- a/src/sim/camera.cpp +++ b/src/sim/camera.cpp @@ -13,10 +13,10 @@ #include #include -#include "common/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 0863e599..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 #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..95efeece 100644 --- a/src/sim/renderer.cpp +++ b/src/sim/renderer.cpp @@ -1,10 +1,10 @@ -#include #include #include #include #include +#include "rcs/utils.h" #include "sim/sim.h" namespace rcs { namespace sim { diff --git a/src/sim/test.cpp b/src/sim/test.cpp index 174e0d0e..602ba8cd 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"