From 586ae481949502d267ea87eb94ba41fc0a11e655 Mon Sep 17 00:00:00 2001 From: nitish3693 Date: Mon, 30 Mar 2026 11:33:24 +0200 Subject: [PATCH 01/12] 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 02/12] 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 03/12] 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: From c59bfff23c40abbbcc1a9978b63523291037c162 Mon Sep 17 00:00:00 2001 From: robotist-nitish Date: Mon, 6 Apr 2026 22:48:57 +0100 Subject: [PATCH 04/12] Changing Enum to Dict and Error Handling --- .../scripts/gripper_control_robotiq_3f.py | 109 ++++++------------ 1 file changed, 37 insertions(+), 72 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 82a04b4c8..d99715fea 100755 --- a/extend_gripper_control_ur/scripts/gripper_control_robotiq_3f.py +++ b/extend_gripper_control_ur/scripts/gripper_control_robotiq_3f.py @@ -1,7 +1,6 @@ #!/usr/bin/env python3 import os import time -from enum import Enum import rospy from std_msgs.msg import Header @@ -11,59 +10,39 @@ FINGER_FORCE = 150 FINGER_SPEED = 255 +ROBOTIQ_3F_GRIPPER_MODE_ENUM_DICT = {"BASIC": 0, "PINCH": 1, "WIDE": 2, "SCISSOR": 3} -class Robotiq3FGripperMode(Enum): - BASIC = 0 - PINCH = 1 - WIDE = 2 - SCISSOR = 3 - class Robotiq3FGripperControlNode: 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) + self.robotiq_control_pub = rospy.Publisher('Robotiq3FGripperRobotOutput', Robotiq3FGripperRobotOutput, queue_size=1) + self.gripper_command_republisher = rospy.Publisher('extend_gripper_republished_command', GripperControl, queue_size=1) + self.gripper_response_pub = 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 - + self.joint_command_values = [0.0] * 3 # 3 Active 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" + # Providing the current gripper mode string + response.data = self.current_mode return response - def reset_gripper(self): gripper_control_msg = Robotiq3FGripperRobotOutput() - self.pub_robotiq_control.publish(gripper_control_msg) + self.robotiq_control_pub.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) + self.robotiq_control_pub.publish(gripper_control_msg) - def mode_select(self, mode): + def mode_select(self): 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 - else: - gripper_control_msg.rMOD = 3 - self.pub_robotiq_control.publish(gripper_control_msg) + gripper_control_msg.rMOD = ROBOTIQ_3F_GRIPPER_MODE_ENUM_DICT[self.current_mode] + self.robotiq_control_pub.publish(gripper_control_msg) def gripper_command_publish(self): gripper_control_msg = Robotiq3FGripperRobotOutput() @@ -86,73 +65,59 @@ def gripper_command_publish(self): gripper_control_msg.rSPS = 0 gripper_control_msg.rFRS = 0 + gripper_control_msg.rMOD = ROBOTIQ_3F_GRIPPER_MODE_ENUM_DICT[self.current_mode] - 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 + # Joint command values are in the range of 0-70 degrees for the fingers + gripper_control_msg.rPRA = int(255 * self.joint_command_values[0] /70.0) + if ROBOTIQ_3F_GRIPPER_MODE_ENUM_DICT[self.current_mode] in (0, 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) + 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) - 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. - - self.pub_robotiq_control.publish(gripper_control_msg) + self.robotiq_control_pub.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 + raise ValueError("Invalid hand joint values received.") 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) + gripper_command_republisher_data = GripperControl() + gripper_command_republisher_data = msg + gripper_command_republisher_data.header = header + self.gripper_command_republisher.publish(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) + gripper_response_data = GripperResponse() + gripper_response_data.header = header + self.gripper_response_pub.publish(gripper_response_data) self.gripper_command_publish() def main(): - gripper_mode = os.getenv("gripperMode", "BASIC") - # Creating the ros node + gripper_mode = os.getenv("gripperMode", "BASIC").upper() + if gripper_mode not in ROBOTIQ_3F_GRIPPER_MODE_ENUM_DICT: + raise ValueError(f"Invalid gripper mode \"{gripper_mode}\" selected. Please select the correct gripper mode") + + # Creating the ros node and class instance rospy.init_node("ur_robotiq_gripper") - 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") + gripper_control_node = Robotiq3FGripperControlNode(mode=gripper_mode) - 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()]) + gripper_control_node.mode_select() + time.sleep(0.5) # Subscribe to Digital Gripper Data Stream from Unity rospy.Subscriber("extend_gripper_command", GripperControl, gripper_control_node.vr_gripper_command_callback) From 33c6be43eb6698a1837246d7d510ee566f0d6753 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Tue, 7 Apr 2026 11:54:42 +0100 Subject: [PATCH 05/12] add python linters for extend grippers package --- .github/workflows/python_linters.yml | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) create mode 100644 .github/workflows/python_linters.yml diff --git a/.github/workflows/python_linters.yml b/.github/workflows/python_linters.yml new file mode 100644 index 000000000..23d6c5e71 --- /dev/null +++ b/.github/workflows/python_linters.yml @@ -0,0 +1,26 @@ +name: PythonLinters + +on: [push] + +jobs: + call-workflow-python-linters: + uses: Extend-Robotics/er_build_tools_internal/.github/workflows/python_linters.yml@main + with: + blacklist: | + universal_robots/ + ur10_moveit_config/ + ur10e_moveit_config/ + ur12e_moveit_config/ + ur15_moveit_config/ + ur16e_moveit_config/ + ur20_moveit_config/ + ur30_moveit_config/ + ur3_moveit_config/ + ur3e_moveit_config/ + ur5_moveit_config/ + ur5e_moveit_config/ + ur7e_moveit_config/ + ur_description/ + ur_gazebo/ + ur_kinematics/ + From 97fb86b99e428569cb0e4cb491244bd37a2ad001 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Tue, 7 Apr 2026 12:26:13 +0100 Subject: [PATCH 06/12] exclude scripts from CI that weren't a part of this PR --- .github/workflows/python_linters.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/python_linters.yml b/.github/workflows/python_linters.yml index 23d6c5e71..02a677f4a 100644 --- a/.github/workflows/python_linters.yml +++ b/.github/workflows/python_linters.yml @@ -23,4 +23,5 @@ jobs: ur_description/ ur_gazebo/ ur_kinematics/ - + extend_gripper_control_ur/scripts/gripper_control_digital.py + extend_gripper_control_ur/scripts/gripper_control_robotiq.py From 376f1bcf0c3fa003b83b4db7cdb227ae7130f9a3 Mon Sep 17 00:00:00 2001 From: nitish3693 Date: Tue, 7 Apr 2026 21:24:52 +0000 Subject: [PATCH 07/12] Changes requested in review --- extend_gripper_control_ur/CMakeLists.txt | 4 +- extend_gripper_control_ur/package.xml | 2 + .../scripts/gripper_control_robotiq_3f.py | 51 +++++++++++-------- 3 files changed, 34 insertions(+), 23 deletions(-) diff --git a/extend_gripper_control_ur/CMakeLists.txt b/extend_gripper_control_ur/CMakeLists.txt index 426d75988..8caaf71f2 100644 --- a/extend_gripper_control_ur/CMakeLists.txt +++ b/extend_gripper_control_ur/CMakeLists.txt @@ -106,12 +106,12 @@ catkin_package(CATKIN_DEPENDS ## LIBRARIES: libraries you create in this project that dependent projects also need ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( +# catkin_package( # INCLUDE_DIRS include # LIBRARIES extend_gripper_control # CATKIN_DEPENDS other_catkin_pkg # DEPENDS system_lib -) +# ) ########### ## Build ## diff --git a/extend_gripper_control_ur/package.xml b/extend_gripper_control_ur/package.xml index e1b758283..4282ce1fe 100644 --- a/extend_gripper_control_ur/package.xml +++ b/extend_gripper_control_ur/package.xml @@ -55,6 +55,8 @@ extend_msgs extend_msgs + robotiq_3f_gripper_articulated_msgs + robotiq_3f_gripper_articulated_msgs 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 d99715fea..642b46916 100755 --- a/extend_gripper_control_ur/scripts/gripper_control_robotiq_3f.py +++ b/extend_gripper_control_ur/scripts/gripper_control_robotiq_3f.py @@ -14,18 +14,20 @@ class Robotiq3FGripperControlNode: + NUM_ACTIVE_JOINTS = 3 + FINGER_POSITION_REGISTER_MAX = 255 + MAX_FINGER_ANGLE_DEGREES = 70.0 def __init__(self, mode): - # Initializing the publishers self.robotiq_control_pub = rospy.Publisher('Robotiq3FGripperRobotOutput', Robotiq3FGripperRobotOutput, queue_size=1) self.gripper_command_republisher = rospy.Publisher('extend_gripper_republished_command', GripperControl, queue_size=1) self.gripper_response_pub = 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 # 3 Active joints for the gripper + self.joint_command_values = [0.0] * self.NUM_ACTIVE_JOINTS + self.analog_command_value = 0.0 - def current_mode_provider(self, request): + def current_mode_provider(self, _): response = GetStringResponse() - # Providing the current gripper mode string response.data = self.current_mode return response @@ -67,32 +69,44 @@ def gripper_command_publish(self): gripper_control_msg.rMOD = ROBOTIQ_3F_GRIPPER_MODE_ENUM_DICT[self.current_mode] - # Joint command values are in the range of 0-70 degrees for the fingers - gripper_control_msg.rPRA = int(255 * self.joint_command_values[0] /70.0) - if ROBOTIQ_3F_GRIPPER_MODE_ENUM_DICT[self.current_mode] in (0, 2): + if self.current_mode in ("BASIC", "WIDE"): + # For BASIC and WIDE modes, joint command values are in the range of 0-70 degrees for the fingers gripper_control_msg.rICF = 1 - 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) - + gripper_control_msg.rPRA = int(self.FINGER_POSITION_REGISTER_MAX * self.joint_command_values[0] / self.MAX_FINGER_ANGLE_DEGREES) + gripper_control_msg.rPRB = int(self.FINGER_POSITION_REGISTER_MAX * self.joint_command_values[1] / self.MAX_FINGER_ANGLE_DEGREES) + gripper_control_msg.rPRC = int(self.FINGER_POSITION_REGISTER_MAX * self.joint_command_values[2] / self.MAX_FINGER_ANGLE_DEGREES) + else: + # For PINCH and SCISSOR modes, all fingers move together based on the analog value between 0-1 + gripper_control_msg.rICF = 0 + gripper_control_msg.rPRA = int(self.FINGER_POSITION_REGISTER_MAX * self.analog_command_value) self.robotiq_control_pub.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 + if self.current_mode in ("PINCH", "SCISSOR"): + self.analog_command_value = msg.gripperAnalog.data + if not 0.0 <= self.analog_command_value <= 1.0: + raise ValueError("Gripper analog command value must be between 0 and 1 for PINCH and SCISSOR modes.") else: - raise ValueError("Invalid hand joint values received.") + if len(msg.handJointValues) == 3: + value_error = [] + self.joint_command_values = msg.handJointValues + for i, value in enumerate(self.joint_command_values): + if not 0.0 <= value <= self.MAX_FINGER_ANGLE_DEGREES: + value_error.append((f"Joint Command {value} recieved for Index {i} is outside [{0.0}, {self.MAX_FINGER_ANGLE_DEGREES}]")) + if value_error: + raise ValueError( "\n".join(value_error)) + else: + raise ValueError("Invalid hand joint values received.") header = Header() header.seq = 0 header.frame_id = "" header.stamp = rospy.Time.now() - gripper_command_republisher_data = GripperControl() gripper_command_republisher_data = msg gripper_command_republisher_data.header = header self.gripper_command_republisher.publish(gripper_command_republisher_data) - # Fetching the Gripper Response Joint States and Force gripper_response_data = GripperResponse() gripper_response_data.header = header self.gripper_response_pub.publish(gripper_response_data) @@ -101,25 +115,20 @@ def vr_gripper_command_callback(self, msg): def main(): gripper_mode = os.getenv("gripperMode", "BASIC").upper() if gripper_mode not in ROBOTIQ_3F_GRIPPER_MODE_ENUM_DICT: - raise ValueError(f"Invalid gripper mode \"{gripper_mode}\" selected. Please select the correct gripper mode") + raise ValueError(f"Invalid gripper mode \"{gripper_mode}\" selected. Valid modes are: {list(ROBOTIQ_3F_GRIPPER_MODE_ENUM_DICT.keys())}") - # Creating the ros node and class instance rospy.init_node("ur_robotiq_gripper") gripper_control_node = Robotiq3FGripperControlNode(mode=gripper_mode) - # 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() time.sleep(0.5) - # Subscribe to Digital Gripper Data Stream from Unity rospy.Subscriber("extend_gripper_command", GripperControl, gripper_control_node.vr_gripper_command_callback) rospy.spin() From d5f9b4e9e3e59d64e439a68d7ad8d0f1b11c7697 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Wed, 8 Apr 2026 15:37:06 +0100 Subject: [PATCH 08/12] trigger checks --- extend_gripper_control_ur/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/extend_gripper_control_ur/CMakeLists.txt b/extend_gripper_control_ur/CMakeLists.txt index 8caaf71f2..2d9cc511b 100644 --- a/extend_gripper_control_ur/CMakeLists.txt +++ b/extend_gripper_control_ur/CMakeLists.txt @@ -216,4 +216,3 @@ catkin_install_python(PROGRAMS scripts/gripper_control_robotiq_3f.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) - From f27f221042c29110fe2ba442fe6293bfe21b86b5 Mon Sep 17 00:00:00 2001 From: tomqext Date: Wed, 8 Apr 2026 15:43:06 +0100 Subject: [PATCH 09/12] Update workflow reference for Python linters --- .github/workflows/python_linters.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/python_linters.yml b/.github/workflows/python_linters.yml index b761f773a..9ad7d417e 100644 --- a/.github/workflows/python_linters.yml +++ b/.github/workflows/python_linters.yml @@ -4,7 +4,7 @@ on: [push] jobs: call-workflow-python-linters: - uses: Extend-Robotics/er_build_tools_internal/.github/workflows/python_linters.yml@main + uses: Extend-Robotics/er_build_tools/.github/workflows/python_linters.yml@main with: blacklist: | universal_robots/ From ab9d73ccc14a0e2354450b53fb5e2f35f16cfd89 Mon Sep 17 00:00:00 2001 From: nitish3693 <75649112+nitish3693@users.noreply.github.com> Date: Wed, 8 Apr 2026 15:52:22 +0100 Subject: [PATCH 10/12] Fix formatting issues in gripper control script --- .../scripts/gripper_control_robotiq_3f.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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 642b46916..b0f75bac9 100755 --- a/extend_gripper_control_ur/scripts/gripper_control_robotiq_3f.py +++ b/extend_gripper_control_ur/scripts/gripper_control_robotiq_3f.py @@ -17,6 +17,7 @@ class Robotiq3FGripperControlNode: NUM_ACTIVE_JOINTS = 3 FINGER_POSITION_REGISTER_MAX = 255 MAX_FINGER_ANGLE_DEGREES = 70.0 + def __init__(self, mode): self.robotiq_control_pub = rospy.Publisher('Robotiq3FGripperRobotOutput', Robotiq3FGripperRobotOutput, queue_size=1) self.gripper_command_republisher = rospy.Publisher('extend_gripper_republished_command', GripperControl, queue_size=1) @@ -94,7 +95,7 @@ def vr_gripper_command_callback(self, msg): if not 0.0 <= value <= self.MAX_FINGER_ANGLE_DEGREES: value_error.append((f"Joint Command {value} recieved for Index {i} is outside [{0.0}, {self.MAX_FINGER_ANGLE_DEGREES}]")) if value_error: - raise ValueError( "\n".join(value_error)) + raise ValueError("\n".join(value_error)) else: raise ValueError("Invalid hand joint values received.") @@ -112,6 +113,7 @@ def vr_gripper_command_callback(self, msg): self.gripper_response_pub.publish(gripper_response_data) self.gripper_command_publish() + def main(): gripper_mode = os.getenv("gripperMode", "BASIC").upper() if gripper_mode not in ROBOTIQ_3F_GRIPPER_MODE_ENUM_DICT: @@ -132,5 +134,6 @@ def main(): rospy.Subscriber("extend_gripper_command", GripperControl, gripper_control_node.vr_gripper_command_callback) rospy.spin() + if __name__ == '__main__': main() From 1c87fe05e1ef4958cef2bf92de5fee373a62169b Mon Sep 17 00:00:00 2001 From: nitish3693 Date: Wed, 8 Apr 2026 17:04:49 +0100 Subject: [PATCH 11/12] PSI-270 Review Comments adjustments --- .../scripts/gripper_control_robotiq_3f.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 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 b0f75bac9..10d49ca3d 100755 --- a/extend_gripper_control_ur/scripts/gripper_control_robotiq_3f.py +++ b/extend_gripper_control_ur/scripts/gripper_control_robotiq_3f.py @@ -34,6 +34,9 @@ def current_mode_provider(self, _): def reset_gripper(self): gripper_control_msg = Robotiq3FGripperRobotOutput() + # rACT = 0 is the reset command for the Robotiq 3F gripper, resets it to its initial state. + # Making the assignmenent explicitly here for clarity, even though the default value of rACT is 0 + gripper_control_msg.rACT = 0 self.robotiq_control_pub.publish(gripper_control_msg) def activate_gripper(self): @@ -88,12 +91,12 @@ def vr_gripper_command_callback(self, msg): if not 0.0 <= self.analog_command_value <= 1.0: raise ValueError("Gripper analog command value must be between 0 and 1 for PINCH and SCISSOR modes.") else: - if len(msg.handJointValues) == 3: + if len(msg.handJointValues) == NUM_ACTIVE_JOINTS: value_error = [] self.joint_command_values = msg.handJointValues for i, value in enumerate(self.joint_command_values): if not 0.0 <= value <= self.MAX_FINGER_ANGLE_DEGREES: - value_error.append((f"Joint Command {value} recieved for Index {i} is outside [{0.0}, {self.MAX_FINGER_ANGLE_DEGREES}]")) + value_error.append((f"Joint Command {value} recieved for Index {i} is outside [0.0, {self.MAX_FINGER_ANGLE_DEGREES}]")) if value_error: raise ValueError("\n".join(value_error)) else: @@ -108,6 +111,9 @@ def vr_gripper_command_callback(self, msg): gripper_command_republisher_data.header = header self.gripper_command_republisher.publish(gripper_command_republisher_data) + # Data collection always requires a response to be published, even if it's empty + # Data collection config is based on arm models and does not consider the attached gripper type. + # Therefore, till we distinguish between different grippers we need to publish the response. gripper_response_data = GripperResponse() gripper_response_data.header = header self.gripper_response_pub.publish(gripper_response_data) From bd3804273b0fa79d8001ad34bc844a8cde4b0ccc Mon Sep 17 00:00:00 2001 From: nitish3693 Date: Wed, 8 Apr 2026 17:07:42 +0100 Subject: [PATCH 12/12] Fixed a Typo --- extend_gripper_control_ur/scripts/gripper_control_robotiq_3f.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 10d49ca3d..58cb7c844 100755 --- a/extend_gripper_control_ur/scripts/gripper_control_robotiq_3f.py +++ b/extend_gripper_control_ur/scripts/gripper_control_robotiq_3f.py @@ -91,7 +91,7 @@ def vr_gripper_command_callback(self, msg): if not 0.0 <= self.analog_command_value <= 1.0: raise ValueError("Gripper analog command value must be between 0 and 1 for PINCH and SCISSOR modes.") else: - if len(msg.handJointValues) == NUM_ACTIVE_JOINTS: + if len(msg.handJointValues) == self.NUM_ACTIVE_JOINTS: value_error = [] self.joint_command_values = msg.handJointValues for i, value in enumerate(self.joint_command_values):