214 lines
6.2 KiB
Python
214 lines
6.2 KiB
Python
#! /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 = b"ACT"
|
|
GTO = b"GTO"
|
|
ATR = b"ATR"
|
|
ARD = b"ARD"
|
|
FOR = b"FOR"
|
|
SPE = b"SPE"
|
|
OBJ = b"OBJ"
|
|
STA = b"STA"
|
|
FLT = b"FLT"
|
|
POS = b"POS"
|
|
|
|
SOCKET_HOST = b"127.0.0.1"
|
|
SOCKET_PORT = 63352
|
|
SOCKET_NAME = b"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(b"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)
|