From 586ae481949502d267ea87eb94ba41fc0a11e655 Mon Sep 17 00:00:00 2001 From: nitish3693 Date: Mon, 30 Mar 2026 11:33:24 +0200 Subject: [PATCH 1/3] PSI-273 Robotiq3F Integration --- extend_gripper_control_ur/CMakeLists.txt | 1 + .../scripts/gripper_control_robotiq_3f.py | 152 ++++++++++++++++++ 2 files changed, 153 insertions(+) create mode 100755 extend_gripper_control_ur/scripts/gripper_control_robotiq_3f.py diff --git a/extend_gripper_control_ur/CMakeLists.txt b/extend_gripper_control_ur/CMakeLists.txt index 698786114..426d75988 100644 --- a/extend_gripper_control_ur/CMakeLists.txt +++ b/extend_gripper_control_ur/CMakeLists.txt @@ -213,6 +213,7 @@ include_directories( catkin_install_python(PROGRAMS scripts/gripper_control_digital.py scripts/gripper_control_robotiq.py + scripts/gripper_control_robotiq_3f.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/extend_gripper_control_ur/scripts/gripper_control_robotiq_3f.py b/extend_gripper_control_ur/scripts/gripper_control_robotiq_3f.py new file mode 100755 index 000000000..07808dc6e --- /dev/null +++ b/extend_gripper_control_ur/scripts/gripper_control_robotiq_3f.py @@ -0,0 +1,152 @@ +#!/usr/bin/env python3 +import os +import time +from enum import Enum + +import rospy + +from std_msgs.msg import Header + +from robotiq_3f_gripper_articulated_msgs.msg import Robotiq3FGripperRobotOutput + +from extend_msgs.msg import GripperControl, GripperResponse + + +FINGER_FORCE = 150 +FINGER_SPEED = 255 + +class Robotiq3FGripperMode(Enum): + BASIC = 0 + PINCH = 1 + WIDE = 2 + SCISSOR = 3 + +class Robotiq3FGripperControlNode: + def __init__(self, mode=Robotiq3FGripperMode.BASIC): + # Initializing the publishers + self.pub_robotiq_control = rospy.Publisher('Robotiq3FGripperRobotOutput', Robotiq3FGripperRobotOutput, queue_size=1) + self.pub_gripper_command_republisher = rospy.Publisher('extend_gripper_republished_command', GripperControl, queue_size=1) + self.pub_gripper_response = rospy.Publisher('extend_gripper_response', GripperResponse, queue_size=1) + self.current_mode = mode + self.joint_command_values = [0.0] * 3 # Assuming 3 joints for the gripper + + + def reset_gripper(self): + gripper_control_msg = Robotiq3FGripperRobotOutput() + self.pub_robotiq_control.publish(gripper_control_msg) + + def activate_gripper(self): + gripper_control_msg = Robotiq3FGripperRobotOutput() + gripper_control_msg.rACT = 1 + self.pub_robotiq_control.publish(gripper_control_msg) + + def mode_select(self, mode): + gripper_control_msg = Robotiq3FGripperRobotOutput() + gripper_control_msg.rACT = 1 + if mode == Robotiq3FGripperMode.BASIC: + gripper_control_msg.rMOD = 0 + elif mode == Robotiq3FGripperMode.PINCH: + gripper_control_msg.rMOD = 1 + elif mode == Robotiq3FGripperMode.WIDE: + gripper_control_msg.rMOD = 2 + elif mode == Robotiq3FGripperMode.SCISSOR: + gripper_control_msg.rMOD = 3 + else: + raise ValueError("Invalid gripper mode selected") + self.pub_robotiq_control.publish(gripper_control_msg) + + def gripper_command_publish(self): + gripper_control_msg = Robotiq3FGripperRobotOutput() + gripper_control_msg.rACT = 1 + gripper_control_msg.rGTO = 1 + gripper_control_msg.rATR = 0 + gripper_control_msg.rGLV = 0 + gripper_control_msg.rICS = 0 + + gripper_control_msg.rSPA = FINGER_SPEED + gripper_control_msg.rFRA = FINGER_FORCE + + gripper_control_msg.rSPB = FINGER_SPEED + gripper_control_msg.rFRB = FINGER_FORCE + + gripper_control_msg.rSPC = FINGER_SPEED + gripper_control_msg.rFRC = FINGER_FORCE + + gripper_control_msg.rPRS = 0 + gripper_control_msg.rSPS = 0 + gripper_control_msg.rFRS = 0 + + + if self.current_mode == Robotiq3FGripperMode.BASIC: + gripper_control_msg.rMOD = 0 + gripper_control_msg.rICF = 1 + gripper_control_msg.rPRA = int(255 * self.joint_command_values[0] /70.0) #Joint command values are in the range of 0-70 degrees for the fingers + gripper_control_msg.rPRB = int(255 * self.joint_command_values[1] /70.0) + gripper_control_msg.rPRC = int(255 * self.joint_command_values[2] /70.0) + + elif self.current_mode == Robotiq3FGripperMode.PINCH: + gripper_control_msg.rMOD = 1 + gripper_control_msg.rPRA = int(255 * self.joint_command_values[0] /70.0) # In pinch mode individual finger control is disabled. + + elif self.current_mode == Robotiq3FGripperMode.WIDE: + gripper_control_msg.rMOD = 2 + gripper_control_msg.rICF = 1 + gripper_control_msg.rPRA = int(255 * self.joint_command_values[0] /70.0) + gripper_control_msg.rPRB = int(255 * self.joint_command_values[1] /70.0) + gripper_control_msg.rPRC = int(255 * self.joint_command_values[2] /70.0) + + elif self.current_mode == Robotiq3FGripperMode.SCISSOR: + gripper_control_msg.rMOD = 3 + # Map analog input value 0-1 to 0-255 for scissor mode + gripper_control_msg.rPRA = int(255 * self.joint_command_values[0] /70.0) # In scissor mode individual finger control is disabled. + + else: + raise ValueError("Invalid gripper mode selected, cannot publish gripper command") + + self.pub_robotiq_control.publish(gripper_control_msg) + + def vr_gripper_command_callback(self, msg): + if len(msg.handJointValues) > 0 and len(msg.handJointValues) == 3: + self.joint_command_values = msg.handJointValues + else: + self.joint_command_values = [msg.gripperAnalog.data] * 3 # Use analog value for all joints if joint values are not provided + + header = Header() + header.seq = 0 + header.frame_id = "" + header.stamp = rospy.Time.now() + + pub_gripper_command_republisher_data = GripperControl() + pub_gripper_command_republisher_data = msg + pub_gripper_command_republisher_data.header = header + self.pub_gripper_command_republisher.publish(pub_gripper_command_republisher_data) + + # Fetching the Gripper Response Joint States and Force + pub_gripper_response_data = GripperResponse() + pub_gripper_response_data.header = header + self.pub_gripper_response.publish(pub_gripper_response_data) + +def main(): + gripper_mode = os.getenv("GRIPPER_MODE", "BASIC") + + # Creating the ros node + rospy.init_node("ur_robotiq_gripper") + gripper_control_node = Robotiq3FGripperControlNode(mode=Robotiq3FGripperMode[gripper_mode.upper()]) + + time.sleep(0.5) + # Reset the Gripper + gripper_control_node.reset_gripper() + time.sleep(1) + # Activate the Gripper + gripper_control_node.activate_gripper() + + time.sleep(0.5) + # Set the Gripper Mode + gripper_control_node.mode_select(mode=Robotiq3FGripperMode[gripper_mode.upper()]) + + # Subscribe to Digital Gripper Data Stream from Unity + rospy.Subscriber("extend_gripper_command", GripperControl, gripper_control_node.vr_gripper_command_callback) + rospy.spin() + +if __name__ == '__main__': + main() From bae35064f8396cf635b6f8aeda3854bf088c5da7 Mon Sep 17 00:00:00 2001 From: nitish3693 Date: Tue, 31 Mar 2026 14:26:20 +0200 Subject: [PATCH 2/3] Added Mode Provider Service and Updated Error Handling --- .../scripts/gripper_control_robotiq_3f.py | 33 +++++++++++++------ 1 file changed, 23 insertions(+), 10 deletions(-) diff --git a/extend_gripper_control_ur/scripts/gripper_control_robotiq_3f.py b/extend_gripper_control_ur/scripts/gripper_control_robotiq_3f.py index 07808dc6e..66406e8a7 100755 --- a/extend_gripper_control_ur/scripts/gripper_control_robotiq_3f.py +++ b/extend_gripper_control_ur/scripts/gripper_control_robotiq_3f.py @@ -10,6 +10,8 @@ from robotiq_3f_gripper_articulated_msgs.msg import Robotiq3FGripperRobotOutput from extend_msgs.msg import GripperControl, GripperResponse +from extend_msgs.srv import GetString, GetStringResponse + FINGER_FORCE = 150 @@ -22,15 +24,29 @@ class Robotiq3FGripperMode(Enum): SCISSOR = 3 class Robotiq3FGripperControlNode: - def __init__(self, mode=Robotiq3FGripperMode.BASIC): + def __init__(self, mode): # Initializing the publishers self.pub_robotiq_control = rospy.Publisher('Robotiq3FGripperRobotOutput', Robotiq3FGripperRobotOutput, queue_size=1) self.pub_gripper_command_republisher = rospy.Publisher('extend_gripper_republished_command', GripperControl, queue_size=1) self.pub_gripper_response = rospy.Publisher('extend_gripper_response', GripperResponse, queue_size=1) + rospy.Service('robotiq_3f_current_mode', GetString, self.current_mode_provider) self.current_mode = mode self.joint_command_values = [0.0] * 3 # Assuming 3 joints for the gripper + def current_mode_provider(self, request): + response = GetStringResponse() + if self.current_mode == Robotiq3FGripperMode.BASIC: + response = "Basic" + elif self.current_mode == Robotiq3FGripperMode.PINCH: + response = "Pinch" + elif self.current_mode == Robotiq3FGripperMode.WIDE: + response = "Wide" + else: + response = "Scissor" + return response + + def reset_gripper(self): gripper_control_msg = Robotiq3FGripperRobotOutput() self.pub_robotiq_control.publish(gripper_control_msg) @@ -49,10 +65,8 @@ def mode_select(self, mode): gripper_control_msg.rMOD = 1 elif mode == Robotiq3FGripperMode.WIDE: gripper_control_msg.rMOD = 2 - elif mode == Robotiq3FGripperMode.SCISSOR: - gripper_control_msg.rMOD = 3 else: - raise ValueError("Invalid gripper mode selected") + gripper_control_msg.rMOD = 3 self.pub_robotiq_control.publish(gripper_control_msg) def gripper_command_publish(self): @@ -95,14 +109,11 @@ def gripper_command_publish(self): gripper_control_msg.rPRB = int(255 * self.joint_command_values[1] /70.0) gripper_control_msg.rPRC = int(255 * self.joint_command_values[2] /70.0) - elif self.current_mode == Robotiq3FGripperMode.SCISSOR: + else: gripper_control_msg.rMOD = 3 # Map analog input value 0-1 to 0-255 for scissor mode gripper_control_msg.rPRA = int(255 * self.joint_command_values[0] /70.0) # In scissor mode individual finger control is disabled. - else: - raise ValueError("Invalid gripper mode selected, cannot publish gripper command") - self.pub_robotiq_control.publish(gripper_control_msg) def vr_gripper_command_callback(self, msg): @@ -128,10 +139,12 @@ def vr_gripper_command_callback(self, msg): def main(): gripper_mode = os.getenv("GRIPPER_MODE", "BASIC") - # Creating the ros node rospy.init_node("ur_robotiq_gripper") - gripper_control_node = Robotiq3FGripperControlNode(mode=Robotiq3FGripperMode[gripper_mode.upper()]) + try: + gripper_control_node = Robotiq3FGripperControlNode(mode=Robotiq3FGripperMode[gripper_mode.upper()]) + except KeyError as e: + raise KeyError(f"Invalid gripper mode \"{gripper_mode}\" selected. Please select the correct gripper mode") time.sleep(0.5) # Reset the Gripper From a58bbfb4d08f02a6d7f664e45c42c82a875a1afb Mon Sep 17 00:00:00 2001 From: nitish3693 <75649112+nitish3693@users.noreply.github.com> Date: Thu, 2 Apr 2026 16:50:44 +0100 Subject: [PATCH 3/3] Change environment variable for gripper mode Updated environment variable name for gripper mode from 'GRIPPER_MODE' to 'gripperMode' and added the function call --- .../scripts/gripper_control_robotiq_3f.py | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/extend_gripper_control_ur/scripts/gripper_control_robotiq_3f.py b/extend_gripper_control_ur/scripts/gripper_control_robotiq_3f.py index 66406e8a7..82a04b4c8 100755 --- a/extend_gripper_control_ur/scripts/gripper_control_robotiq_3f.py +++ b/extend_gripper_control_ur/scripts/gripper_control_robotiq_3f.py @@ -4,19 +4,15 @@ from enum import Enum import rospy - from std_msgs.msg import Header - from robotiq_3f_gripper_articulated_msgs.msg import Robotiq3FGripperRobotOutput - from extend_msgs.msg import GripperControl, GripperResponse from extend_msgs.srv import GetString, GetStringResponse - - FINGER_FORCE = 150 FINGER_SPEED = 255 + class Robotiq3FGripperMode(Enum): BASIC = 0 PINCH = 1 @@ -136,9 +132,10 @@ def vr_gripper_command_callback(self, msg): pub_gripper_response_data = GripperResponse() pub_gripper_response_data.header = header self.pub_gripper_response.publish(pub_gripper_response_data) + self.gripper_command_publish() def main(): - gripper_mode = os.getenv("GRIPPER_MODE", "BASIC") + gripper_mode = os.getenv("gripperMode", "BASIC") # Creating the ros node rospy.init_node("ur_robotiq_gripper") try: