├── setup.py ├── LICENSE ├── .gitignore ├── README.md └── Robotiq2F85Driver └── Robotiq2F85Driver.py /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | setup( 4 | name='2f85-python-driver', 5 | version='0.0.1', 6 | description='A simple independant Python driver to use the Robotiq 2F-85 Gripper.', 7 | author='Philippe Nadeau', 8 | author_email='philippe.nadeau@robotics.utias.utoronto.ca', 9 | license='MIT', 10 | packages=['Robotiq2F85Driver'], 11 | install_requires=['minimalmodbus>=2.1'], 12 | python_requires=">=3.8", 13 | include_package_data=True 14 | ) 15 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 Philippe Nadeau 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | *.py,cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | cover/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | .pybuilder/ 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | # For a library or package, you might want to ignore these files since the code is 87 | # intended to run in multiple environments; otherwise, check them in: 88 | # .python-version 89 | 90 | # pipenv 91 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 92 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 93 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 94 | # install all needed dependencies. 95 | #Pipfile.lock 96 | 97 | # poetry 98 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 99 | # This is especially recommended for binary packages to ensure reproducibility, and is more 100 | # commonly ignored for libraries. 101 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 102 | #poetry.lock 103 | 104 | # pdm 105 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 106 | #pdm.lock 107 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 108 | # in version control. 109 | # https://pdm.fming.dev/#use-with-ide 110 | .pdm.toml 111 | 112 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 113 | __pypackages__/ 114 | 115 | # Celery stuff 116 | celerybeat-schedule 117 | celerybeat.pid 118 | 119 | # SageMath parsed files 120 | *.sage.py 121 | 122 | # Environments 123 | .env 124 | .venv 125 | env/ 126 | venv/ 127 | ENV/ 128 | env.bak/ 129 | venv.bak/ 130 | 131 | # Spyder project settings 132 | .spyderproject 133 | .spyproject 134 | 135 | # Rope project settings 136 | .ropeproject 137 | 138 | # mkdocs documentation 139 | /site 140 | 141 | # mypy 142 | .mypy_cache/ 143 | .dmypy.json 144 | dmypy.json 145 | 146 | # Pyre type checker 147 | .pyre/ 148 | 149 | # pytype static type analyzer 150 | .pytype/ 151 | 152 | # Cython debug symbols 153 | cython_debug/ 154 | 155 | # PyCharm 156 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 157 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 158 | # and can be added to the global gitignore or merged into this file. For a more nuclear 159 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 160 | #.idea/ 161 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Robotiq 2F-85 Driver 2 | 3 | The `Robotiq2F85Driver` is a Python module that provides a driver for interacting with the Robotiq 2F-85 gripper. It includes functionalities for finding the gripper's serial port, activating/deactivating the gripper, reading its status, and controlling its movements. 4 | 5 | ## Features 6 | Compared to existing packages, this one is different in the following ways: 7 | - It is independant from ROS, you only need to import the Python module to use the gripper. 8 | - It is a pure Python implementation, no need to compile any C++ code. 9 | - It detects the gripper's serial port based on its serial number. 10 | - It provides a function to account for the motion of the fingertips when planning grasps. 11 | 12 | ## Installation 13 | 14 | Make sure you have Python 3.x installed. After cloning the repository, you can install the package using the following command: 15 | 16 | ```bash 17 | pip install . 18 | ``` 19 | 20 | executed from the root directory of the repository. 21 | 22 | To allow access to the serial connection, you will also need to add make your user a member of the `dialout` group with 23 | ```bash 24 | sudo adduser $USER dialout 25 | ``` 26 | and logout/login to refresh the membership. 27 | 28 | ## Usage 29 | 30 | ```python 31 | from Robotiq2F85Driver import Robotiq2F85Driver 32 | 33 | # Initialize the driver with the gripper's serial number 34 | gripper = Robotiq2F85Driver(serial_number='DAK1RLYZ') 35 | 36 | # Reset the gripper 37 | gripper.reset() 38 | 39 | # Move the gripper to fully open position (opening = 85 mm) 40 | # The motion is done at 150 mm/s with a force of up to 235 Newtons. 41 | gripper.go_to(opening=85, speed=150, force=235) 42 | 43 | # Get the current gripper opening 44 | print(gripper.opening) 45 | ``` 46 | 47 | ## Feature Desrciption 48 | 49 | ### Getting Serial Port from Serial Number 50 | 51 | In contrast to most existing packages, this driver finds the serial port of a device with a specific serial number so you dont need to worry about the port number changing when you plug the device in. This is done by iterating over all `/dev/ttyUSB*` devices and finding the one with the given serial number. 52 | 53 | 54 | #### Accounting for the motion of the fingertips 55 | Ever had the gripper collide with the table when closing onto a small object? Not anymore! This driver provides a function that can be used to account for the motion of the fingertips when planning grasps. 56 | 57 | Due to how the gripper is designed, the fingertips move away from the gripper's base frame when the gripper closes. This can be inconvenient when the Tool Center Point (TCP) is on the fingertips and needs to be precisely positioned to accurately grasp small objects. The `tcp_Z_offset` function calculates the distance along the gripper Z+ axis that the TCP will move when the gripper goes from its current opening to the desired opening. This function is particularly useful when planning robotic movements for grasping objects. Knowing the offset allows the robot to compensate for the gripper's movement and position the TCP precisely at the desired location before grasping an object of known thickness. 58 | 59 | ##### Simple Geometrical Model 60 | ![image](https://github.com/PhilNad/2f85-python-driver/assets/10478385/144962e3-0422-4eec-9504-95bdf656d31f) 61 | 62 | ##### Derivation 63 | Let 64 | ```math 65 | d = \frac{opening}{2} + pad\_thickness 66 | ``` 67 | be defined in millimeters, then 68 | ```math 69 | \theta = 70 | \begin{cases} 71 | d \lt 12.7 :& -\arcsin\left(\frac{12.7-d}{57.15}\right)\\ 72 | d \geq 12.7 :& \arcsin\left(\frac{d-12.7}{57.15}\right) 73 | \end{cases} 74 | ``` 75 | and 76 | ```math 77 | \begin{align} 78 | TCP_z &= 61.308 + 57.15\cos(\theta) + 7 + 38/2\\ 79 | &= 87.308 + 57.15\cos(\theta) 80 | \end{align} 81 | ``` 82 | is the distance along the Z+ axis of the base frame between the base frame and the TCP frame. 83 | 84 | Noting that 85 | ```math 86 | \cos(\arcsin(x)) = \cos(-\arcsin(x)) = \sqrt{1-x^2} 87 | ``` 88 | we get 89 | ```math 90 | TCP_Z = 91 | \begin{cases} 92 | d \lt 12.7 :& 87.308 + 57.15\sqrt{1-\left(\frac{12.7-d}{57.15}\right)^2}\\ 93 | d \geq 12.7 :& 87.308 + 57.15\sqrt{1-\left(\frac{d-12.7}{57.15}\right)^2} 94 | \end{cases} 95 | ``` 96 | that provides an expression for the position of the TCP frame as a function of the opening and pad thickness (both in millimeters). 97 | 98 | 99 | #### Useful Gripper Properties 100 | 101 | - `opening: float`: Current gripper opening in millimeters. 102 | 103 | - `current: float`: Current in milliamps. 104 | 105 | - `is_activated: bool`: Check if the gripper is activated. 106 | 107 | - `is_moving: bool`: Check if the gripper is currently moving. 108 | 109 | - `object_detected: bool`: Check if an object is detected by the gripper. 110 | 111 | - `in_fault: bool`: Check if the gripper is in a fault state. 112 | 113 | ## Limitations 114 | Currently assumes that Linux is used as the operating system, which could be easily extended to support other operating systems. 115 | 116 | ## License 117 | 118 | This project is licensed under the [MIT License](LICENSE). 119 | -------------------------------------------------------------------------------- /Robotiq2F85Driver/Robotiq2F85Driver.py: -------------------------------------------------------------------------------- 1 | from pathlib import Path 2 | import subprocess 3 | import minimalmodbus as mm 4 | from dataclasses import dataclass 5 | import struct 6 | import math 7 | import time 8 | 9 | class LinuxFindTTYWithSerialNumber: 10 | def __init__(self): 11 | pass 12 | 13 | def find(self, serial_number): 14 | ''' 15 | Iterate over all /dev/ttyUSB* devices and try to find the one with the given serial number. 16 | 17 | A list of devices can be obtained with: ls /dev/ttyUSB* 18 | The serial number of a given device can be obtained with: udevadm info -a -n /dev/ttyUSB0 | grep 'ATTRS{serial}' | head -n1 19 | producing the output: ATTRS{serial}=="serial_number". 20 | 21 | Parameters: 22 | ----------- 23 | serial_number : str 24 | Serial number of the device to find. 25 | 26 | Returns: 27 | -------- 28 | str 29 | The path to the device with the given serial number or None if no device matching the specified serial number was found. 30 | ''' 31 | 32 | # Get the list of all /dev/ttyUSB* devices. 33 | tty_devices = Path('/dev').glob('ttyUSB*') 34 | 35 | # Iterate over all /dev/ttyUSB* devices and try to find the one with the given serial number. 36 | for tty_device in tty_devices: 37 | # Get the serial number of the current device. 38 | current_serial_number = self.get_serial_number(tty_device) 39 | 40 | # Check if the serial number of the current device matches the specified serial number. 41 | if current_serial_number == serial_number: 42 | # Return the path to the current device. 43 | return str(tty_device) 44 | 45 | # Return None if no device matching the specified serial number was found. 46 | return None 47 | 48 | def get_serial_number(self, tty_device:Path): 49 | ''' 50 | Get the serial number of a given device. 51 | 52 | Parameters: 53 | ----------- 54 | tty_device : Path 55 | Path to the device. 56 | 57 | Returns: 58 | -------- 59 | str 60 | The serial number of the device. 61 | ''' 62 | 63 | # Get the serial number of the device by running the following command: 64 | # udevadm info -a -n /dev/ttyUSB0 | grep 'ATTRS{serial}' | head -n1 65 | # producing the output: ATTRS{serial}=="serial_number". 66 | output = subprocess.check_output(['udevadm', 'info', '-a', '-n', str(tty_device)]).decode('utf-8') 67 | output = output.split('\n') 68 | output = [line for line in output if 'ATTRS{serial}' in line] 69 | 70 | #If no line with 'ATTRS{serial}' was found, return None. 71 | if len(output) == 0: 72 | return None 73 | 74 | try: 75 | serial_number = output[0].split('"')[1] 76 | except: 77 | serial_number = None 78 | 79 | # Return the serial number of the device. 80 | return serial_number 81 | 82 | @dataclass 83 | class GripperFault: 84 | #Reactivation must be performed before any further movement. 85 | reactivation_required: bool 86 | #Activation bit must be set prior to action. 87 | activation_required: bool 88 | #Gripper's temperature has risen above 85C and it needs to cool down. 89 | overheating: bool 90 | #There was no communication within the last second 91 | communication_timeout: bool 92 | #The voltage supplied to the gripper is below 21.6 Volts 93 | undervoltage: bool 94 | #Automatic release in progress 95 | is_auto_releasing: bool 96 | #Automatic release completed 97 | auto_release_completed: bool 98 | #Internal fault, contact manufacturer. 99 | internal_fault: bool 100 | #Activation fault 101 | activation_fault: bool 102 | #A current of more than 1 Amp. was supplied 103 | overcurrent: bool 104 | 105 | @dataclass 106 | class GripperStatus: 107 | activated: bool 108 | moving: bool 109 | #In milliamps 110 | current: float 111 | obj_detected: bool 112 | #In millimeters 113 | opening: float 114 | #In millimeters 115 | goal_opening: float 116 | is_reset: bool 117 | is_activating: bool 118 | is_activated: bool 119 | fault: GripperFault = GripperFault(0,0,0,0,0,0,0,0,0,0) 120 | 121 | 122 | class Robotiq2F85Driver: 123 | def __init__(self, serial_number:str, debug=False): 124 | self.debug = debug 125 | self.device_serial_number = serial_number 126 | self.tty_device = LinuxFindTTYWithSerialNumber().find(serial_number) 127 | 128 | if self.tty_device is None: 129 | raise Exception('No device with serial number {} found.'.format(serial_number)) 130 | 131 | self.client = mm.Instrument(port=self.tty_device, slaveaddress=9, mode=mm.MODE_RTU, debug=self.debug) 132 | self.client.serial.baudrate = 115200 133 | self.client.serial.parity = mm.serial.PARITY_NONE 134 | self.client.serial.bytesize = 8 135 | self.client.serial.stopbits = mm.serial.STOPBITS_ONE 136 | 137 | @property 138 | def opening(self): 139 | '''Current opening in millimeters''' 140 | return self.read_status().opening 141 | 142 | @property 143 | def goal_opening(self): 144 | '''Goal opening in millimeters''' 145 | return self.read_status().goal_opening 146 | 147 | @property 148 | def current(self): 149 | '''Current in milliamps''' 150 | return self.read_status().current 151 | 152 | @property 153 | def is_reset(self): 154 | return self.read_status().is_reset 155 | 156 | @property 157 | def is_activating(self): 158 | return self.read_status().is_activating 159 | 160 | @property 161 | def is_activated(self): 162 | return self.read_status().is_activated 163 | 164 | @property 165 | def is_moving(self): 166 | return self.read_status().moving 167 | 168 | @property 169 | def object_detected(self): 170 | return self.read_status().obj_detected 171 | 172 | @property 173 | def in_fault(self): 174 | fault = self.read_status().fault 175 | return fault.reactivation_required or fault.activation_required or fault.overheating or fault.undervoltage or fault.internal_fault or fault.activation_fault or fault.overcurrent 176 | 177 | def count_to_opening(self, count:int): 178 | '''Converts a count to an opening in millimeters''' 179 | count = min(max(count, 0), 255) 180 | opening = (230 - count) * 0.39 181 | opening = min(max(opening, 0), 85) 182 | return opening 183 | 184 | def opening_to_count(self, opening:float): 185 | '''Converts an opening in millimeters to a count''' 186 | opening = min(max(opening, 0), 85) 187 | count = 230 - (opening / 0.39) 188 | return int(count) 189 | 190 | def count_to_speed(self, count:int): 191 | '''Converts a count to a speed in mm/s 192 | The speed is between 20-150 mm/s for counts 0-255. 193 | ''' 194 | count = min(max(count, 0), 255) 195 | speed = (count / 255) * (150 - 20) + 20 196 | return speed 197 | 198 | def speed_to_count(self, speed:float): 199 | '''Converts a speed in mm/s to a count. 200 | The speed is between 20-150 mm/s for counts 0-255. 201 | ''' 202 | count = (speed - 20) / (150 - 20) * 255 203 | count = min(max(count, 0), 255) 204 | return int(count) 205 | 206 | def count_to_force(self, count:int): 207 | '''Converts a count to a force in N 208 | The force is between 20-235 N for counts 0-255. 209 | ''' 210 | force = (count / 255) * (235 - 20) + 20 211 | force = min(max(force, 0), 255) 212 | return force 213 | 214 | def force_to_count(self, force:float): 215 | '''Converts a force in N to a count 216 | The force is between 20-235 N for counts 0-255. 217 | ''' 218 | count = (force - 20) / (235 - 20) * 255 219 | count = min(max(count, 0), 255) 220 | return int(count) 221 | 222 | def count_to_current(self, count:int): 223 | '''Converts a count to a current in mA''' 224 | current = count * 0.1 225 | return current 226 | 227 | def activate(self, blocking_call:bool=True): 228 | ''' 229 | Activate the gripper. 230 | ''' 231 | action_request_register = 1 << 8 232 | gripper_options1_register = 0 233 | self.client.write_registers(registeraddress=1000, values=[action_request_register + gripper_options1_register]) 234 | 235 | if blocking_call: 236 | #Read the status while the gripper is activating 237 | while self.is_activating: 238 | pass 239 | #Read the status until the gripper is activated 240 | while not self.is_activated: 241 | pass 242 | 243 | def deactivate(self, blocking_call:bool=True): 244 | ''' 245 | Deactivate the gripper. 246 | ''' 247 | action_request_register = 0 << 8 248 | gripper_options1_register = 0 249 | self.client.write_registers(registeraddress=1000, values=[action_request_register + gripper_options1_register]) 250 | 251 | if blocking_call: 252 | #Read the status until the gripper is deactivated 253 | while self.is_activated: 254 | pass 255 | 256 | def reset(self, blocking_call:bool=True): 257 | ''' 258 | Reset the gripper. 259 | ''' 260 | self.deactivate(blocking_call) 261 | self.activate(blocking_call) 262 | 263 | def tcp_Z_from_opening(self, opening:float, pad_thickness:float=7.8): 264 | ''' 265 | Returns the distance between the gripper base frame and the middle of the fingertips 266 | when the distance between the fingertips is `opening` and the pad thickness is `pad_thickness`. 267 | 268 | Parameters 269 | ---------- 270 | opening : float 271 | Distance between the fingertips. Fully open is 85mm, and fully closed is 0mm. 272 | pad_thickness : float 273 | Thickness of the pads. Default is 7.8mm (silicone pads). 274 | ''' 275 | #Distance from the Z axis to the farthest side of the fingertip 276 | d = opening/2 + pad_thickness 277 | if d < 12.7: 278 | tcp_z = 87.308 + 57.15*math.sqrt(1 - ((12.7 - d) / 57.15)**2) 279 | else: 280 | tcp_z = 87.308 + 57.15*math.sqrt(1 - ((d - 12.7) / 57.15)**2) 281 | return tcp_z 282 | 283 | def tcp_Z_offset(self, desired_opening:float, pad_thickness:float=7.8): 284 | ''' 285 | Returns the distance along the gripper Z+ axis that the TCP (fixed at the middle of the fingertip) 286 | will move when the gripper goes from its current opening to the desired opening. 287 | 288 | This can be used to compensate for the gripper's movement when the opening is changed such that 289 | the TCP ends up at the desired position. This requires knowing the thickness of the object to be 290 | grasped. The robot can be moved to compensate for this offset prior to grasping the object. 291 | 292 | Parameters 293 | ---------- 294 | desired_opening : float 295 | Desired opening in millimeters. 296 | pad_thickness : float 297 | Thickness of the pads. Default is 7.8mm (silicone pads). 298 | ''' 299 | current_opening = self.opening 300 | current_tcp_z = self.tcp_Z_from_opening(current_opening, pad_thickness) 301 | desired_tcp_z = self.tcp_Z_from_opening(desired_opening, pad_thickness) 302 | tcp_z_offset = desired_tcp_z - current_tcp_z 303 | return tcp_z_offset 304 | 305 | def go_to(self, opening:float, speed:float, force:float, blocking_call:bool=True): 306 | ''' 307 | Move the gripper to the specified opening, speed and force. 308 | 309 | Parameters: 310 | ----------- 311 | opening : float 312 | Opening in millimeters. Must be between 0 and 85 mm. 313 | speed : float 314 | Speed in mm/s. Must be between 20 and 150 mm/s. 315 | force : float 316 | Force in N. Must be between 20 and 235 N. 317 | ''' 318 | opening_count = self.opening_to_count(opening) 319 | speed_count = self.speed_to_count(speed) 320 | force_count = self.force_to_count(force) 321 | 322 | #Byte 0 323 | action_request_register = (2**0 + 2**3) << 8 324 | #Byte 1 325 | gripper_options1_register = 0 326 | #Byte 2 327 | gripper_options2_register = 0 328 | #Byte 3 329 | position_request_register = opening_count 330 | #Byte 4 331 | speed_register = speed_count << 8 332 | #Byte 5 333 | force_register = force_count 334 | 335 | self.client.write_registers(registeraddress=1000, values=[action_request_register + gripper_options1_register, 336 | gripper_options2_register + position_request_register, 337 | speed_register + force_register]) 338 | 339 | if blocking_call: 340 | #Read the status until the gripper is stopped 341 | while self.is_moving: 342 | pass 343 | 344 | def read_status(self) -> GripperStatus: 345 | values = self.client.read_registers(registeraddress=2000, number_of_registers=3, functioncode=4) 346 | #Each register is 16 bits and therefore contains two unsigned char each 347 | gripper_status_register, reserved_register = struct.unpack("BB", values[0].to_bytes(2, "big")) 348 | fault_status_register, position_request_echo_register = struct.unpack("BB", values[1].to_bytes(2, "big")) 349 | position_register, current_register = struct.unpack("BB", values[2].to_bytes(2, "big")) 350 | 351 | status = GripperStatus 352 | 353 | gripper_fault = GripperFault 354 | gripper_fault.reactivation_required = bool(fault_status_register == 0x05) 355 | gripper_fault.activation_required = bool(fault_status_register == 0x07) 356 | gripper_fault.overheating = bool(fault_status_register == 0x08) 357 | gripper_fault.undervoltage = bool(fault_status_register == 0x0A) 358 | gripper_fault.is_auto_releasing = bool(fault_status_register == 0x0B) 359 | gripper_fault.internal_fault = bool(fault_status_register == 0x0C) 360 | gripper_fault.activation_fault = bool(fault_status_register == 0x0D) 361 | gripper_fault.overcurrent = bool(fault_status_register == 0x0E) 362 | gripper_fault.auto_release_completed= bool(fault_status_register == 0x0F) 363 | status.fault = gripper_fault 364 | 365 | status.activated = bool(gripper_status_register & 2**0) 366 | status.moving = bool(gripper_status_register & 2**3) and (not bool(gripper_status_register & 2**6) and not bool(gripper_status_register & 2**7)) 367 | status.is_reset = not bool(gripper_status_register & 2**4) and not bool(gripper_status_register & 2**5) 368 | status.is_activating = bool(gripper_status_register & 2**4) and not bool(gripper_status_register & 2**5) 369 | status.is_activated = bool(gripper_status_register & 2**4) and bool(gripper_status_register & 2**5) 370 | status.obj_detected = ( bool(gripper_status_register & 2**6) and not bool(gripper_status_register & 2**7) ) or ( not bool(gripper_status_register & 2**6) and bool(gripper_status_register & 2**7) ) 371 | 372 | status.goal_opening = self.count_to_opening(position_request_echo_register) 373 | status.current = self.count_to_current(current_register) 374 | status.opening = self.count_to_opening(position_register) 375 | 376 | #Print the status of the gripper 377 | if self.debug: 378 | print("Gripper Status: ") 379 | print("\tActivated: {}".format(status.activated)) 380 | print("\tMoving: {}".format(status.activated)) 381 | print("\tIs Reset: {}".format(status.activated)) 382 | print("\tIs Activating: {}".format(status.is_activating)) 383 | print("\tIs Activated: {}".format(status.is_activated)) 384 | print("\tObj Detected: {}".format(status.obj_detected)) 385 | print("\tFaults: ",end='') 386 | if gripper_fault.reactivation_required: 387 | print("Reactivation required, ", end='') 388 | if gripper_fault.activation_required: 389 | print("Activation required, ",end='') 390 | if gripper_fault.overheating: 391 | print("Overheating, ",end='') 392 | if gripper_fault.undervoltage: 393 | print("Undervoltage, ",end='') 394 | if gripper_fault.is_auto_releasing: 395 | print("Is auto-releasing, ",end='') 396 | if gripper_fault.internal_fault: 397 | print("Internal fault, ",end='') 398 | if gripper_fault.activation_fault: 399 | print("Activation fault, ",end='') 400 | if gripper_fault.overcurrent: 401 | print("Overcurrent, ",end='') 402 | if gripper_fault.auto_release_completed: 403 | print("Auto-release completed", end='') 404 | print() 405 | 406 | #The maximum rate at which readings/commands can be sent is 200Hz 407 | time.sleep(5/1000) 408 | 409 | return status 410 | 411 | 412 | if __name__ == '__main__': 413 | robotiq_2f85_driver = Robotiq2F85Driver(serial_number='DAK1RLYZ') 414 | robotiq_2f85_driver.reset() 415 | robotiq_2f85_driver.go_to(opening=85, speed=150, force=235) 416 | print(robotiq_2f85_driver.opening) 417 | 418 | --------------------------------------------------------------------------------