├── .gitignore ├── requirements.txt ├── img └── rg6_2x.gif ├── README.md ├── LICENSE └── src ├── demo.py └── onrobot.py /.gitignore: -------------------------------------------------------------------------------- 1 | docs 2 | __pycache__ -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | pymodbus==2.5.3 -------------------------------------------------------------------------------- /img/rg6_2x.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/takuya-ki/onrobot-rg/HEAD/img/rg6_2x.gif -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # onrobot-rg 2 | 3 | [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) 4 | ![repo size](https://img.shields.io/github/repo-size/takuya-ki/onrobot-rg) 5 | 6 | Controller for OnRobot RG2 and RG6 grippers. 7 | 8 | ## Requirements 9 | 10 | - Python 3.7.3 11 | - pymodbus==2.5.3 12 | 13 | ## Installation 14 | 15 | ```bash 16 | git clone git@github.com:takuya-ki/onrobot-rg.git && cd onrobot-rg && pip install -r requirements.txt 17 | ``` 18 | 19 | ## Usage 20 | 21 | 1. Connect the cable between Compute Box and Tool Changer. 22 | 2. Connect an ethernet cable between Compute Box and your computer. 23 | 3. Execute a demo script as below 24 | ```bash 25 | python src/demo.py --ip 192.168.1.1 --port 502 --gripper rg2 26 | ``` 27 | ```bash 28 | python src/demo.py --ip 192.168.1.1 --port 502 --gripper rg6 29 | ``` 30 | 31 | 32 | 33 | ## Author / Contributor 34 | 35 | [Takuya Kiyokawa](https://takuya-ki.github.io/) 36 | 37 | ## License 38 | 39 | This software is released under the MIT License, see [LICENSE](./LICENSE). 40 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Takuya Kiyokawa 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 | -------------------------------------------------------------------------------- /src/demo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import time 4 | import argparse 5 | 6 | from onrobot import RG 7 | 8 | 9 | def run_demo(): 10 | """Runs gripper open-close demonstration once.""" 11 | rg = RG(gripper, toolchanger_ip, toolchanger_port) 12 | 13 | if not rg.get_status()[0]: # not busy 14 | print("Current hand opening width: " + 15 | str(rg.get_width_with_offset()) + 16 | " mm") 17 | 18 | rg.open_gripper() # fully opened 19 | while True: 20 | time.sleep(0.5) 21 | if not rg.get_status()[0]: 22 | break 23 | rg.close_gripper() # fully closed 24 | while True: 25 | time.sleep(0.5) 26 | if not rg.get_status()[0]: 27 | break 28 | rg.move_gripper(800) # move to middle point 29 | while True: 30 | time.sleep(0.5) 31 | if not rg.get_status()[0]: 32 | break 33 | 34 | rg.close_connection() 35 | 36 | 37 | def get_options(): 38 | """Returns user-specific options.""" 39 | parser = argparse.ArgumentParser(description='Set options.') 40 | parser.add_argument( 41 | '--gripper', dest='gripper', type=str, 42 | default="rg6", choices=['rg2', 'rg6'], 43 | help='set gripper type, rg2 or rg6') 44 | parser.add_argument( 45 | '--ip', dest='ip', type=str, default="192.168.1.1", 46 | help='set ip address') 47 | parser.add_argument( 48 | '--port', dest='port', type=str, default="502", 49 | help='set port number') 50 | return parser.parse_args() 51 | 52 | 53 | if __name__ == '__main__': 54 | args = get_options() 55 | gripper = args.gripper 56 | toolchanger_ip = args.ip 57 | toolchanger_port = args.port 58 | run_demo() 59 | -------------------------------------------------------------------------------- /src/onrobot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | from pymodbus.client.sync import ModbusTcpClient as ModbusClient 4 | 5 | 6 | class RG(): 7 | 8 | def __init__(self, gripper, ip, port): 9 | self.client = ModbusClient( 10 | ip, 11 | port=port, 12 | stopbits=1, 13 | bytesize=8, 14 | parity='E', 15 | baudrate=115200, 16 | timeout=1) 17 | if gripper not in ['rg2', 'rg6']: 18 | print("Please specify either rg2 or rg6.") 19 | return 20 | self.gripper = gripper # RG2/6 21 | if self.gripper == 'rg2': 22 | self.max_width = 1100 23 | self.max_force = 400 24 | elif self.gripper == 'rg6': 25 | self.max_width = 1600 26 | self.max_force = 1200 27 | self.open_connection() 28 | 29 | def open_connection(self): 30 | """Opens the connection with a gripper.""" 31 | self.client.connect() 32 | 33 | def close_connection(self): 34 | """Closes the connection with the gripper.""" 35 | self.client.close() 36 | 37 | def get_fingertip_offset(self): 38 | """Reads the current fingertip offset in 1/10 millimeters. 39 | Please note that the value is a signed two's complement number. 40 | """ 41 | result = self.client.read_holding_registers( 42 | address=258, count=1, unit=65) 43 | offset_mm = result.registers[0] / 10.0 44 | return offset_mm 45 | 46 | def get_width(self): 47 | """Reads current width between gripper fingers in 1/10 millimeters. 48 | Please note that the width is provided without any fingertip offset, 49 | as it is measured between the insides of the aluminum fingers. 50 | """ 51 | result = self.client.read_holding_registers( 52 | address=267, count=1, unit=65) 53 | width_mm = result.registers[0] / 10.0 54 | return width_mm 55 | 56 | def get_status(self): 57 | """Reads current device status. 58 | This status field indicates the status of the gripper and its motion. 59 | It is composed of 7 flags, described in the table below. 60 | 61 | Bit Name Description 62 | 0 (LSB): busy High (1) when a motion is ongoing, 63 | low (0) when not. 64 | The gripper will only accept new commands 65 | when this flag is low. 66 | 1: grip detected High (1) when an internal- or 67 | external grip is detected. 68 | 2: S1 pushed High (1) when safety switch 1 is pushed. 69 | 3: S1 trigged High (1) when safety circuit 1 is activated. 70 | The gripper will not move 71 | while this flag is high; 72 | can only be reset by power cycling. 73 | 4: S2 pushed High (1) when safety switch 2 is pushed. 74 | 5: S2 trigged High (1) when safety circuit 2 is activated. 75 | The gripper will not move 76 | while this flag is high; 77 | can only be reset by power cycling. 78 | 6: safety error High (1) when on power on any of 79 | the safety switch is pushed. 80 | 10-16: reserved Not used. 81 | """ 82 | # address : register number 83 | # count : number of registers to be read 84 | # unit : slave device address 85 | result = self.client.read_holding_registers( 86 | address=268, count=1, unit=65) 87 | status = format(result.registers[0], '016b') 88 | status_list = [0] * 7 89 | if int(status[-1]): 90 | print("A motion is ongoing so new commands are not accepted.") 91 | status_list[0] = 1 92 | if int(status[-2]): 93 | print("An internal- or external grip is detected.") 94 | status_list[1] = 1 95 | if int(status[-3]): 96 | print("Safety switch 1 is pushed.") 97 | status_list[2] = 1 98 | if int(status[-4]): 99 | print("Safety circuit 1 is activated so it will not move.") 100 | status_list[3] = 1 101 | if int(status[-5]): 102 | print("Safety switch 2 is pushed.") 103 | status_list[4] = 1 104 | if int(status[-6]): 105 | print("Safety circuit 2 is activated so it will not move.") 106 | status_list[5] = 1 107 | if int(status[-7]): 108 | print("Any of the safety switch is pushed.") 109 | status_list[6] = 1 110 | 111 | return status_list 112 | 113 | def get_width_with_offset(self): 114 | """Reads current width between gripper fingers in 1/10 millimeters. 115 | The set fingertip offset is considered. 116 | """ 117 | result = self.client.read_holding_registers( 118 | address=275, count=1, unit=65) 119 | width_mm = result.registers[0] / 10.0 120 | return width_mm 121 | 122 | def set_control_mode(self, command): 123 | """The control field is used to start and stop gripper motion. 124 | Only one option should be set at a time. 125 | Please note that the gripper will not start a new motion 126 | before the one currently being executed is done 127 | (see busy flag in the Status field). 128 | The valid flags are: 129 | 130 | 1 (0x0001): grip 131 | Start the motion, with the target force and width. 132 | Width is calculated without the fingertip offset. 133 | Please note that the gripper will ignore this command 134 | if the busy flag is set in the status field. 135 | 8 (0x0008): stop 136 | Stop the current motion. 137 | 16 (0x0010): grip_w_offset 138 | Same as grip, but width is calculated 139 | with the set fingertip offset. 140 | """ 141 | result = self.client.write_register( 142 | address=2, value=command, unit=65) 143 | 144 | def set_target_force(self, force_val): 145 | """Writes the target force to be reached 146 | when gripping and holding a workpiece. 147 | It must be provided in 1/10th Newtons. 148 | The valid range is 0 to 400 for the RG2 and 0 to 1200 for the RG6. 149 | """ 150 | result = self.client.write_register( 151 | address=0, value=force_val, unit=65) 152 | 153 | def set_target_width(self, width_val): 154 | """Writes the target width between 155 | the finger to be moved to and maintained. 156 | It must be provided in 1/10th millimeters. 157 | The valid range is 0 to 1100 for the RG2 and 0 to 1600 for the RG6. 158 | Please note that the target width should be provided 159 | corrected for any fingertip offset, 160 | as it is measured between the insides of the aluminum fingers. 161 | """ 162 | result = self.client.write_register( 163 | address=1, value=width_val, unit=65) 164 | 165 | def close_gripper(self, force_val=400): 166 | """Closes gripper.""" 167 | params = [force_val, 0, 16] 168 | print("Start closing gripper.") 169 | result = self.client.write_registers( 170 | address=0, values=params, unit=65) 171 | 172 | def open_gripper(self, force_val=400): 173 | """Opens gripper.""" 174 | params = [force_val, self.max_width, 16] 175 | print("Start opening gripper.") 176 | result = self.client.write_registers( 177 | address=0, values=params, unit=65) 178 | 179 | def move_gripper(self, width_val, force_val=400): 180 | """Moves gripper to the specified width.""" 181 | params = [force_val, width_val, 16] 182 | print("Start moving gripper.") 183 | result = self.client.write_registers( 184 | address=0, values=params, unit=65) 185 | 186 | --------------------------------------------------------------------------------