feat(fr3): frankik integration for analytical fast franka ik

Author: juelg

extensions/rcs_fr3/pyproject.toml

@@ -8,6 +8,7 @@ version = "0.5.2"
 description="RCS libfranka integration"
 dependencies = [
     "rcs>=0.5.0",
+    "frankik @ git+https://github.com/juelg/frankik",
 ]
 readme = "README.md"
 maintainers = [

extensions/rcs_fr3/src/rcs_fr3/creators.py

@@ -1,9 +1,11 @@
 import logging
+import typing
 from os import PathLike
 
 import gymnasium as gym
 import numpy as np
 import rcs.hand.tilburg_hand
+from rcs._core.common import Kinematics, Pose
 from rcs.camera.hw import HardwareCameraSet
 from rcs.envs.base import (
     CameraSetWrapper,
@@ -22,11 +24,36 @@
 from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg
 
 import rcs
+from frankik import FrankaKinematics
 
 logger = logging.getLogger(__name__)
 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:
+        print("forward called")
+        return Pose(pose_matrix=self.kin.forward(q0, tcp_offset.pose_matrix()))
+
+    def inverse(
+        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)