diff --git a/crisp_py/config/grippers/gripper_robotiq_2f85.yaml b/crisp_py/config/grippers/gripper_robotiq_2f85.yaml new file mode 100644 index 0000000..f7d9ff1 --- /dev/null +++ b/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 diff --git a/crisp_py/gripper/gripper.py b/crisp_py/gripper/gripper.py index fb80afb..95dbddd 100644 --- a/crisp_py/gripper/gripper.py +++ b/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( diff --git a/crisp_py/gripper/gripper_config.py b/crisp_py/gripper/gripper_config.py index 0e16165..7ad5a12 100644 --- a/crisp_py/gripper/gripper_config.py +++ b/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 diff --git a/examples/19_robotiq_gripper.py b/examples/19_robotiq_gripper.py new file mode 100644 index 0000000..7341a81 --- /dev/null +++ b/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()