#! /usr/bin/env python """ Python library to control Robotiq Two Finger Gripper connected to UR robot via Python-URX Tested using a UR5 Version CB3 and Robotiq 2-Finger Gripper Version 85 SETUP You must install the driver first and then power on the gripper from the gripper UI. The driver can be found here: http://support.robotiq.com/pages/viewpage.action?pageId=5963876 FAQ Q: Why does this class group all the commands together and run them as a single program as opposed to running each line seperately (like most of URX)? A: The gripper is controlled by connecting to the robot's computer (TCP/IP) and then communicating with the gripper via a socket (127.0.0.1:63352). The scope of the socket is at the program level. It will be automatically closed whenever a program finishes. Therefore it's important that we run all commands as a single program. DOCUMENTATION - This code was developed by downloading the gripper package "DCU-1.0.10" from http://support.robotiq.com/pages/viewpage.action?pageId=5963876. Or more directly from http://support.robotiq.com/download/attachments/5963876/DCU-1.0.10.zip - The file robotiq_2f_gripper_programs_CB3/rq_script.script was referenced to create this class FUTURE FEATURES Though I haven't developed it yet, if you look in robotiq_2f_gripper_programs_CB3/advanced_template_test.script and view function "rq_get_var" there is an example of how to determine the current state of the gripper and if it's holding an object. """ # noqa import logging import os import time from urx.urscript import URScript # Gripper Variables ACT = "ACT" GTO = "GTO" ATR = "ATR" ARD = "ARD" FOR = "FOR" SPE = "SPE" OBJ = "OBJ" STA = "STA" FLT = "FLT" POS = "POS" SOCKET_HOST = "127.0.0.1" SOCKET_PORT = 63352 SOCKET_NAME = "gripper_socket" class RobotiqScript(URScript): def __init__(self, socket_host=SOCKET_HOST, socket_port=SOCKET_PORT, socket_name=SOCKET_NAME): self.socket_host = socket_host self.socket_port = socket_port self.socket_name = socket_name super(RobotiqScript, self).__init__() # Reset connection to gripper self._socket_close(self.socket_name) self._socket_open(self.socket_host, self.socket_port, self.socket_name) def _import_rq_script(self): dir_path = os.path.dirname(os.path.realpath(__file__)) rq_script = os.path.join(dir_path, 'rq_script.script') with open(rq_script, 'rb') as f: rq_script = f.read() self.add_header_to_program(rq_script) def _rq_get_var(self, var_name, nbytes): self._socket_send_string("GET {}".format(var_name)) self._socket_read_byte_list(nbytes) def _get_gripper_fault(self): self._rq_get_var(FLT, 2) def _get_gripper_object(self): self._rq_get_var(OBJ, 1) def _get_gripper_status(self): self._rq_get_var(STA, 1) def _set_gripper_activate(self): self._socket_set_var(GTO, 1, self.socket_name) def _set_gripper_force(self, value): """ FOR is the variable range is 0 - 255 0 is no force 255 is full force """ value = self._constrain_unsigned_char(value) self._socket_set_var(FOR, value, self.socket_name) def _set_gripper_position(self, value): """ SPE is the variable range is 0 - 255 0 is no speed 255 is full speed """ value = self._constrain_unsigned_char(value) self._socket_set_var(POS, value, self.socket_name) def _set_gripper_speed(self, value): """ SPE is the variable range is 0 - 255 0 is no speed 255 is full speed """ value = self._constrain_unsigned_char(value) self._socket_set_var(SPE, value, self.socket_name) def _set_robot_activate(self): self._socket_set_var(ACT, 1, self.socket_name) class Robotiq_Two_Finger_Gripper(object): def __init__(self, robot, payload=0.85, speed=255, force=50, socket_host=SOCKET_HOST, socket_port=SOCKET_PORT, socket_name=SOCKET_NAME): self.robot = robot self.payload = payload self.speed = speed self.force = force self.socket_host = socket_host self.socket_port = socket_port self.socket_name = socket_name self.logger = logging.getLogger(u"robotiq") def _get_new_urscript(self): """ Set up a new URScript to communicate with gripper """ urscript = RobotiqScript(socket_host=self.socket_host, socket_port=self.socket_port, socket_name=self.socket_name) # Set input and output voltage ranges urscript._set_analog_inputrange(0, 0) urscript._set_analog_inputrange(1, 0) urscript._set_analog_inputrange(2, 0) urscript._set_analog_inputrange(3, 0) urscript._set_analog_outputdomain(0, 0) urscript._set_analog_outputdomain(1, 0) urscript._set_tool_voltage(0) urscript._set_runstate_outputs() # Set payload, speed and force urscript._set_payload(self.payload) urscript._set_gripper_speed(self.speed) urscript._set_gripper_force(self.force) # Initialize the gripper urscript._set_robot_activate() urscript._set_gripper_activate() return urscript def gripper_action(self, value): """ Activate the gripper to a given value from 0 to 255 0 is open 255 is closed """ urscript = self._get_new_urscript() # Move to the position sleep = 2.0 urscript._set_gripper_position(value) urscript._sleep(sleep) # Send the script self.robot.send_program(urscript()) # sleep the code the same amount as the urscript to ensure that # the action completes time.sleep(sleep) def open_gripper(self): self.gripper_action(0) def close_gripper(self): self.gripper_action(255)