feat: frankik integration

.github/workflows/ci.yaml

@@ -35,7 +35,7 @@ jobs:
       - name: Install wheel
         run: python -m pip install wheel
       - name: Install the package
-        run: python -m pip install -v --no-build-isolation '.[dev]'
+        run: python -m pip install -ve '.[dev]' --no-build-isolation
       - name: Check formatting
         run: make pycheckformat
       - name: Code linting
@@ -95,10 +95,10 @@ jobs:
         run: pip install -r requirements.txt
 
       - name: Install rcs
-        run: pip install -v --no-build-isolation '.[dev]'
+        run: pip install -ve '.[dev]' --no-build-isolation
 
       - name: Install extension
-        run: pip install -v --no-build-isolation extensions/${{ matrix.extension }}
+        run: pip install -ve extensions/${{ matrix.extension }} --no-build-isolation
 
   build_cpp_extensions:
     needs: [pythonpackage, check-paths]
@@ -138,9 +138,9 @@ jobs:
       - name: Install Python dependencies
         run: pip install -r requirements.txt
       - name: Install RCS
-        run: pip install -v --no-build-isolation '.[dev]'
+        run: pip install -ve '.[dev]' --no-build-isolation
       - name: Install extension
-        run: pip install -v --no-build-isolation extensions/${{ matrix.extension }}
+        run: pip install -ve extensions/${{ matrix.extension }} --no-build-isolation
       - name: Check that stub files are up-to-date
         run: make -C extensions/${{ matrix.extension }} stubgen && git diff --exit-code
       - name: Check clang format

extensions/rcs_fr3/pyproject.toml

