Merge pull request #195 from utn-mi/juelg/refactor-independent-hw-robots

Independent Hardware Robots

Author: juelg

.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

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)

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

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
+

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()

python/examples/__init__.py examples/__init__.pypython/examples/__init__.py renamed to examples/__init__.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 <ip>
+python -m rcs_fr3 unlock <ip>
 
 or put it into guiding mode using:
 
-python -m rcs fr3 guiding-mode <ip>
+python -m rcs_fr3 guiding-mode <ip>
 
 When you are done you lock it again using:
 
-python -m rcs fr3 lock <ip>
+python -m rcs_fr3 lock <ip>
 
 or even shut it down using:
 
-python -m rcs fr3 shutdown <ip>
+python -m rcs_fr3 shutdown <ip>
 """
 
 
@@ -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