feat(robotiq): async support and config

- added config with force, speed and async option
- added close method to close serial properly
- rename class to reflect 2f85 type (hard coded 85mm width)

Author: juelg

extensions/rcs_robotiq2f85/pyproject.toml

@@ -8,7 +8,7 @@ version = "0.6.3"
 description="RCS RobotiQ module"
 dependencies = [
     "rcs>=0.6.3",
-    "2f85-python-driver @ git+https://github.com/PhilNad/2f85-python-driver.git",
+    "2f85-python-driver @ git+https://github.com/RobotControlStack/2f85-python-driver.git",
 ]
 readme = "README.md"
 maintainers = [

extensions/rcs_robotiq2f85/src/rcs_robotiq2f85/hw.py

@@ -1,10 +1,36 @@
-from rcs._core.common import Gripper
-from Robotiq2F85Driver.Robotiq2F85Driver import Robotiq2F85Driver
+from dataclasses import dataclass
 
+from rcs._core.common import Gripper, GripperConfig, GripperState
+from Robotiq2F85Driver.Robotiq2F85Driver import GripperStatus, Robotiq2F85Driver
 
-class RobotiQGripper(Gripper):
-    def __init__(self, serial_number):
+
+@dataclass
+class RobotiQ2F85GripperConfig(GripperConfig):
+    speed: float = 100
+    """Speed in mm/s. Must be between 20 and 150 mm/s."""
+    force: float = 50
+    """Force in N. Must be between 20 and 235 N."""
+    async_control: bool = True
+    """If True, gripper commands return immediately without waiting for the movement to complete.
+    A new command interrupts any ongoing movement."""
+
+
+@dataclass(kw_only=True)
+class RobotiQ2F85GripperState(GripperState):
+    state: GripperStatus
+
+    def __post_init__(self):
+        super().__init__()
+
+
+class RobotiQ2F85Gripper(Gripper):
+    def __init__(self, serial_number: str, cfg: RobotiQ2F85GripperConfig):
+        """
+        serial_number:
+            Get the serial number with `udevadm info -a -n /dev/ttyUSB0 | grep serial`, make sure you have read/write permissions to the port.
+        """
         super().__init__()
+        self._cfg: RobotiQ2F85GripperConfig = cfg
         self.gripper = Robotiq2F85Driver(serial_number=serial_number)
 
     def get_normalized_width(self) -> float:
@@ -15,7 +41,7 @@ def grasp(self) -> None:
         """
         Close the gripper to grasp an object.
         """
-        self.set_normalized_width(0.0)
+        self.set_normalized_width(0.0, force=self._cfg.force)
 
     def open(self) -> None:
         """
@@ -26,18 +52,35 @@ def open(self) -> None:
     def reset(self) -> None:
         self.gripper.reset()
 
-    def set_normalized_width(self, width: float, _: float = 0) -> None:
+    def set_normalized_width(self, width: float, force: 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 = width * 85
-        self.gripper.go_to(opening=float(abs_width), speed=150.0, force=30.0)
+        self.gripper.go_to(
+            opening=float(abs_width),
+            speed=self._cfg.speed,
+            force=force if force != 0 else self._cfg.force,
+            blocking_call=not self._cfg.async_control,
+        )
 
     def shut(self) -> None:
         """
         Close the gripper.
         """
         self.set_normalized_width(0.0)
+
+    def close(self) -> None:
+        self.gripper.client.serial.close()
+
+    def get_config(self) -> GripperConfig:
+        return self._cfg
+
+    def set_config(self, cfg: RobotiQ2F85GripperConfig) -> None:
+        self._cfg = cfg
+
+    def get_state(self) -> GripperState:
+        return RobotiQ2F85GripperState(state=self.gripper.read_status())

src/pybind/rcs.cpp

@@ -373,8 +373,10 @@ PYBIND11_MODULE(_core, m) {
       .def_readwrite("robot_platform",
                      &rcs::common::RobotConfig::robot_platform);
   py::class_<rcs::common::RobotState>(common, "RobotState").def(py::init<>());
-  py::class_<rcs::common::GripperConfig>(common, "GripperConfig").def(py::init<>());
-  py::class_<rcs::common::GripperState>(common, "GripperState").def(py::init<>());
+  py::class_<rcs::common::GripperConfig>(common, "GripperConfig")
+      .def(py::init<>());
+  py::class_<rcs::common::GripperState>(common, "GripperState")
+      .def(py::init<>());
   py::enum_<rcs::common::GraspType>(common, "GraspType")
       .value("POWER_GRASP", rcs::common::GraspType::POWER_GRASP)
       .value("PRECISION_GRASP", rcs::common::GraspType::PRECISION_GRASP)