@@ -7,7 +7,8 @@ name = "rcs_fr3"
 version = "0.5.2"
 description="RCS libfranka integration"
 dependencies = [
-    "rcs>=0.5.0",
+    "rcs>=0.5.2",
+    "frankik @ git+https://github.com/juelg/frankik",
 ]
 readme = "README.md"
 maintainers = [
@@ -29,7 +30,7 @@ install.components = ["python_package"]
 
 [tool.black]
 line-length = 120
-target-version = ["py311"]
+target-version = ["py310"]
 
 [tool.isort]
 profile = "black"

extensions/rcs_fr3/src/rcs_fr3/__init__.py

@@ -1,8 +1,9 @@
 from rcs_fr3 import desk, envs
-from rcs_fr3._core import hw
+from rcs_fr3._core import __version__, hw
 
 __all__ = [
     "desk",
     "hw",
     "envs",
+    "__version__",
 ]

extensions/rcs_fr3/src/rcs_fr3/creators.py

@@ -1,9 +1,12 @@
 import logging
+import typing
 from os import PathLike
 
 import gymnasium as gym
 import numpy as np
 import rcs.hand.tilburg_hand
+from frankik import FrankaKinematics
+from rcs._core.common import Kinematics, Pose
 from rcs.camera.hw import HardwareCameraSet
 from rcs.envs.base import (
     CameraSetWrapper,
@@ -27,6 +30,30 @@
 logger.setLevel(logging.INFO)
 
 
+class FrankIK(Kinematics):
+    def __init__(self, allow_elbow_flips: bool = False):
+        Kinematics.__init__(self)
+        self.allow_elbow_flips = allow_elbow_flips
+        self.kin = FrankaKinematics(robot_type="fr3")
+
+    def forward(self, q0: np.ndarray[tuple[typing.Literal[7]], np.dtype[np.float64]], tcp_offset: Pose) -> Pose:  # type: ignore
+        print("forward called")
+        return Pose(pose_matrix=self.kin.forward(q0, tcp_offset.pose_matrix()))
+
+    def inverse(  # type: ignore
+        self, pose: Pose, q0: np.ndarray[tuple[typing.Literal[7]], np.dtype[np.float64]], tcp_offset: Pose
+    ) -> np.ndarray[tuple[typing.Literal[7]], np.dtype[np.float64]] | None:
+        tcp_offset = self.kin.FrankaHandTCPOffset
+        return self.kin.inverse(
+            pose.pose_matrix(), q0, tcp_offset.pose_matrix(), allow_elbow_flips=self.allow_elbow_flips
+        )
+
+
+# FYI: this needs to be in global namespace to avoid auto garbage collection issues
+# pybind11 3.x would avoid this but with smart_holder but we cannot update due to the subfiles issue yet
+FastIK = FrankIK()
+
+
 class RCSFR3EnvCreator(RCSHardwareEnvCreator):
     def __call__(  # type: ignore
         self,
@@ -62,6 +89,7 @@ def __call__(  # type: ignore
             robot_cfg.attachment_site,
             urdf=robot_cfg.kinematic_model_path.endswith(".urdf"),
         )
+        # ik = FastIK
         # ik = rcs_robotics_library._core.rl.RoboticsLibraryIK(robot_cfg.kinematic_model_path)
         robot = hw.Franka(ip, ik)
         robot.set_config(robot_cfg)

extensions/rcs_panda/pyproject.toml

@@ -7,7 +7,7 @@ name = "rcs_panda"
 version = "0.5.2"
 description="RCS libfranka integration"
 dependencies = [
-    "rcs>=0.5.0",
+    "rcs>=0.5.2",
 ]
 readme = "README.md"
 maintainers = [
@@ -29,7 +29,7 @@ install.components = ["python_package"]
 
 [tool.black]
 line-length = 120
-target-version = ["py311"]
+target-version = ["py310"]
 
 [tool.isort]
 profile = "black"

extensions/rcs_panda/src/rcs_panda/__init__.py

@@ -1,7 +1,8 @@
 from rcs_panda import envs
-from rcs_panda._core import hw
+from rcs_panda._core import __version__, hw
 
 __all__ = [
     "hw",
     "envs",
+    "__version__",
 ]

extensions/rcs_realsense/pyproject.toml

@@ -7,7 +7,7 @@ name = "rcs_realsense"
 version = "0.5.2"
 description="RCS realsense module"
 dependencies = [
-    "rcs>=0.5.0",
+    "rcs>=0.5.2",
     "pyrealsense2~=2.55.1",
     "pupil_apriltags",
     "diskcache",
@@ -22,7 +22,7 @@ requires-python = ">=3.11"
 
 [tool.black]
 line-length = 120
-target-version = ["py311"]
+target-version = ["py310"]
 
 [tool.isort]
 profile = "black"

extensions/rcs_realsense/src/rcs_realsense/__init__.py

@@ -0,0 +1 @@
+__version__ = "0.5.2"

extensions/rcs_robotics_library/pyproject.toml

@@ -8,7 +8,7 @@ version = "0.5.2"
 description="RCS robotics library integration"
 readme = "README.md"
 dependencies = [
-    "rcs>=0.5.0",
+    "rcs>=0.5.2",
 ]
 maintainers = [
   { name = "Tobias Jülg", email = "tobias.juelg@utn.de" },
@@ -30,7 +30,7 @@ install.components = ["python_package"]
 
 [tool.black]
 line-length = 120
-target-version = ["py311"]
+target-version = ["py310"]
 
 [tool.isort]
 profile = "black"

extensions/rcs_robotics_library/src/rcs_robotics_library/__init__.py

@@ -1,5 +1,6 @@
-from rcs_robotics_library._core import rl
+from rcs_robotics_library._core import __version__, rl
 
 __all__ = [
     "rl",
+    "__version__",
 ]

extensions/rcs_so101/pyproject.toml

@@ -7,7 +7,7 @@ name = "rcs_so101"
 version = "0.5.2"
 description="RCS SO101 module"
 dependencies = [
-    "rcs>=0.5.0",
+    "rcs>=0.5.2",
     "lerobot==0.3.3",
 ]
 readme = "README.md"
@@ -29,7 +29,7 @@ install.components = ["python_package"]
 
 [tool.black]
 line-length = 120
-target-version = ["py311"]
+target-version = ["py310"]
 
 [tool.isort]
 profile = "black"

extensions/rcs_so101/src/rcs_so101/__init__.py

@@ -1,6 +1,6 @@
+from rcs_so101._core import __version__
 from rcs_so101._core.so101_ik import SO101IK
+from rcs_so101.creators import RCSSO101EnvCreator
+from rcs_so101.hw import SO101, SO101Config, SO101Gripper
 
-from .creators import RCSSO101EnvCreator
-from .hw import SO101, SO101Config, SO101Gripper
-
-__all__ = ["SO101IK", "RCSSO101EnvCreator", "SO101", "SO101Config", "SO101Gripper"]
+__all__ = ["SO101IK", "RCSSO101EnvCreator", "SO101", "SO101Config", "SO101Gripper", "__version__"]

extensions/rcs_tacto/pyproject.toml

@@ -7,7 +7,7 @@ name = "rcs_tacto"
 version = "0.5.0"
 description="RCS integration of tacto"
 dependencies = [
-    "rcs>=0.5.0",
+    "rcs>=0.5.2",
     "mujoco-tacto@git+https://github.com/utn-air/mujoco-tacto.git@main"
 ]
 readme = "README.md"
@@ -22,7 +22,7 @@ requires-python = ">=3.10"
 
 [tool.black]
 line-length = 120
-target-version = ["py311"]
+target-version = ["py310"]
 
 [tool.isort]
 profile = "black"

extensions/rcs_tacto/src/rcs_tacto/__init__.py

@@ -0,0 +1 @@
+__version__ = "0.5.2"

extensions/rcs_ur5e/pyproject.toml

@@ -7,7 +7,7 @@ name = "rcs_ur5e"
 version = "0.5.2"
 description="RCS UR5e module"
 dependencies = [
-    "rcs>=0.5.0",
+    "rcs>=0.5.2",
     "ur_rtde==1.6.1",
 ]
 readme = "README.md"
@@ -19,9 +19,11 @@ authors = [
   { name = "Johannes Hechtl", email = "johannes.hechtl@siemens.com" },
 ]
 requires-python = ">=3.10"
-classifiers = [
-  "Development Status :: 3 - Alpha",
-  "License :: OSI Approved :: GNU Affero General Public License v3 (AGPLv3)",
-  "Programming Language :: Python :: 3",
-]
+
+[tool.black]
+line-length = 120
+target-version = ["py310"]
+
+[tool.isort]
+profile = "black"
 

extensions/rcs_ur5e/src/rcs_ur5e/__init__.py

@@ -0,0 +1 @@
+__version__ = "0.5.2"

extensions/rcs_ur5e/src/rcs_ur5e/hw.py

@@ -278,7 +278,7 @@ def set_cartesian_position(self, pose: common.Pose) -> None:
         if q is None:
             print("IK failed")
             return
-        self.set_joint_position(q[0:6])
+        self.set_joint_position(q[0:6])  # type: ignore
         return
 
     def set_joint_position(self, q: np.ndarray[tuple[typing.Any], np.dtype[np.float64]]) -> None:

extensions/rcs_usb_cam/pyproject.toml

@@ -7,7 +7,7 @@ name = "rcs_usb_cam"
 version = "0.5.0"
 description="RCS USB Camera module"
 dependencies = [
-    "rcs>=0.5.0",
+    "rcs>=0.5.2",
     "opencv-python~=4.10.0"
 ]
 maintainers = [
@@ -21,7 +21,7 @@ requires-python = ">=3.10"
 
 [tool.black]
 line-length = 120
-target-version = ["py311"]
+target-version = ["py310"]
 
 [tool.isort]
 profile = "black"

extensions/rcs_usb_cam/src/rcs_usb_cam/__init__.py

@@ -0,0 +1 @@
+__version__ = "0.5.2"

extensions/rcs_xarm7/pyproject.toml

@@ -7,7 +7,7 @@ name = "rcs_xarm7"
 version = "0.5.2"
 description="RCS xArm7 module"
 dependencies = [
-    "rcs>=0.5.0",
+    "rcs>=0.5.2",
     "xarm-python-sdk==1.17.0",
 ]
 readme = "README.md"
@@ -23,7 +23,7 @@ requires-python = ">=3.10"
 
 [tool.black]
 line-length = 120
-target-version = ["py311"]
+target-version = ["py310"]
 
 [tool.isort]
 profile = "black"

extensions/rcs_xarm7/src/rcs_xarm7/__init__.py

@@ -0,0 +1 @@
+__version__ = "0.5.2"

pyproject.toml

@@ -155,20 +155,41 @@ version_files = [
     "CMakeLists.txt:VERSION",
     "python/rcs/_core/__init__.pyi:__version__",
 
+    # --- FR3 Extension ---
     "extensions/rcs_fr3/CMakeLists.txt:VERSION",
     "extensions/rcs_fr3/src/rcs_fr3/_core/__init__.pyi:__version__",
-    "extensions/rcs_fr3/pyproject.toml:version",
+    "extensions/rcs_fr3/pyproject.toml:version", 
+    # The line below updates the dependency string "rcs>=x.y.z"
+    "extensions/rcs_fr3/pyproject.toml:\"rcs>=(.*)\"",
 
+    # --- Panda Extension ---
     "extensions/rcs_panda/CMakeLists.txt:VERSION",
     "extensions/rcs_panda/src/rcs_fr3/_core/__init__.pyi:__version__",
     "extensions/rcs_panda/pyproject.toml:version",
+    "extensions/rcs_panda/pyproject.toml:\"rcs>=(.*)\"",
 
+    # --- Robotics Library ---
     "extensions/rcs_robotics_library/CMakeLists.txt:VERSION",
     "extensions/rcs_robotics_library/src/rcs_robotics_library/_core/__init__.pyi:__version__",
     "extensions/rcs_robotics_library/pyproject.toml:version",
+    "extensions/rcs_robotics_library/pyproject.toml:\"rcs>=(.*)\"",
 
-    "extensions/rcs_realsense/pyproject.toml:version",
+    # --- SO101 ---
+    "extensions/rcs_so101/CMakeLists.txt:VERSION",
+    "extensions/rcs_so101/src/rcs_so101/_core/__init__.pyi:__version__",
     "extensions/rcs_so101/pyproject.toml:version",
+    "extensions/rcs_so101/pyproject.toml:\"rcs>=(.*)\"",
+
+    # --- Other Extensions ---
+    "extensions/rcs_realsense/pyproject.toml:version",
+    "extensions/rcs_realsense/src/rcs_realsense/__init__.py:__version__",
+    "extensions/rcs_realsense/pyproject.toml:\"rcs>=(.*)\"",
+
     "extensions/rcs_xarm7/pyproject.toml:version",
+    "extensions/rcs_xarm7/src/rcs_xarm7/__init__.py:__version__",
+    "extensions/rcs_xarm7/pyproject.toml:\"rcs>=(.*)\"",
+
     "extensions/rcs_ur5e/pyproject.toml:version",
+    "extensions/rcs_ur5e/src/rcs_ur5e/__init__.py:__version__",
+    "extensions/rcs_ur5e/pyproject.toml:\"rcs>=(.*)\"",
 ]

python/rcs/_core/common.pyi

@@ -130,6 +130,7 @@ class HandState:
     def __init__(self) -> None: ...
 
 class Kinematics:
+    def __init__(self) -> None: ...
     def forward(self, q0: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]], tcp_offset: Pose = ...) -> Pose: ...
     def inverse(
         self, pose: Pose, q0: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]], tcp_offset: Pose = ...