feat: add a robotiq config and example

crisp_py/config/grippers/gripper_robotiq_2f85.yaml

@@ -0,0 +1,6 @@
+  min_value: 0.8
+  max_value: 0.0
+  command_topic: "/robotiq_gripper_controller/gripper_cmd"
+  use_gripper_command_action: True
+  max_delta: 1.0
+  max_effort: 5.0

crisp_py/gripper/gripper.py

@@ -5,6 +5,8 @@
 import numpy as np
 import rclpy
 import yaml
+from control_msgs.action import GripperCommand
+from rclpy.action.client import ActionClient
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
@@ -61,12 +63,27 @@ def __init__(
             self.node, stale_threshold=self.config.max_joint_delay
         )
 
-        self._command_publisher = self.node.create_publisher(
-            Float64MultiArray,
-            self.config.command_topic,
-            qos_profile_system_default,
-            callback_group=ReentrantCallbackGroup(),
+        self._command_publisher = (
+            self.node.create_publisher(
+                Float64MultiArray,
+                self.config.command_topic,
+                qos_profile_system_default,
+                callback_group=ReentrantCallbackGroup(),
+            )
+            if not self.config.use_gripper_command_action
+            else None
+        )
+        self._command_action_client = (
+            ActionClient(
+                self.node,
+                GripperCommand,
+                self.config.command_topic,
+                callback_group=ReentrantCallbackGroup(),
+            )
+            if self.config.use_gripper_command_action
+            else None
         )
+
         self._joint_subscriber = self.node.create_subscription(
             JointState,
             self.config.joint_state_topic,
@@ -216,7 +233,12 @@ def target(self) -> float:
 
     def is_ready(self) -> bool:
         """Returns True if the gripper is fully ready to operate."""
-        return self._value is not None
+        action_client_ready = (
+            self._command_action_client.wait_for_server(timeout_sec=0.0)
+            if self._command_action_client
+            else True
+        )
+        return self._value is not None and action_client_ready
 
     def wait_until_ready(self, timeout: float = 10.0, check_frequency: float = 10.0):
         """Wait until the gripper is available."""
@@ -247,6 +269,27 @@ def _callback_publish_target(self):
         """Publish the target command."""
         if self._target is None:
             return
+
+        if self.config.use_gripper_command_action:
+            if self._command_action_client is None:
+                raise RuntimeError("Command action client is not initialized.")
+
+            goal = GripperCommand.Goal()
+            goal.command.position = self._unnormalize(
+                self.value
+                + np.clip(
+                    self._normalize(self._target) - self.value,
+                    -self.config.max_delta,
+                    self.config.max_delta,
+                )
+            )
+            goal.command.max_effort = self.config.max_effort
+            self._command_action_client.send_goal_async(goal)
+            return
+
+        if self._command_publisher is None:
+            raise RuntimeError("Command publisher is not initialized.")
+
         msg = Float64MultiArray()
         msg.data = [
             self._unnormalize(

crisp_py/gripper/gripper_config.py

@@ -13,6 +13,19 @@ class GripperConfig:
     """Gripper default config.
 
     Can be extented to be used with other grippers.
+
+    Attributes:
+        min_value (float): Minimum gripper value (fully closed).
+        max_value (float): Maximum gripper value (fully open).
+        command_topic (str): Topic to publish gripper commands to.
+        joint_state_topic (str): Topic to subscribe for joint states.
+        reboot_service (str): Service to reboot the gripper.
+        enable_torque_service (str): Service to enable torque on the gripper.
+        index (int): Index of the gripper joint in the joint states message.
+        publish_frequency (float): Frequency to publish gripper state.
+        max_joint_delay (float): Maximum delay for joint state updates.
+        max_delta (float): Maximum change in gripper value per update.
+        use_gripper_command_action (bool): Whether to use GripperCommandAction.
     """
 
     min_value: float
@@ -25,6 +38,8 @@ class GripperConfig:
     publish_frequency: float = 30.0
     max_joint_delay: float = 1.0
     max_delta: float = 0.1
+    use_gripper_command_action: bool = False
+    max_effort: float = 10.0
 
     @classmethod
     def from_yaml(cls, path: str | Path, **overrides) -> "GripperConfig":  # noqa: ANN003

examples/19_robotiq_gripper.py

@@ -0,0 +1,23 @@
+import time
+
+from crisp_py.gripper import make_gripper
+
+
+gripper = make_gripper("gripper_robotiq_2f85")
+
+# %%
+
+gripper.wait_until_ready()
+
+# %%
+gripper.open()
+time.sleep(3.0)
+
+gripper.close()
+time.sleep(3.0)
+
+gripper.set_target(0.5)
+time.sleep(3.0)
+
+
+gripper.shutdown()