PSI-273 Robotiq3f integration

.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/

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 ##
@@ -213,6 +213,6 @@ 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}
 )
-

extend_gripper_control_ur/package.xml

@@ -55,6 +55,8 @@
 
   <build_depend>extend_msgs</build_depend>
   <exec_depend>extend_msgs</exec_depend>
+  <build_depend>robotiq_3f_gripper_articulated_msgs</build_depend>
+  <exec_depend>robotiq_3f_gripper_articulated_msgs</exec_depend>
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->

extend_gripper_control_ur/scripts/gripper_control_robotiq_3f.py

@@ -0,0 +1,145 @@
+#!/usr/bin/env python3
+import os
+import time
+
+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
+ROBOTIQ_3F_GRIPPER_MODE_ENUM_DICT = {"BASIC": 0, "PINCH": 1, "WIDE": 2, "SCISSOR": 3}
+
+
+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)
+        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] * self.NUM_ACTIVE_JOINTS
+        self.analog_command_value = 0.0
+
+    def current_mode_provider(self, _):
+        response = GetStringResponse()
+        response.data = self.current_mode
+        return response
+
+    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):
+        gripper_control_msg = Robotiq3FGripperRobotOutput()
+        gripper_control_msg.rACT = 1
+        self.robotiq_control_pub.publish(gripper_control_msg)
+
+    def mode_select(self):
+        gripper_control_msg = Robotiq3FGripperRobotOutput()
+        gripper_control_msg.rACT = 1
+        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()
+        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
+
+        gripper_control_msg.rMOD = ROBOTIQ_3F_GRIPPER_MODE_ENUM_DICT[self.current_mode]
+
+        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.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 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:
+            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):
+                    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 = 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)
+        self.gripper_command_publish()
+
+
+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. Valid modes are: {list(ROBOTIQ_3F_GRIPPER_MODE_ENUM_DICT.keys())}")
+
+    rospy.init_node("ur_robotiq_gripper")
+    gripper_control_node = Robotiq3FGripperControlNode(mode=gripper_mode)
+
+    gripper_control_node.reset_gripper()
+    time.sleep(1)
+
+    gripper_control_node.activate_gripper()
+    time.sleep(0.5)
+
+    gripper_control_node.mode_select()
+    time.sleep(0.5)
+
+    rospy.Subscriber("extend_gripper_command", GripperControl, gripper_control_node.vr_gripper_command_callback)
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    main()