feat: add robotiq

extensions/rcs_robotiq/pyproject.toml

@@ -0,0 +1,21 @@
+[build-system]
+requires = ["setuptools"]
+build-backend = "setuptools.build_meta"
+
+[project]
+name = "rcs_robotiq"
+version = "0.5.2"
+description="RCS RobotiQ module"
+dependencies = [
+    "rcs>=0.5.0",
+    "2f85-python-driver @ git+https://github.com/PhilNad/2f85-python-driver.git",
+]
+readme = "README.md"
+maintainers = [
+  { name = "Tobias Jülg", email = "tobias.juelg@utn.de" },
+]
+authors = [
+  { name = "Tobias Jülg", email = "tobias.juelg@utn.de" },
+]
+requires-python = ">=3.10"
+license = { text = "AGPL-3.0-or-later" }

extensions/rcs_robotiq/src/rcs_robotiq/hw.py

@@ -0,0 +1,46 @@
+
+
+from rcs._core.common import Gripper
+from Robotiq2F85Driver import Robotiq2F85Driver
+
+
+class RobotiQGripper(Gripper):
+    def __init__(self, serial_number):
+        self.gripper = Robotiq2F85Driver(serial_number=serial_number)
+
+    def get_normalized_width(self) -> float:
+        # value between 0 and 1 (0 is closed)
+        return (85 - self.gripper.opening()) / 85
+
+    def grasp(self) -> None:
+        """
+        Close the gripper to grasp an object.
+        """
+        self.set_normalized_width(0.0)
+
+    def open(self) -> None:
+        """
+        Open the gripper to its maximum width.
+        """
+        self.set_normalized_width(1.0)
+
+    def reset(self) -> None:
+        self.gripper.reset()
+
+    def set_normalized_width(self, width: float, _: float = 0) -> None:
+        """
+        Set the gripper width to a normalized value between 0 and 1.
+        """
+        if not (0 <= width <= 1):
+            msg = f"Width must be between 0 and 1, got {width}."
+            raise ValueError(msg)
+        abs_width = (1 - width) * 85
+        # print(f"Setting gripper width to {width:.2f} (absolute: {abs_width:.2f})")
+        self.gripper.move(int(abs_width), int(self.gripper._max_speed), int(self.gripper._max_force))
+        self.gripper.go_to(opening=int(abs_width), speed=150, force=30)
+
+    def shut(self) -> None:
+        """
+        Close the gripper.
+        """
+        self.set_normalized_width(0.0)