Created
April 26, 2025 02:59
-
-
Save ksvbka/413f09cff498606a3966b25d80dea9d3 to your computer and use it in GitHub Desktop.
Control inspire_hand using python
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| import serial | |
| import struct | |
| # import numpy | |
| # import string | |
| # import binascii | |
| import time | |
| class InspireHandR: | |
| def __init__(self): | |
| # Serial port settings | |
| self.ser = serial.Serial('/dev/ttyUSB0', 115200) | |
| self.ser.isOpen() | |
| self.hand_id = 1 | |
| power1 = 400 | |
| power2 = 400 | |
| power3 = 400 | |
| power4 = 400 | |
| power5 = 400 | |
| power6 = 800 | |
| self.setpower(power1, power2, power3, power4, power5, power6) | |
| speed1 = 1000 | |
| speed2 = 1000 | |
| speed3 = 1000 | |
| speed4 = 1000 | |
| speed5 = 1000 | |
| speed6 = 1000 | |
| self.setspeed(speed1, speed2, speed3, speed4, speed5, speed6) | |
| self.f1_init_pos = 400 # Initial position of pinky | |
| self.f2_init_pos = 400 # Initial position of ring finger | |
| self.f3_init_pos = 400 # Initial position of middle finger | |
| self.f4_init_pos = 400 # Initial position of index finger | |
| self.f5_init_pos = 1200 # Initial position of thumb | |
| self.f6_init_pos = 700 # Initial position of thumb turning to palm | |
| # Open your hand for testing | |
| # self.f1_init_pos = 0 # Initial position of pinky | |
| # self.f2_init_pos = 0 # Initial position of ring finger | |
| # self.f3_init_pos = 0 # Initial position of middle finger | |
| # self.f4_init_pos = 0 # Initial position of index finger | |
| # self.f5_init_pos = 0 # Initial position of thumb | |
| # self.f6_init_pos = 0 # Initial position of thumb turning to palm | |
| self.reset() | |
| # Divide the data into high byte and low byte | |
| def data2bytes(self, data): | |
| rdata = [0xff]*2 | |
| if data == -1: | |
| rdata[0] = 0xff | |
| rdata[1] = 0xff | |
| else: | |
| rdata[0] = data & 0xff | |
| rdata[1] = (data >> 8) & (0xff) | |
| return rdata | |
| # Convert hexadecimal or decimal numbers to bytes | |
| def num2str(self, num): | |
| str = hex(num) | |
| str = str[2:4] | |
| if (len(str) == 1): | |
| str = '0' + str | |
| str = bytes.fromhex(str) | |
| # print(str) | |
| return str | |
| # Checksum | |
| def checknum(self, data, leng): | |
| result = 0 | |
| for i in range(2, leng): | |
| result += data[i] | |
| result = result & 0xff | |
| # print(result) | |
| return result | |
| def setpos(self, pos1, pos2, pos3, pos4, pos5, pos6): | |
| global hand_id | |
| if pos1 < -1 or pos1 > 2000: | |
| print('Data out of correct range:-1-2000') | |
| return | |
| if pos2 < -1 or pos2 > 2000: | |
| print('Data out of correct range:-1-2000') | |
| return | |
| if pos3 < -1 or pos3 > 2000: | |
| print('Data out of correct range:-1-2000') | |
| return | |
| if pos4 < -1 or pos4 > 2000: | |
| print('Data out of correct range:-1-2000') | |
| return | |
| if pos5 < -1 or pos5 > 2000: | |
| print('Data out of correct range:-1-2000') | |
| return | |
| if pos6 < -1 or pos6 > 2000: | |
| print('Data out of correct range:-1-2000') | |
| return | |
| datanum = 0x0F | |
| b = [0]*(datanum + 5) | |
| # Baotou | |
| b[0] = 0xEB | |
| b[1] = 0x90 | |
| # hand_id | |
| b[2] = self.hand_id | |
| # Number of data | |
| b[3] = datanum | |
| # Write Operation | |
| b[4] = 0x12 | |
| # address | |
| b[5] = 0xC2 | |
| b[6] = 0x05 | |
| # data | |
| b[7] = self.data2bytes(pos1)[0] | |
| b[8] = self.data2bytes(pos1)[1] | |
| b[9] = self.data2bytes(pos2)[0] | |
| b[10] = self.data2bytes(pos2)[1] | |
| b[11] = self.data2bytes(pos3)[0] | |
| b[12] = self.data2bytes(pos3)[1] | |
| b[13] = self.data2bytes(pos4)[0] | |
| b[14] = self.data2bytes(pos4)[1] | |
| b[15] = self.data2bytes(pos5)[0] | |
| b[16] = self.data2bytes(pos5)[1] | |
| b[17] = self.data2bytes(pos6)[0] | |
| b[18] = self.data2bytes(pos6)[1] | |
| # Checksum | |
| b[19] = self.checknum(b, datanum+4) | |
| # Send data to the serial port | |
| putdata = b'' | |
| for i in range(1, datanum+6): | |
| putdata = putdata + self.num2str(b[i-1]) | |
| self.ser.write(putdata) | |
| # print('Data sent:',putdata) | |
| # print('Data sent:') | |
| # for i in range(1,datanum+6): | |
| # print(hex(putdata[i-1])) | |
| getdata = self.ser.read(9) | |
| # print('Returned data:',getdata) | |
| # print('Returned data:') | |
| # for i in range(1,10): | |
| # print(hex(getdata[i-1])) | |
| return | |
| # Setting Angle | |
| # angle1 = 0 # pinky straighten 1000, bend 0 | |
| # angle2 = 0 # ring finger straighten 1000, bend 0 | |
| # angle3 = 0 # middle finger straighten 1000, bend 0 | |
| # angle4 = 0 # index finger straighten 1000, bend 0 | |
| # angle5 = 1000 # thumb straighten 1000, bend 0 | |
| # angle6 = 1000 # thumb turns to the palm 0 | |
| # setangle(angle1,angle2,angle3,angle4,angle5,angle6) | |
| def setangle(self, angle1, angle2, angle3, angle4, angle5, angle6): | |
| if angle1 < -1 or angle1 > 1000: | |
| print('Data out of correct range: -1-1000') | |
| return | |
| if angle2 < -1 or angle2 > 1000: | |
| print('Data out of correct range: -1-1000') | |
| return | |
| if angle3 < -1 or angle3 > 1000: | |
| print('Data out of correct range: -1-1000') | |
| return | |
| if angle4 < -1 or angle4 > 1000: | |
| print('Data out of correct range: -1-1000') | |
| return | |
| if angle5 < -1 or angle5 > 1000: | |
| print('Data out of correct range: -1-1000') | |
| return | |
| if angle6 < -1 or angle6 > 1000: | |
| print('Data out of correct range: -1-1000') | |
| return | |
| datanum = 0x0F | |
| b = [0]*(datanum + 5) | |
| # Baotou | |
| b[0] = 0xEB | |
| b[1] = 0x90 | |
| # hand_id | |
| b[2] = self.hand_id | |
| # Number of data | |
| b[3] = datanum | |
| # Write Operation | |
| b[4] = 0x12 | |
| # address | |
| b[5] = 0xCE | |
| b[6] = 0x05 | |
| # data | |
| b[7] = self.data2bytes(angle1)[0] | |
| b[8] = self.data2bytes(angle1)[1] | |
| b[9] = self.data2bytes(angle2)[0] | |
| b[10] = self.data2bytes(angle2)[1] | |
| b[11] = self.data2bytes(angle3)[0] | |
| b[12] = self.data2bytes(angle3)[1] | |
| b[13] = self.data2bytes(angle4)[0] | |
| b[14] = self.data2bytes(angle4)[1] | |
| b[15] = self.data2bytes(angle5)[0] | |
| b[16] = self.data2bytes(angle5)[1] | |
| b[17] = self.data2bytes(angle6)[0] | |
| b[18] = self.data2bytes(angle6)[1] | |
| # Checksum | |
| b[19] = self.checknum(b, datanum+4) | |
| # Send data to the serial port | |
| putdata = b'' | |
| for i in range(1, datanum+6): | |
| putdata = putdata + self.num2str(b[i-1]) | |
| self.ser.write(putdata) | |
| print('Data sent:') | |
| for i in range(1, datanum+6): | |
| print(hex(putdata[i-1])) | |
| getdata = self.ser.read(9) | |
| print('Returned data:') | |
| for i in range(1, 10): | |
| print(hex(getdata[i-1])) | |
| # Setting the force control threshold | |
| def setpower(self, power1, power2, power3, power4, power5, power6): | |
| if power1 < 0 or power1 > 1000: | |
| print('Data out of range: 0-1000') | |
| return | |
| if power2 < 0 or power2 > 1000: | |
| print('Data out of range: 0-1000') | |
| return | |
| if power3 < 0 or power3 > 1000: | |
| print('Data out of range: 0-1000') | |
| return | |
| if power4 < 0 or power4 > 1000: | |
| print('Data out of range: 0-1000') | |
| return | |
| if power5 < 0 or power5 > 1000: | |
| print('Data out of range: 0-1000') | |
| return | |
| if power6 < 0 or power6 > 1000: | |
| print('Data out of range: 0-1000') | |
| return | |
| datanum = 0x0F | |
| b = [0]*(datanum + 5) | |
| # Baotou | |
| b[0] = 0xEB | |
| b[1] = 0x90 | |
| # hand_id | |
| b[2] = self.hand_id | |
| # Number of data | |
| b[3] = datanum | |
| # Write Operation | |
| b[4] = 0x12 | |
| # address | |
| b[5] = 0xDA | |
| b[6] = 0x05 | |
| # data | |
| b[7] = self.data2bytes(power1)[0] | |
| b[8] = self.data2bytes(power1)[1] | |
| b[9] = self.data2bytes(power2)[0] | |
| b[10] = self.data2bytes(power2)[1] | |
| b[11] = self.data2bytes(power3)[0] | |
| b[12] = self.data2bytes(power3)[1] | |
| b[13] = self.data2bytes(power4)[0] | |
| b[14] = self.data2bytes(power4)[1] | |
| b[15] = self.data2bytes(power5)[0] | |
| b[16] = self.data2bytes(power5)[1] | |
| b[17] = self.data2bytes(power6)[0] | |
| b[18] = self.data2bytes(power6)[1] | |
| # Checksum | |
| b[19] = self.checknum(b, datanum+4) | |
| # Send data to the serial port | |
| putdata = b'' | |
| for i in range(1, datanum+6): | |
| putdata = putdata + self.num2str(b[i-1]) | |
| self.ser.write(putdata) | |
| print('Data sent:') | |
| for i in range(1, datanum+6): | |
| print(hex(putdata[i-1])) | |
| getdata = self.ser.read(9) | |
| print('Returned data:') | |
| for i in range(1, 10): | |
| print(hex(getdata[i-1])) | |
| # Setting the speed | |
| def setspeed(self, speed1, speed2, speed3, speed4, speed5, speed6): | |
| if speed1 < 0 or speed1 > 1000: | |
| print('Data out of range: 0-1000') | |
| return | |
| if speed2 < 0 or speed2 > 1000: | |
| print('Data out of range: 0-1000') | |
| return | |
| if speed3 < 0 or speed3 > 1000: | |
| print('Data out of range: 0-1000') | |
| return | |
| if speed4 < 0 or speed4 > 1000: | |
| print('Data out of range: 0-1000') | |
| return | |
| if speed5 < 0 or speed5 > 1000: | |
| print('Data out of range: 0-1000') | |
| return | |
| if speed6 < 0 or speed6 > 1000: | |
| print('Data out of range: 0-1000') | |
| return | |
| datanum = 0x0F | |
| b = [0]*(datanum + 5) | |
| # Baotou | |
| b[0] = 0xEB | |
| b[1] = 0x90 | |
| # hand_id | |
| b[2] = self.hand_id | |
| # Number of data | |
| b[3] = datanum | |
| # Write Operation | |
| b[4] = 0x12 | |
| # address | |
| b[5] = 0xF2 | |
| b[6] = 0x05 | |
| # data | |
| b[7] = self.data2bytes(speed1)[0] | |
| b[8] = self.data2bytes(speed1)[1] | |
| b[9] = self.data2bytes(speed2)[0] | |
| b[10] = self.data2bytes(speed2)[1] | |
| b[11] = self.data2bytes(speed3)[0] | |
| b[12] = self.data2bytes(speed3)[1] | |
| b[13] = self.data2bytes(speed4)[0] | |
| b[14] = self.data2bytes(speed4)[1] | |
| b[15] = self.data2bytes(speed5)[0] | |
| b[16] = self.data2bytes(speed5)[1] | |
| b[17] = self.data2bytes(speed6)[0] | |
| b[18] = self.data2bytes(speed6)[1] | |
| # Checksum | |
| b[19] = self.checknum(b, datanum+4) | |
| # Send data to the serial port | |
| putdata = b'' | |
| for i in range(1, datanum+6): | |
| putdata = putdata + self.num2str(b[i-1]) | |
| self.ser.write(putdata) | |
| print('Data sent:') | |
| for i in range(1, datanum+6): | |
| print(hex(putdata[i-1])) | |
| getdata = self.ser.read(9) | |
| print('Returned data:') | |
| for i in range(1, 10): | |
| print(hex(getdata[i-1])) | |
| # Read the actual position value of the drive | |
| def get_setpos(self): | |
| datanum = 0x04 | |
| b = [0]*(datanum + 5) | |
| # Baotou | |
| b[0] = 0xEB | |
| b[1] = 0x90 | |
| # hand_id | |
| b[2] = self.hand_id | |
| # Number of data | |
| b[3] = datanum | |
| # Read Operation | |
| b[4] = 0x11 | |
| # address | |
| b[5] = 0xC2 | |
| b[6] = 0x05 | |
| # Read the length of the register | |
| b[7] = 0x0C | |
| # Checksum | |
| b[8] = self.checknum(b, datanum+4) | |
| # Send data to the serial port | |
| putdata = b'' | |
| for i in range(1, datanum+6): | |
| putdata = putdata + self.num2str(b[i-1]) | |
| self.ser.write(putdata) | |
| # print('Data sent:',putdata) | |
| print('Data sent:') | |
| for i in range(1, datanum+6): | |
| print(hex(putdata[i-1])) | |
| getdata = self.ser.read(20) | |
| print('Returned data:') | |
| for i in range(1, 21): | |
| print(hex(getdata[i-1])) | |
| setpos = [0]*6 | |
| for i in range(1, 7): | |
| if getdata[i*2+5] == 0xff and getdata[i*2+6] == 0xff: | |
| setpos[i-1] = -1 | |
| else: | |
| setpos[i-1] = getdata[i*2+5] + (getdata[i*2+6] << 8) | |
| return setpos | |
| # Reading setting angle | |
| def get_setangle(self): | |
| datanum = 0x04 | |
| b = [0]*(datanum + 5) | |
| # Baotou | |
| b[0] = 0xEB | |
| b[1] = 0x90 | |
| # hand_id | |
| b[2] = self.hand_id | |
| # Number of data | |
| b[3] = datanum | |
| # Read Operation | |
| b[4] = 0x11 | |
| # address | |
| b[5] = 0xCE | |
| b[6] = 0x05 | |
| # Read the length of the register | |
| b[7] = 0x0C | |
| # Checksum | |
| b[8] = self.checknum(b, datanum+4) | |
| # Send data to the serial port | |
| putdata = b'' | |
| for i in range(1, datanum+6): | |
| putdata = putdata + self.num2str(b[i-1]) | |
| self.ser.write(putdata) | |
| print('Data sent:') | |
| for i in range(1, datanum+6): | |
| print(hex(putdata[i-1])) | |
| getdata = self.ser.read(20) | |
| print('Returned data:') | |
| for i in range(1, 21): | |
| print(hex(getdata[i-1])) | |
| setangle = [0]*6 | |
| for i in range(1, 7): | |
| if getdata[i*2+5] == 0xff and getdata[i*2+6] == 0xff: | |
| setangle[i-1] = -1 | |
| else: | |
| setangle[i-1] = getdata[i*2+5] + (getdata[i*2+6] << 8) | |
| return setangle | |
| # Read the force control threshold set by the drive | |
| def get_setpower(self): | |
| datanum = 0x04 | |
| b = [0]*(datanum + 5) | |
| # Baotou | |
| b[0] = 0xEB | |
| b[1] = 0x90 | |
| # hand_id | |
| b[2] = self.hand_id | |
| # Number of data | |
| b[3] = datanum | |
| # Read Operation | |
| b[4] = 0x11 | |
| # address | |
| b[5] = 0xDA | |
| b[6] = 0x05 | |
| # Read the length of the register | |
| b[7] = 0x0C | |
| # Checksum | |
| b[8] = self.checknum(b, datanum+4) | |
| # Send data to the serial port | |
| putdata = b'' | |
| for i in range(1, datanum+6): | |
| putdata = putdata + self.num2str(b[i-1]) | |
| self.ser.write(putdata) | |
| print('Data sent:') | |
| for i in range(1, datanum+6): | |
| print(hex(putdata[i-1])) | |
| getdata = self.ser.read(20) | |
| print('Returned data:') | |
| for i in range(1, 21): | |
| print(hex(getdata[i-1])) | |
| setpower = [0]*6 | |
| for i in range(1, 7): | |
| if getdata[i*2+5] == 0xff and getdata[i*2+6] == 0xff: | |
| setpower[i-1] = -1 | |
| else: | |
| setpower[i-1] = getdata[i*2+5] + (getdata[i*2+6] << 8) | |
| return setpower | |
| # Read the actual position value of the drive | |
| def get_actpos(self): | |
| datanum = 0x04 | |
| b = [0]*(datanum + 5) | |
| # Baotou | |
| b[0] = 0xEB | |
| b[1] = 0x90 | |
| # hand_id | |
| b[2] = self.hand_id | |
| # Number of data | |
| b[3] = datanum | |
| # Read Operation | |
| b[4] = 0x11 | |
| # address | |
| b[5] = 0xFE | |
| b[6] = 0x05 | |
| # Read the length of the register | |
| b[7] = 0x0C | |
| # Checksum | |
| b[8] = self.checknum(b, datanum+4) | |
| # Send data to the serial port | |
| putdata = b'' | |
| for i in range(1, datanum+6): | |
| putdata = putdata + self.num2str(b[i-1]) | |
| self.ser.write(putdata) | |
| print('Data sent:') | |
| for i in range(1, datanum+6): | |
| print(hex(putdata[i-1])) | |
| getdata = self.ser.read(20) | |
| print('Returned data:') | |
| for i in range(1, 21): | |
| print(hex(getdata[i-1])) | |
| actpos = [0]*6 | |
| for i in range(1, 7): | |
| if getdata[i*2+5] == 0xff and getdata[i*2+6] == 0xff: | |
| actpos[i-1] = -1 | |
| else: | |
| actpos[i-1] = getdata[i*2+5] + (getdata[i*2+6] << 8) | |
| return actpos | |
| # Read the actual angle value | |
| def get_actangle(self): | |
| datanum = 0x04 | |
| b = [0]*(datanum + 5) | |
| # Baotou | |
| b[0] = 0xEB | |
| b[1] = 0x90 | |
| # hand_id | |
| b[2] = self.hand_id | |
| # Number of data | |
| b[3] = datanum | |
| # Read Operation | |
| b[4] = 0x11 | |
| # address | |
| b[5] = 0x0A | |
| b[6] = 0x06 | |
| # Read the length of the register | |
| b[7] = 0x0C | |
| # Checksum | |
| b[8] = self.checknum(b, datanum+4) | |
| # Send data to the serial port | |
| putdata = b'' | |
| for i in range(1, datanum+6): | |
| putdata = putdata + self.num2str(b[i-1]) | |
| self.ser.write(putdata) | |
| # print('Data sent:') | |
| # for i in range(1,datanum+6): | |
| # print(hex(putdata[i-1])) | |
| getdata = self.ser.read(20) | |
| # print('Returned data:') | |
| # for i in range(1,21): | |
| # print(hex(getdata[i-1])) | |
| actangle = [0]*6 | |
| for i in range(1, 7): | |
| if getdata[i*2+5] == 0xff and getdata[i*2+6] == 0xff: | |
| actangle[i-1] = -1 | |
| else: | |
| actangle[i-1] = getdata[i*2+5] + (getdata[i*2+6] << 8) | |
| return actangle | |
| # Read the actual force | |
| def get_actforce(self): | |
| datanum = 0x04 | |
| b = [0]*(datanum + 5) | |
| # Baotou | |
| b[0] = 0xEB | |
| b[1] = 0x90 | |
| # hand_id | |
| b[2] = self.hand_id | |
| # Number of data | |
| b[3] = datanum | |
| # Read Operation | |
| b[4] = 0x11 | |
| # address | |
| b[5] = 0x2E | |
| b[6] = 0x06 | |
| # Read the length of the register | |
| b[7] = 0x0C | |
| # Checksum | |
| b[8] = self.checknum(b, datanum+4) | |
| # Send data to the serial port | |
| putdata = b'' | |
| for i in range(1, datanum+6): | |
| putdata = putdata + self.num2str(b[i-1]) | |
| self.ser.write(putdata) | |
| # print('Data sent:') | |
| # for i in range(1,datanum+6): | |
| # print(hex(putdata[i-1])) | |
| getdata = self.ser.read(20) | |
| # print('Returned data:') | |
| # for i in range(1,21): | |
| # print(hex(getdata[i-1])) | |
| actforce = [0]*6 | |
| for i in range(1, 7): | |
| if getdata[i*2+5] == 0xff and getdata[i*2+6] == 0xff: | |
| actforce[i-1] = -1 | |
| else: | |
| actforce[i-1] = getdata[i*2+5] + (getdata[i*2+6] << 8) | |
| # The serial port receives an unsigned hexadecimal number consisting of two bytes, and the decimal representation range is 0 to 65536, while the actual data is signed data, indicating different directions of force, ranging from -32768 to 32767. | |
| # Therefore, it is necessary to process the received data to obtain the actual force sensor data: when the reading is greater than 32767, subtract 65536 from the number. | |
| for i in range(len(actforce)): | |
| if actforce[i] > 32767: | |
| actforce[i] = actforce[i] - 65536 | |
| return actforce | |
| # Reading current | |
| def get_current(self): | |
| datanum = 0x04 | |
| b = [0]*(datanum + 5) | |
| # Baotou | |
| b[0] = 0xEB | |
| b[1] = 0x90 | |
| # hand_id | |
| b[2] = self.hand_id | |
| # Number of data | |
| b[3] = datanum | |
| # Read Operation | |
| b[4] = 0x11 | |
| # address | |
| b[5] = 0x3A | |
| b[6] = 0x06 | |
| # Read the length of the register | |
| b[7] = 0x0C | |
| # Checksum | |
| b[8] = self.checknum(b, datanum+4) | |
| # Send data to the serial port | |
| putdata = b'' | |
| for i in range(1, datanum+6): | |
| putdata = putdata + self.num2str(b[i-1]) | |
| self.ser.write(putdata) | |
| print('Data sent:') | |
| for i in range(1, datanum+6): | |
| print(hex(putdata[i-1])) | |
| getdata = self.ser.read(20) | |
| print('Returned data:') | |
| for i in range(1, 21): | |
| print(hex(getdata[i-1])) | |
| current = [0]*6 | |
| for i in range(1, 7): | |
| if getdata[i*2+5] == 0xff and getdata[i*2+6] == 0xff: | |
| current[i-1] = -1 | |
| else: | |
| current[i-1] = getdata[i*2+5] + (getdata[i*2+6] << 8) | |
| return current | |
| # Read fault information | |
| def get_error(self): | |
| datanum = 0x04 | |
| b = [0]*(datanum + 5) | |
| # Baotou | |
| b[0] = 0xEB | |
| b[1] = 0x90 | |
| # hand_id | |
| b[2] = self.hand_id | |
| # Number of data | |
| b[3] = datanum | |
| # Read Operation | |
| b[4] = 0x11 | |
| # address | |
| b[5] = 0x46 | |
| b[6] = 0x06 | |
| # Read the length of the register | |
| b[7] = 0x06 | |
| # Checksum | |
| b[8] = self.checknum(b, datanum+4) | |
| # Send data to the serial port | |
| putdata = b'' | |
| for i in range(1, datanum+6): | |
| putdata = putdata + self.num2str(b[i-1]) | |
| self.ser.write(putdata) | |
| print('Data sent:') | |
| for i in range(1, datanum+6): | |
| print(hex(putdata[i-1])) | |
| getdata = self.ser.read(14) | |
| print('Returned data:') | |
| for i in range(1, 15): | |
| print(hex(getdata[i-1])) | |
| error = [0]*6 | |
| for i in range(1, 7): | |
| error[i-1] = getdata[i+6] | |
| return error | |
| # Reading status information | |
| def get_status(self): | |
| datanum = 0x04 | |
| b = [0]*(datanum + 5) | |
| # Baotou | |
| b[0] = 0xEB | |
| b[1] = 0x90 | |
| # hand_id | |
| b[2] = self.hand_id | |
| # Number of data | |
| b[3] = datanum | |
| # Read Operation | |
| b[4] = 0x11 | |
| # address | |
| b[5] = 0x4C | |
| b[6] = 0x06 | |
| # Read the length of the register | |
| b[7] = 0x06 | |
| # Checksum | |
| b[8] = self.checknum(b, datanum+4) | |
| # Send data to the serial port | |
| putdata = b'' | |
| for i in range(1, datanum+6): | |
| putdata = putdata + self.num2str(b[i-1]) | |
| self.ser.write(putdata) | |
| print('Data sent:') | |
| for i in range(1, datanum+6): | |
| print(hex(putdata[i-1])) | |
| getdata = self.ser.read(14) | |
| print('Returned data:') | |
| for i in range(1, 15): | |
| print(hex(getdata[i-1])) | |
| status = [0]*6 | |
| for i in range(1, 7): | |
| status[i-1] = getdata[i+6] | |
| return status | |
| # Read temperature information | |
| def get_temp(self): | |
| datanum = 0x04 | |
| b = [0]*(datanum + 5) | |
| # Baotou | |
| b[0] = 0xEB | |
| b[1] = 0x90 | |
| # hand_id | |
| b[2] = self.hand_id | |
| # Number of data | |
| b[3] = datanum | |
| # Read Operation | |
| b[4] = 0x11 | |
| # address | |
| b[5] = 0x52 | |
| b[6] = 0x06 | |
| # Read the length of the register | |
| b[7] = 0x06 | |
| # Checksum | |
| b[8] = self.checknum(b, datanum+4) | |
| # Send data to the serial port | |
| putdata = b'' | |
| for i in range(1, datanum+6): | |
| putdata = putdata + self.num2str(b[i-1]) | |
| self.ser.write(putdata) | |
| print('Data sent:') | |
| for i in range(1, datanum+6): | |
| print(hex(putdata[i-1])) | |
| getdata = self.ser.read(14) | |
| print('Returned data:') | |
| for i in range(1, 15): | |
| print(hex(getdata[i-1])) | |
| temp = [0]*6 | |
| for i in range(1, 7): | |
| temp[i-1] = getdata[i+6] | |
| return temp | |
| # Clear Errors | |
| def set_clear_error(self): | |
| datanum = 0x04 | |
| b = [0]*(datanum + 5) | |
| # Baotou | |
| b[0] = 0xEB | |
| b[1] = 0x90 | |
| # hand_id | |
| b[2] = self.hand_id | |
| # Number of data | |
| b[3] = datanum | |
| # Write Operation | |
| b[4] = 0x12 | |
| # address | |
| b[5] = 0xEC | |
| b[6] = 0x03 | |
| # data | |
| b[7] = 0x01 | |
| # Checksum | |
| b[8] = self.checknum(b, datanum+4) | |
| # Send data to the serial port | |
| putdata = b'' | |
| for i in range(1, datanum+6): | |
| putdata = putdata + self.num2str(b[i-1]) | |
| self.ser.write(putdata) | |
| print('Data sent:') | |
| for i in range(1, datanum+6): | |
| print(hex(putdata[i-1])) | |
| getdata = self.ser.read(9) | |
| print('Returned data:') | |
| for i in range(1, 10): | |
| print(hex(getdata[i-1])) | |
| # Save parameters to FLASHH | |
| def set_save_flash(self): | |
| datanum = 0x04 | |
| b = [0]*(datanum + 5) | |
| # Baotou | |
| b[0] = 0xEB | |
| b[1] = 0x90 | |
| # hand_id | |
| b[2] = self.hand_id | |
| # Number of data | |
| b[3] = datanum | |
| # Write Operation | |
| b[4] = 0x12 | |
| # address | |
| b[5] = 0xED | |
| b[6] = 0x03 | |
| # data | |
| b[7] = 0x01 | |
| # Checksum | |
| b[8] = self.checknum(b, datanum+4) | |
| # Send data to the serial port | |
| putdata = b'' | |
| for i in range(1, datanum+6): | |
| putdata = putdata + self.num2str(b[i-1]) | |
| self.ser.write(putdata) | |
| print('Data sent:') | |
| for i in range(1, datanum+6): | |
| print(hex(putdata[i-1])) | |
| getdata = self.ser.read(18) | |
| print('Returned data:') | |
| for i in range(1, 19): | |
| print(hex(getdata[i-1])) | |
| # Force sensor calibration | |
| def gesture_force_clb(self): | |
| datanum = 0x04 | |
| b = [0]*(datanum + 5) | |
| # Baotou | |
| b[0] = 0xEB | |
| b[1] = 0x90 | |
| # hand_id | |
| b[2] = self.hand_id | |
| # Number of data | |
| b[3] = datanum | |
| # Write Operation | |
| b[4] = 0x12 | |
| # address | |
| b[5] = 0xF1 | |
| b[6] = 0x03 | |
| # data | |
| b[7] = 0x01 | |
| # Checksum | |
| b[8] = self.checknum(b, datanum+4) | |
| # Send data to the serial port | |
| putdata = b'' | |
| for i in range(1, datanum+6): | |
| putdata = putdata + self.num2str(b[i-1]) | |
| self.ser.write(putdata) | |
| print('Data sent:') | |
| for i in range(1, datanum+6): | |
| print(hex(putdata[i-1])) | |
| getdata = self.ser.read(18) | |
| print('Returned data:') | |
| for i in range(1, 19): | |
| print(hex(getdata[i-1])) | |
| # Set the power-on speed | |
| def setdefaultspeed(self, speed1, speed2, speed3, speed4, speed5, speed6): | |
| if speed1 < 0 or speed1 > 1000: | |
| print('Data out of range: 0-1000') | |
| return | |
| if speed2 < 0 or speed2 > 1000: | |
| return | |
| if speed3 < 0 or speed3 > 1000: | |
| return | |
| if speed4 < 0 or speed4 > 1000: | |
| return | |
| if speed5 < 0 or speed5 > 1000: | |
| return | |
| if speed6 < 0 or speed6 > 1000: | |
| return | |
| datanum = 0x0F | |
| b = [0]*(datanum + 5) | |
| # Baotou | |
| b[0] = 0xEB | |
| b[1] = 0x90 | |
| # hand_id | |
| b[2] = self.hand_id | |
| # Number of data | |
| b[3] = datanum | |
| # Write Operation | |
| b[4] = 0x12 | |
| # address | |
| b[5] = 0x08 | |
| b[6] = 0x04 | |
| # data | |
| b[7] = self.data2bytes(speed1)[0] | |
| b[8] = self.data2bytes(speed1)[1] | |
| b[9] = self.data2bytes(speed2)[0] | |
| b[10] = self.data2bytes(speed2)[1] | |
| b[11] = self.data2bytes(speed3)[0] | |
| b[12] = self.data2bytes(speed3)[1] | |
| b[13] = self.data2bytes(speed4)[0] | |
| b[14] = self.data2bytes(speed4)[1] | |
| b[15] = self.data2bytes(speed5)[0] | |
| b[16] = self.data2bytes(speed5)[1] | |
| b[17] = self.data2bytes(speed6)[0] | |
| b[18] = self.data2bytes(speed6)[1] | |
| # Checksum | |
| b[19] = self.checknum(b, datanum+4) | |
| # Send data to the serial port | |
| putdata = b'' | |
| for i in range(1, datanum+6): | |
| putdata = putdata + self.num2str(b[i-1]) | |
| self.ser.write(putdata) | |
| print('Data sent:') | |
| for i in range(1, datanum+6): | |
| print(hex(putdata[i-1])) | |
| getdata = self.ser.read(9) | |
| print('Returned data:') | |
| for i in range(1, 10): | |
| print(hex(getdata[i-1])) | |
| # Set the power-on control threshold | |
| def setdefaultpower(self, power1, power2, power3, power4, power5, power6): | |
| if power1 < 0 or power1 > 1000: | |
| print('Data out of range: 0-1000') | |
| return | |
| if power2 < 0 or power2 > 1000: | |
| return | |
| if power3 < 0 or power3 > 1000: | |
| return | |
| if power4 < 0 or power4 > 1000: | |
| return | |
| if power5 < 0 or power5 > 1000: | |
| return | |
| if power6 < 0 or power6 > 1000: | |
| return | |
| datanum = 0x0F | |
| b = [0]*(datanum + 5) | |
| # Baotou | |
| b[0] = 0xEB | |
| b[1] = 0x90 | |
| # hand_id | |
| b[2] = self.hand_id | |
| # Number of data | |
| b[3] = datanum | |
| # Write Operation | |
| b[4] = 0x12 | |
| # address | |
| b[5] = 0x14 | |
| b[6] = 0x04 | |
| # data | |
| b[7] = self.data2bytes(power1)[0] | |
| b[8] = self.data2bytes(power1)[1] | |
| b[9] = self.data2bytes(power2)[0] | |
| b[10] = self.data2bytes(power2)[1] | |
| b[11] = self.data2bytes(power3)[0] | |
| b[12] = self.data2bytes(power3)[1] | |
| b[13] = self.data2bytes(power4)[0] | |
| b[14] = self.data2bytes(power4)[1] | |
| b[15] = self.data2bytes(power5)[0] | |
| b[16] = self.data2bytes(power5)[1] | |
| b[17] = self.data2bytes(power6)[0] | |
| b[18] = self.data2bytes(power6)[1] | |
| # Checksum | |
| b[19] = self.checknum(b, datanum+4) | |
| # Send data to the serial port | |
| putdata = b'' | |
| for i in range(1, datanum+6): | |
| putdata = putdata + self.num2str(b[i-1]) | |
| self.ser.write(putdata) | |
| print('Data sent:') | |
| for i in range(1, datanum+6): | |
| print(hex(putdata[i-1])) | |
| getdata = self.ser.read(9) | |
| print('Returned data:') | |
| for i in range(1, 10): | |
| print(hex(getdata[i-1])) | |
| def soft_setpos(self, pos1, pos2, pos3, pos4, pos5, pos6): | |
| value0 = 0 | |
| temp_value = [0, 0, 0, 0, 0, 0] | |
| is_static = [0, 0, 0, 0, 0, 0] | |
| static_value = [0, 0, 0, 0, 0, 0] | |
| pos_value = [pos1, pos2, pos3, pos4, pos5, pos6] | |
| n = 5 | |
| diffpos = pos1 - self.f1_init_pos | |
| tic = time.time() | |
| for ii in range(5): | |
| # self.setpos(pos1,pos2,pos3,pos4,pos5,pos6) | |
| # print('==========================') | |
| actforce = self.get_actforce() | |
| print('actforce: ', actforce) | |
| for i, f in enumerate(actforce[0:5]): | |
| if is_static[i]: | |
| continue | |
| if f > 1000: | |
| continue | |
| if i == 5: # thumb | |
| if f > 100: # If the force on the finger is greater than 100, maintain the previous position | |
| is_static[i] = 1 # Marked as static finger, the finger remains in this position and does not move | |
| static_value[i] = temp_value[i] # The i-th finger position of the previous step | |
| else: | |
| if f > 50: # If the force on the finger is greater than 100, maintain the previous position | |
| is_static[i] = 1 # Marked as static finger, the finger remains in this position and does not move | |
| static_value[i] = temp_value[i] # The i-th finger position of the previous step | |
| temp_value = pos_value.copy() | |
| for i in range(6): | |
| if is_static[i]: | |
| pos_value[i] = static_value[i] | |
| pos1 = pos_value[0] # Little finger straightened 0, bent 2000 | |
| pos2 = pos_value[1] # Ring finger straightened 0, bent 2000 | |
| pos3 = pos_value[2] # Middle finger straight 0, bent 2000 | |
| pos4 = pos_value[3] # Index finger straight 0, bent 2000 | |
| pos5 = pos_value[4] # Thumb straight 0, bent 2000 | |
| pos6 = pos_value[5] # Thumb turned to palm 2000 | |
| self.setpos(pos1, pos2, pos3, pos4, pos5, pos6) | |
| toc = time.time() | |
| print('ii: %d,toc=%f' % (ii, toc - tic)) | |
| def reset(self): | |
| pos1 = self.f1_init_pos # Little finger straightened 0, bent 2000 | |
| pos2 = self.f2_init_pos # Ring finger straightened 0, bent 2000 | |
| pos3 = self.f3_init_pos # Middle finger straight 0, bent 2000 | |
| pos4 = self.f4_init_pos # Index finger straight 0, bent 2000 | |
| pos5 = self.f5_init_pos # Thumb straight 0, bent 2000 | |
| pos6 = self.f6_init_pos # Thumb turned to palm 2000 | |
| self.setpos(pos1, pos2, pos3, pos4, pos5, pos6) | |
| return | |
| def reset_0(self): | |
| pos1 = 0 # Little finger straightened 0, bent 2000 | |
| pos2 = 0 # Ring finger straightened 0, bent 2000 | |
| pos3 = 0 # Middle finger straight 0, bent 2000 | |
| pos4 = 0 # Index finger straight 0, bent 2000 | |
| pos5 = 0 # Thumb straight 0, bent 2000 | |
| pos6 = 0 # Thumb turned to palm 2000 | |
| self.setpos(pos1, pos2, pos3, pos4, pos5, pos6) | |
| return | |
| if __name__ == "__main__": | |
| hand = InspireHandR() | |
| power1 = 400 | |
| power2 = 400 | |
| power3 = 400 | |
| power4 = 400 | |
| power5 = 400 | |
| power6 = 800 | |
| hand.setpower(power1, power2, power3, power4, power5, power6) | |
| speed1 = 1000 | |
| speed2 = 1000 | |
| speed3 = 1000 | |
| speed4 = 1000 | |
| speed5 = 1000 | |
| speed6 = 500 | |
| hand.setspeed(speed1, speed2, speed3, speed4, speed5, speed6) | |
| temp_value = [0, 0, 0, 0, 0, 0] | |
| is_static = [0, 0, 0, 0, 0, 0] | |
| static_value = [0, 0, 0, 0, 0, 0] | |
| tforce = 100 | |
| while 1: | |
| value0 = 0 | |
| temp_value = [0, 0, 0, 0, 0, 0] | |
| is_static = [0, 0, 0, 0, 0, 0] | |
| static_value = [0, 0, 0, 0, 0, 0] | |
| for i in range(200): | |
| value = 10*i | |
| pos_value = [value]*6 # Theoretical position of the next step of 6 fingers | |
| actforce = hand.get_actforce() | |
| for i, f in enumerate(actforce[0:5]): | |
| if is_static[i]: | |
| continue | |
| if f > 1000: | |
| continue | |
| if i == 4: # | |
| if f > 100: # If the force on the finger is greater than 100, maintain the previous position | |
| is_static[i] = 1 # Marked as static finger, the finger remains in this position and does not move | |
| static_value[i] = temp_value[i] # The i-th finger position of the previous step | |
| else: | |
| if f > 100: # If the force on the finger is greater than 100, maintain the previous position | |
| is_static[i] = 1 # Marked as static finger, the finger remains in this position and does not move | |
| static_value[i] = temp_value[i] # The i-th finger position of the previous step | |
| pos_value[5] = pos_value[5] + 1000 | |
| if pos_value[4] > 2000: | |
| pos_value[4] = 2000 | |
| if pos_value[5] > 2000: | |
| pos_value[5] = 2000 | |
| temp_value = pos_value.copy() | |
| for i in range(6): | |
| if is_static[i]: | |
| pos_value[i] = static_value[i] | |
| pos1 = pos_value[0] # Little finger straightened 0, bent 2000 | |
| pos2 = pos_value[1] # Ring finger straightened 0, bent 2000 | |
| pos3 = pos_value[2] # Middle finger straight 0, bent 2000 | |
| pos4 = pos_value[3] # Index finger straight 0, bent 2000 | |
| pos5 = pos_value[4] # Thumb straight 0, bent 2000 | |
| pos6 = pos_value[5] # Thumb turned to palm 2000 | |
| hand.setpos(pos1, pos2, pos3, pos4, pos5, pos6) | |
| print('444444444444444444444444444') | |
| print('pos:', pos1, pos2, pos3, pos4, pos5, pos6) | |
| print("actforce:", actforce) | |
| print('5555555555555555') | |
| curr_pos = hand.get_actpos() | |
| print('currpos', curr_pos) | |
| time.sleep(0.005) | |
| time.sleep(2) | |
| hand.reset() | |
| time.sleep(2) |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment