"""
Created on Wed Oct 17 17:33:20 2018
Python class to control robotiq gripper via python
@author: Benoit CASTETS
"""
#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"
#General information
__author__ = "Benoit CASTETS"
__email__ = "b.castets@robotiq.com"
__license__ = "Apache License, Version 2.0"
[docs]
class RobotiqGripper( mm.Instrument ):
""""Class to control Robotiq grippers (2F85, 2F140, hande,...).
Suppose that the gripper is connected via the USB/RS485 adapter to the PC executing this code.
Some general information about robotiq gripper.
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
"""
def __init__(self, portname=AUTO_DETECTION,slaveAddress=9):
"""Create a RobotiqGripper object use to control Robotiq grippers
using modbus RTU protocol USB/RS485 connection.
Parameters
----------
portname:
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:
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.
The dictionnary is organize in 2 levels:
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 the parameter dictionnary
Dictionnary keys are:
- gOBJ Object detection status, is a built-in feature that 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 (input registers and status).
- gFLT Fault status returns general error messages that are useful for troubleshooting.
Fault LED (red) is present on the gripper chassis,LED can be blue, red or both and 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 *value read in mA.
"""
#Clear parameter dictionnary data
self.paramDic={}
registers=self.read_registers(2000,3)#Changed from 6 to 3 register reading. This modification has not been tested.
#########################################
#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
"""
#Turn the variable which indicate that the gripper is processing
#an action to True
self.processing=True
#Activate the gripper
self.write_registers(1000,[0b0000000100000000,0,0])#rACT=1 Activate Gripper (must stay on after activation routine is completed).
#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 activate
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.
Parameters
----------
position:
Position of the gripper. Integer between 0 and 255. 0 being the
open position and 255 being the close position.
speed:
Gripper speed between 0 and 255
force:
Gripper force between 0 and 255
Return
------
objectDetected:
True if object detected
position:
End position of the gripper
"""
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)
self.write_registers(1000,[0b0000100100000000,#gACT=1 (Gripper activation.) and gGTO=1 (Go to Position Request.)
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()
gOBJ=self.paramDic["gOBJ"]#Object detection status, is a built-in feature that provides information on possible object pick-up. Ignore if gGTO == 0.
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 closeGripper(self,speed=255,force=255):
"""Close the gripper
Parameters
----------
speed:
Gripper speed between 0 and 255
force:
Gripper force between 0 and 255
"""
self.goTo(255,speed,force)
[docs]
def openGripper(self,speed=255,force=255):
"""Open the gripper
Parameters
----------
speed:
Gripper speed between 0 and 255
force:
Gripper force between 0 and 255
"""
self.goTo(0,force,speed)
[docs]
def goTomm(self,positionmm,speed=255,force=255):
"""Go to the requested opening expressed in mm
Parameters
----------
positionmm:
Gripper opening in mm.
speed:
Gripper speed between 0 and 255
force:
Gripper force between 0 and 255
Return
------
Return 0 if succeed, 1 if failed.
"""
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 bit.
Return
------
position:
Gripper position in bit
current:
Motor current in bit. 1bit is about 10mA.
"""
self.readAll()
position=self.paramDic["gPO"]
return position
def _mmToBit(self,mm):
"""Convert a mm gripper opening in bit opening.
Calibration is needed to use this function.
"""
bit=(mm-self._bCoef)/self._aCoef
return bit
def _bitTomm(self,bit):
"""Convert a bit gripper opening in mm opening.
Calibration is needed to use this function.
"""
mm=self._aCoef*bit+self._bCoef
return mm
[docs]
def getPositionmm(self):
"""Return the position of the gripper in mm.
Calibration is need to use this function.
"""
position=self.getPosition()
positionmm=self._bitTomm(position)
return positionmm
[docs]
def calibrate(self,closemm,openmm):
"""Calibrate the gripper for mm positionning
"""
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 treminal
"""
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
Return
------
activated - bool:
True is 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
"""
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)