"""pyRobotiqGripper: Python Driver for Robotiq Grippers via Modbus RTU
pyRobotiqGripper is a Python library designed to facilitate control of Robotiq\
grippers using Modbus RTU communication via serial port.
This module provides documentation in two formats:
- Docstrings: Embedded within the code for easy access.
- Online Documentation: Extensive documentation available at\
<https://pyrobotiqgripper.readthedocs.io/en/latest/>.
"""
#General information
__author__ = "Benoit CASTETS"
__email__ = "opensourceeng@robotiq.com"
__license__ = "Apache License, Version 2.0"
__url__ = "https://github.com/castetsb/pyRobotiqGripper"
__version__ = "1.0.0"
#Iport libraries
import minimalmodbus as mm
import time
import serial
import serial.tools.list_ports
#Constants
BAUDRATE=115200
BYTESIZE=8
PARITY="N"
STOPBITS=1
TIMEOUT=0.2
AUTO_DETECTION="auto"
[docs]
class RobotiqGripper( mm.Instrument ):
"""Object control Robotiq grippers (2F85, 2F140 or hande).
Suppose that the gripper is connected via the USB/RS485 adapter to the PC\
executing this code.
Modbus RTU function code supported by robotiq gripper
======================================= ====================
Description Modbus function code
======================================= ====================
Read registers 4
Write registers 16
Master read & write multiple registers 23
======================================= ====================
For more information for gripper communication please check gripper manual
on Robotiq website.
https://robotiq.com/support/2f-85-2f-140
.. note::
This object cannot be use to control epick, 3F or powerpick.
"""
def __init__(self, portname=AUTO_DETECTION,slaveAddress=9):
"""Create a RobotiqGripper object whic can be use to control Robotiq\
grippers using modbus RTU protocol USB/RS485 connection.
Args:
- portname (str, optional): The serial port name, for example\
/dev/ttyUSB0 (Linux), /dev/tty.usbserial (OS X) or COM4\
(Windows). It is necesary to allowpermission to access this\
connection using the bash comman sudo chmod 666 /dev/ttyUSB0.\
By default the portname is set to "auto". In this case the\
connection is done with the first gripper found as connected\
to the PC.
- slaveaddress (int, optional): Address of the gripper (integer)\
usually 9.
"""
#Gripper salve address
self.slaveAddress=slaveAddress
#Port on which is connected the gripper
if portname == "auto":
self.portname=self._autoConnect()
if self.portname is None:
raise Exception("No gripper detected")
else:
self.portname=portname
#Create a pyserial object to connect to the gripper
ser=serial.Serial(self.portname,
BAUDRATE,
BYTESIZE,
PARITY,
STOPBITS,
TIMEOUT)
#Create the object using parent class contructor
super().__init__(ser,
self.slaveAddress,
mm.MODE_RTU,
close_port_after_each_call=False,
debug=False)
#Attribute to monitore if the gripper is processing an action
self.processing=False
#Maximum allowed time to perform and action
self.timeOut=10
#Dictionnary where are stored description of each register state
self.registerDic={}
self._buildRegisterDic()
#Dictionnary where are stored register values retrived from the gripper
self.paramDic={}
self.readAll()
#Attributes to store open and close distance state information
#Distance between the fingers when gripper is closed
self.closemm=None
#Position in bit when gripper is closed
self.closebit=None
#Distance between the fingers when gripper is open
self.openmm=None
#Position in bit when gripper is open
self.openbit=None
self._aCoef=None
self._bCoef=None
def _autoConnect(self):
"""Return the name of the port on which is connected the gripper
"""
ports=serial.tools.list_ports.comports()
portName=None
for port in ports:
try:
# Try opening the port
ser = serial.Serial(port.device,BAUDRATE,BYTESIZE,PARITY,STOPBITS,TIMEOUT)
device=mm.Instrument(ser,self.slaveAddress,mm.MODE_RTU,close_port_after_each_call=False,debug=False)
#Try to write the position 100
device.write_registers(1000,[0,100,0])
#Try to read the position request eco
registers=device.read_registers(2000,3,4)
posRequestEchoReg3=registers[1] & 0b0000000011111111
#Check if position request eco reflect the requested position
if posRequestEchoReg3 != 100:
raise Exception("Not a gripper")
portName=port.device
del device
ser.close() # Close the port
except:
pass # Skip if port cannot be opened
# If no suitable port is found
return portName
def _buildRegisterDic(self):
"""Build a dictionnary with comment to explain each register variable.
Dictionnary key are variable names. Dictionnary value are dictionnary\
with comments about each statut of the variable (key=variable value,\
value=comment)
"""
######################################################################
#input register variable
self.registerDic.update({"gOBJ":{},"gSTA":{},"gGTO":{},"gACT":{},
"kFLT":{},"gFLT":{},"gPR":{},"gPO":{},"gCU":{}})
#gOBJ
gOBJdic=self.registerDic["gOBJ"]
gOBJdic[0]="Fingers are in motion towards requested position. No\
object detected."
gOBJdic[1]="Fingers have stopped due to a contact while opening before\
requested position. Object detected opening."
gOBJdic[2]="Fingers have stopped due to a contact while closing before\
requested position. Object detected closing."
gOBJdic[3]="Fingers are at requested position. No object detected or\
object has been loss / dropped."
#gSTA
gSTAdic=self.registerDic["gSTA"]
gSTAdic[0]="Gripper is in reset ( or automatic release ) state. See\
Fault Status if Gripper is activated."
gSTAdic[1]="Activation in progress."
gSTAdic[3]="Activation is completed."
#gGTO
gGTOdic=self.registerDic["gGTO"]
gGTOdic[0]="Stopped (or performing activation / automatic release)."
gGTOdic[1]="Go to Position Request."
gGTOdic[2]="Unknown status"
gGTOdic[3]="Unknown status"
#gACT
gACTdic=self.registerDic["gACT"]
gACTdic[0]="Gripper reset."
gACTdic[1]="Gripper activation."
#kFLT
kFLTdic=self.registerDic["kFLT"]
i=0
while i<256:
kFLTdic[i]=i
i+=1
#See your optional Controller Manual (input registers & status).
#gFLT
gFLTdic=self.registerDic["gFLT"]
i=0
while i<256:
gFLTdic[i]=i
i+=1
gFLTdic[0]="No fault (LED is blue)"
gFLTdic[5]="Priority faults (LED is blue). Action delayed, activation\
(reactivation) must be completed prior to perfmoring the action."
gFLTdic[7]="Priority faults (LED is blue). The activation bit must be\
set prior to action."
gFLTdic[8]="Minor faults (LED continuous red). Maximum operating\
temperature exceeded, wait for cool-down."
gFLTdic[9]="Minor faults (LED continuous red). No communication during\
at least 1 second."
gFLTdic[10]="Major faults (LED blinking red/blue) - Reset is required\
(rising edge on activation bit rACT needed). Under minimum\
operating voltage."
gFLTdic[11]="Major faults (LED blinking red/blue) - Reset is required\
(rising edge on activation bit rACT needed). Automatic release in\
progress."
gFLTdic[12]="Major faults (LED blinking red/blue) - Reset is required\
(rising edge on activation bit rACT needed). Internal fault;\
contact support@robotiq.com."
gFLTdic[13]="Major faults (LED blinking red/blue) - Reset is required\
(rising edge on activation bit rACT needed). Activation fault,\
verify that no interference or other error occurred."
gFLTdic[14]="Major faults (LED blinking red/blue) - Reset is required\
(rising edge on activation bit rACT needed). Overcurrent triggered."
gFLTdic[15]="Major faults (LED blinking red/blue) - Reset is required\
(rising edge on activation bit rACT needed). Automatic release\
completed."
#gPR
gPRdic=self.registerDic["gPR"]
i=0
while i<256:
gPRdic[i]="Echo of the requested position for the Gripper:\
{}/255".format(i)
i+=1
#gPO
gPOdic=self.registerDic["gPO"]
i=0
while i<256:
gPOdic[i]="Actual position of the Gripper obtained via the encoders:\
{}/255".format(i)
i+=1
#gCU
gCUdic=self.registerDic["gCU"]
i=0
while i<256:
current=i*10
gCUdic[i]="The current is read instantaneously from the motor\
drive, approximate current: {} mA".format(current)
i+=1
######################################################################
#output register variable
self.registerDic.update({"rARD":{},
"rATR":{},
"rGTO":{},
"rACT":{},
"rPR":{},
"rFR":{},
"rSP":{}})
######################################################################
[docs]
def readAll(self):
"""Retrieve gripper output register information and save it in the\
parameter dictionary.
The dictionary keys are as follows:
- gOBJ: Object detection status. This built-in feature provides\
information on possible object pick-up. Ignore if gGTO == 0.
- gSTA: Gripper status. Returns the current status and motion of the\
gripper fingers.
- gGTO: Action status. Echo of the rGTO bit (go-to bit).
- gACT: Activation status. Echo of the rACT bit (activation bit).
- kFLT: See your optional controller manual for input registers and\
status.
- gFLT: Fault status. Returns general error messages useful for\
troubleshooting. A fault LED (red) is present on the gripper\
chassis. The LED can be blue, red, or both, and can be solid\
or blinking.
- gPR: Echo of the requested position for the gripper. Value between\
0x00 and 0xFF.
- gPO: Actual position of the gripper obtained via the encoders.\
Value between 0x00 and 0xFF.
- gCU: The current is read instantaneously from the motor drive. Value\
between 0x00 and 0xFF. Approximate current equivalent is 10 times\
the value read in mA.
"""
#Clear parameter dictionnary data
self.paramDic={}
#Read 3 16bits registers starting from register 2000
registers=self.read_registers(2000,3)
#########################################
#Register 2000
#First Byte: gripperStatus
#Second Byte: RESERVED
#First Byte: gripperStatus
gripperStatusReg0=(registers[0] >> 8) & 0b11111111 #xxxxxxxx00000000
#########################################
#Object detection
self.paramDic["gOBJ"]=(gripperStatusReg0 >> 6) & 0b11 #xx000000
#Gripper status
self.paramDic["gSTA"]=(gripperStatusReg0 >> 4) & 0b11 #00xx0000
#Action status. echo of rGTO (go to bit)
self.paramDic["gGTO"]=(gripperStatusReg0 >> 3) & 0b1 #0000x000
#Activation status
self.paramDic["gACT"]=gripperStatusReg0 & 0b00000001 #0000000x
#########################################
#Register 2001
#First Byte: Fault status
#Second Byte: Pos request echo
#First Byte: fault status
faultStatusReg2= (registers[1] >>8) & 0b11111111 #xxxxxxxx00000000
#########################################
#Universal controler
self.paramDic["kFLT"]=(faultStatusReg2 >> 4) & 0b1111 #xxxx0000
#Fault
self.paramDic["gFLT"]=faultStatusReg2 & 0b00001111 #0000xxxx
#########################################
#Second Byte: Pos request echo
posRequestEchoReg3=registers[1] & 0b11111111 #00000000xxxxxxxx
#########################################
#Echo of request position
self.paramDic["gPR"]=posRequestEchoReg3
#########################################
#Register 2002
#First Byte: Position
#Second Byte: Current
#First Byte: Position
positionReg4=(registers[2] >> 8) & 0b11111111 #xxxxxxxx00000000
#########################################
#Actual position of the gripper
self.paramDic["gPO"]=positionReg4
#########################################
#Second Byte: Current
currentReg5=registers[2] & 0b0000000011111111 #00000000xxxxxxxx
#########################################
#Current
self.paramDic["gCU"]=currentReg5
[docs]
def reset(self):
"""Reset the gripper (clear previous activation if any)
"""
#Reset the gripper
self.write_registers(1000,[0,0,0])
[docs]
def activate(self):
"""If not already activated, activate the gripper.
.. warning::
When you execute this function the gripper is going to fully open\
and close. During this operation the gripper must be able to freely\
move. Do not place object inside the gripper.
"""
#Turn the variable which indicate that the gripper is processing
#an action to True
self.processing=True
#Activate the gripper
#rACT=1 Activate Gripper (must stay on after activation routine is
#completed).
self.write_registers(1000,[0b0000000100000000,0,0])
#Waiting for activation to complete
activationStartTime=time.time()
activationCompleted=False
activationTime=0
while (not activationCompleted) and activationTime < self.timeOut:
activationTime = time.time() - activationStartTime
self.readAll()
gSTA=self.paramDic["gSTA"]
if gSTA==3:
activationCompleted=True
print("Activation completed. Activation time : "
, activationTime)
if activationTime > self.timeOut:
raise Exception("Activation did not complete without timeout.")
self.processing=False
[docs]
def resetActivate(self):
"""Reset the gripper (clear previous activation if any) and activat\
the gripper. During this operation the gripper will open and close.
"""
#Reset the gripper
self.reset()
#Activate the gripper
self.activate()
[docs]
def goTo(self,position,speed=255,force=255):
"""Go to the position with determined speed and force.
Args:
- position (int): Position of the gripper. Integer between 0 and 255.\
0 being the open position and 255 being the close position.
- speed (int): Gripper speed between 0 and 255
- force (int): Gripper force between 0 and 255
Returns:
- objectDetected (bool): True if object detected
- position (int): End position of the gripper in bits
"""
position=int(position)
speed=int(speed)
force=int(force)
#Check if the grippre is activated
if self.isActivated == False:
raise Exception ("Gripper must be activated before requesting\
an action.")
#Check input value
if position>255:
raise Exception("Position value cannot exceed 255")
elif position<0:
raise Exception("Position value cannot be under 0")
self.processing=True
#rARD(5) rATR(4) rGTO(3) rACT(0)
#gACT=1 (Gripper activation.) and gGTO=1 (Go to Position Request.)
self.write_registers(1000,[0b0000100100000000,
position,
speed * 0b100000000 + force])
#Waiting for activation to complete
motionStartTime=time.time()
motionCompleted=False
motionTime=0
objectDetected=False
while (not objectDetected) and (not motionCompleted)\
and (motionTime<self.timeOut):
motionTime= time.time()- motionStartTime
self.readAll()
#Object detection status, is a built-in feature that provides
#information on possible object pick-up. Ignore if gGTO == 0.
gOBJ=self.paramDic["gOBJ"]
if gOBJ==1 or gOBJ==2:
#Fingers have stopped due to a contact
objectDetected=True
elif gOBJ==3:
#Fingers are at requested position.
objectDetected=False
motionCompleted=True
if motionTime>self.timeOut:
raise Exception("Gripper never reach its requested position and\
no object have been detected")
position=self.paramDic["gPO"]
return position, objectDetected
[docs]
def close(self,speed=255,force=255):
"""Close the gripper.
Args:
- speed (int, optional): Gripper speed between 0 and 255.\
Default is 255.
- force (int, optional): Gripper force between 0 and 255.\
Default is 255.
"""
self.goTo(255,speed,force)
[docs]
def open(self,speed=255,force=255):
"""Open the gripper
Args:
- speed (int, optional): Gripper speed between 0 and 255.\
Default is 255.
- force (int, optional): Gripper force between 0 and 255.\
Default is 255.
"""
self.goTo(0,force,speed)
[docs]
def goTomm(self,positionmm,speed=255,force=255):
"""Go to the requested opening expressed in mm
Args:
- positionmm (float): Gripper opening in mm.
- speed (int, optional): Gripper speed between 0 and 255.\
Default is 255.
- force (int, optional): Gripper force between 0 and 255.\
Default is 255.
.. note::
Calibration is needed to use this function.\n
Execute the function calibrate at least 1 time before using this function.
"""
if self.isCalibrated == False:
raise Exception("The gripper must be calibrated before been requested to go to a position in mm")
if positionmm>self.openmm:
raise Exception("The maximum opening is {}".format(self.openmm))
position=int(self._mmToBit(positionmm))
self.goTo(position,speed,force)
[docs]
def getPosition(self):
"""Return the position of the gripper in bits
Returns:
- int: Position of the gripper in bits.
"""
self.readAll()
position=self.paramDic["gPO"]
return position
def _mmToBit(self,mm):
"""Convert a mm gripper opening in bit opening.
.. note::
Calibration is needed to use this function.\n
Execute the function calibrate at least 1 time before using this function.
"""
bit=(mm-self._bCoef)/self._aCoef
return bit
def _bitTomm(self,bit):
"""Convert a bit gripper opening in mm opening.
Returns:
float: Gripper position converted in mm
.. note::
Calibration is needed to use this function.\n
Execute the function calibrate at least 1 time before using this function.
"""
mm=self._aCoef*bit+self._bCoef
return mm
[docs]
def getPositionmm(self):
"""Return the position of the gripper in mm.
Returns:
float: Current gripper position in mm
.. note::
Calibration is needed to use this function.\n
Execute the function calibrate at least 1 time before using this function.
"""
position=self.getPosition()
positionmm=self._bitTomm(position)
return positionmm
[docs]
def calibrate(self,closemm,openmm):
"""Calibrate the gripper for mm positionning.
Once the calibration is done it is possible to control the gripper in\
mm.
Args:
- closemm (float): Distance between the fingers when the gripper is\
fully closed.
- openmm (float): Distance between the fingers when the gripper is\
fully open.
"""
self.closemm=closemm
self.openmm=openmm
self.openGripper()
#get open bit
self.openbit=self.getPosition()
obit=self.openbit
self.closeGripper()
#get close bit
self.closebit=self.getPosition()
cbit=self.closebit
self._aCoef=(closemm-openmm)/(cbit-obit)
self._bCoef=(openmm*cbit-obit*closemm)/(cbit-obit)
[docs]
def printInfo(self):
"""Print gripper register info in the python terminal
"""
self.readAll()
for key,value in self.paramDic.items():
print("{} : {}".format(key,value))
print(self.registerDic[key][value])
[docs]
def isActivated(self):
"""Tells if the gripper is activated
Returns:
bool: True if the gripper is activated. False otherwise.
"""
self.readAll()
is_activated = (self.paramDic["gSTA"]==3)
return is_activated
[docs]
def isCalibrated(self):
"""Return if the gripper is qualibrated
Returns:
bool: True if the gripper is calibrated. False otherwise.
"""
is_calibrated = False
if (self.openmm is None) or (self.closemm is None):
is_calibrated = False
else:
is_calibrated=True
return is_calibrated
#Test
if False:
grip=RobotiqGripper()
grip.resetActivate()
#grip.reset()
#grip.goTo(0)
#grip.goTo(255)
#grip.printInfo()
#grip.goTo(255)
#time.sleep(5)
#grip.printInfo()
#grip.activate()
#grip.printInfo()
#grip.goTo(20)
#grip.goTo(230)
#grip.goTo(40)
#grip.goTo(80)
#grip.calibrate(0,36)
#grip.goTomm(10,255,255)
#grip.goTomm(40,1,255)