cyberglove_with_calib_22dof.py
Go to the documentation of this file.
1#!/usr/bin/env python
2
3import serial
4import os
5import time
6import signal
7import sys
8import tty
9import termios
10
11import numpy as np
12
13import sys, traceback, Ice
14import IceStorm
15import time
16
17from thread import start_new_thread, allocate_lock
18
19os.system("clear")
20
21status = 0
22ic = None
23
24class bcolors:
25 OKBLUE = '\033[94m'
26 OKGREEN = '\033[92m'
27 WARNING = '\033[93m'
28 FAIL = '\033[91m'
29 ENDC = '\033[0m'
30
31print bcolors.OKBLUE+"""
32 ______ __ ________
33 / ____/_ __/ /_ ___ _____/ ____/ /___ _ _____
34 / / / / / / __ \/ _ \/ ___/ / __/ / __ \ | / / _ \\
35/ /___/ /_/ / /_/ / __/ / / /_/ / / /_/ / |/ / __/
36\____/\__, /_.___/\___/_/ \____/_/\____/|___/\___/
37 /____/
38""" + bcolors.ENDC
39
40print bcolors.WARNING + "Permission needed to open ports for gloves. If prompted, please type your sudo password below." + bcolors.ENDC
41#os.system("sudo chmod 777 /dev/ttyUSB0")
42#os.system("sudo chmod 777 /dev/ttyUSB1")
43
44print ""
45print ""
46print ""
47
48PI = 3.14159
49
50def signal_handler(signal, frame):
51 print '\nExiting...'
52 sys.exit(0)
53signal.signal(signal.SIGINT, signal_handler)
54
55right_port = '/dev/ttyUSB0'
56
57right_ser = serial.Serial(right_port, 115200, timeout=1)
58right_ser.write("1dw") #disable wifi
59right_ser.write("1eu") #enable usb
60right_ser.write("1ds") #disable sd card
61right_ser.write("1S") #enable data streaming
62
63right_s = ""
64right_cal = ["" for x in range(33)]
65right_time = ""
66glove_data_lock = allocate_lock()
67show_stream = 1
68show_cal_stream = 0
69
70class CalibStruct():
71 gain = np.zeros(24)
72 offset = np.zeros(24)
73
74calib_fac = CalibStruct()
75#calib_fac.gain = np.zeros(24)
76#calib_fac.offset = np.zeros(24)
77
78def read_glove():
79
80 ########################################################## ICE part
81 try:
82 properties = Ice.createProperties(sys.argv)
83 properties.load("./default.generated.cfg")
84 properties.load("./default.cfg")
85
86 init_data = Ice.InitializationData()
87 init_data.properties = properties
88
89 ic = Ice.initialize(init_data)
90
91 Ice.loadSlice('-I%s %s -DMININTERFACE' % (Ice.getSliceDir(), 'CyberGloveInterface.ice'))
92 print("loadSlice")
93 import armarx
94
95
96 obj = ic.stringToProxy("IceStorm/TopicManager")
97 print("stringToProxy")
98
99 topicMan = IceStorm.TopicManagerPrx.checkedCast(obj)
100 print("checkedCast")
101 topicName = "CyberGloveValues"
102 print(topicName)
103
104 try:
105 topic = topicMan.retrieve(topicName);
106 except IceStorm.NoSuchTopic:
107 print("No such topic.")
108 try:
109
110 topic = topicMan.create(topicName)
111 print("Try to create topic.")
112
113 except IceStorm.TopicExists:
114 # if the topic has been created in the meanwhile
115 # retry the retrieval.
116 topic = topicMan.retrieve(topicName)
117 print("Try again.")
118
119 pub = topic.getPublisher().ice_oneway();
120 glove = armarx.CyberGloveListenerInterfacePrx.uncheckedCast(pub);
121
122
123 #base = ic.stringToProxy("SimpleprosthesisInterfaceUnit")
124
125 #prosthesis = armarx.SimpleprosthesisInterfacePrx.checkedCast(base)
126 if not glove:
127 raise RuntimeError("Invalid proxy")
128
129 #val = 0
130 #print(val)
131
132 except:
133 traceback.print_exc()
134 status = 1
135
136 ############################################################ Glove part
137
138 Rcache = right_ser.read(61)
139 #Rcache = Rcache[15:]
140 Rcache = Rcache[Rcache.index("S")+1:]
141 Rcache = Rcache + right_ser.read(61-len(Rcache))
142
143 while(1):
144
145 ######RIGHT SENSE#######
146 #right_s = ""
147 #right_ser.write("1G")
148 #right_ser = serial.Serial(right_port, 115200, timeout=0)
149 global right_s
150 #global right_cal
151 global show_stream
152 #global show_cal_stream
153 #global right_ser
154 #right_ser.write("1G")
155 #Rcache = right_ser.read(53)
156 #right_s = right_s[15:]
157 #right_s = right_s[right_s.index("S")-13:right_s.index("S")+50]
158 #print len(right_s)
159 glove_data_lock.acquire()
160 right_s = right_ser.read(61)
161 if len(right_s) != 0 and right_s[13] != "S":
162 print "Missed beginning."
163 #print right_s[10]
164 right_s = right_s[right_s.index("S"):]
165 cut_len = 48 - len(right_s)
166 if cut_len <= 0:
167 cut_len = 14
168 right_s = right_s + right_ser.read(cut_len)
169 #print len(right_s)
170 #print right_s[0]
171 #print right_s[39]
172 right_s = ""
173 if len(right_s) != 0 and show_stream:
174 timestamp = ""
175 sensorData = np.zeros(23)
176 i = 0;
177 for n in range(14):
178 #print right_s[n]
179 timestamp = timestamp + right_s[n]
180 for n in range(14, len(right_s)-1, 2):
181 #print ord(right_s[n])
182 #print ord(right_s[n+1])
183 dataReading = 256*ord(right_s[n]) + ord(right_s[n+1])
184 #print dataReading
185 sensorData[i] = dataReading
186 i = i + 1
187 print str(len(right_s)) + " " + timestamp + " " + str(time.time())
188 values = armarx.CyberGloveValues(name = "gloveValues", time = timestamp, thumbCMC = sensorData[0], thumbMCP = sensorData[1], thumbIP = sensorData[2], thumbAbd = sensorData[3], indexMCP = sensorData[4], indexPIP = sensorData[5], indexDIP = sensorData[6], middleMCP = sensorData[7], middlePIP = sensorData[8], middleDIP = sensorData[9], middleAbd = sensorData[10], ringMCP = sensorData[11], ringPIP = sensorData[12], ringDIP = sensorData[13], ringAbd = sensorData[14], littleMCP = sensorData[15], littlePIP = sensorData[16], littleDIP = sensorData[17], littleAbd = sensorData[18], palmArch = sensorData[19], wristFlex = sensorData[20], wristAbd = sensorData[21])
189 glove.reportGloveValues(values)
190
191 glove_data_lock.release()
192 #right_glove_value = right_s[38].encode('hex')
193 #print int(glove_value, 16)-open_avg
194 #print "RIGHT is released"
195
196def main_loop():
197 global show_stream
198 global show_cal_stream
199 while(1):
200 c = raw_input("Choose raw sensor value (s), constant raw stream (r), glove calibration (c), stop stream (d) or quit (q)")
201 if c == "s":
202 #print "lock is acquired"
203 glove_data_lock.acquire()
204 #right_ser.write("1G")
205 global right_s
206 for n in range(14):
207 print right_s[n]
208 for n in range(14, len(right_s)-1, 2):
209 print 256*ord(right_s[n]) + ord(right_s[n+1])
210 #print ord(right_s[n])
211 #print ord(right_s[n+1])
212 glove_data_lock.release()
213 elif c == "r":
214 show_stream = 1
215 elif c == "t":
216 glove_data_lock.acquire()
217 act_s = right_s
218 glove_data_lock.release()
219 if len(act_s) != 0:
220 for n in range(14):
221 right_cal[n] = act_s[n]
222 #if show_cal_stream:
223 print right_cal[n]
224 for n in range(14, len(act_s)-1, 2):
225 #print ord(right_s[n])
226 #print ord(right_s[n+1])
227 right_cal[(n-14)/2 + 14] = (256*ord(act_s[n]) + ord(act_s[n+1]) - calib_fac.offset[(n-14)/2])*calib_fac.gain[(n-14)/2]
228 #if show_cal_stream:
229 print right_cal[(n-14)/2 + 14]
230
231 elif c == "c":
232 calibrate_glove()
233 elif c == "d":
234 show_stream = 0
235 elif c == "q":
236 break
237
238def calibrate_glove():
239 global calib_fac
240 raw_input("Flat hand straight fingers pose.")
241 glove_data_lock.acquire()
242 for finger in range(5):
243 for joint in range(4):
244 if joint == 3:
245 if finger <= 1:
246 break
247 if joint == 0:
248 if finger == 0:
249 joint = joint + 1
250 if finger == 0:
251 sensorNo = joint
252 calNo = sensorNo
253 elif finger == 1:
254 sensorNo = 4 + joint
255 calNo = sensorNo
256 else:
257 sensorNo = 7 + (finger - 2)*4 + joint
258 calNo = finger*4 + joint
259 sensorNo = sensorNo*2 + 14
260 calib_fac.offset[calNo] = 256*ord(right_s[sensorNo]) + ord(right_s[sensorNo + 1])
261 print "Finger" + str(finger)
262 print "Joint" + str(joint)
263 print sensorNo
264 print calNo
265 print calib_fac.offset[calNo]
266 calib_fac.offset[20] = 256*ord(right_s[52]) + ord(right_s[53])
267 glove_data_lock.release()
268
269 calNo = 1
270 sensorNo = calNo*2 + 14
271 fingerAngle = 37
272 raw_input("Thumb proximal joint bent 37 degrees.")
273 glove_data_lock.acquire()
274 calib_fac.gain[calNo] = (fingerAngle*PI/180)/(256*ord(right_s[sensorNo]) + ord(right_s[sensorNo+1]) - calib_fac.offset[calNo])
275 print 256*ord(right_s[sensorNo]) + ord(right_s[sensorNo+1])
276 glove_data_lock.release()
277 #print "Thumb1"
278 print calib_fac.gain[calNo]
279
280 calNo = 2
281 sensorNo = calNo*2 + 14
282 fingerAngle = 76
283 raw_input("Thumb distal joint bent 76 degrees.")
284 glove_data_lock.acquire()
285 calib_fac.gain[calNo] = (fingerAngle*PI/180)/(256*ord(right_s[sensorNo]) + ord(right_s[sensorNo+1]) - calib_fac.offset[calNo])
286 print 256*ord(right_s[sensorNo]) + ord(right_s[sensorNo+1])
287 glove_data_lock.release()
288 print calib_fac.gain[calNo]
289
290 for finger in range(1,5):
291 for joint in range(3):
292 fingerAngle = 76
293 if finger == 1:
294 calNo = 4 + joint
295 sensorNo = calNo*2 + 14
296 else:
297 calNo = finger*4 + joint
298 sensorNo = (7 + (finger - 2)*4 + joint)*2 + 14
299 raw_input("Finger " + str(finger) + ", joint " + str(joint) + " bent " + str(fingerAngle) + " degrees.")
300 glove_data_lock.acquire()
301 calib_fac.gain[calNo] = (fingerAngle*PI/180)/(256*ord(right_s[sensorNo]) + ord(right_s[sensorNo+1]) - calib_fac.offset[calNo])
302 print 256*ord(right_s[sensorNo]) + ord(right_s[sensorNo+1])
303 glove_data_lock.release()
304 print calib_fac.gain[calNo]
305
306 raw_input("Finger abducted with 25, 20 and 21 degrees.")
307 glove_data_lock.acquire()
308 calib_fac.gain[11] = (25*PI/180)/(256*ord(right_s[34]) + ord(right_s[35]) - calib_fac.offset[11])
309 print "Middle: " + str(256*ord(right_s[34]) + ord(right_s[35]))
310 print calib_fac.gain[11]
311 calib_fac.gain[15] = (20*PI/180)/(256*ord(right_s[42]) + ord(right_s[43]) - calib_fac.offset[15])
312 print "Ring: " + str(256*ord(right_s[42]) + ord(right_s[43]))
313 print calib_fac.gain[15]
314 calib_fac.gain[19] = (21*PI/180)/(256*ord(right_s[50]) + ord(right_s[51]) - calib_fac.offset[19])
315 print "Little: " + str(256*ord(right_s[50]) + ord(right_s[51]))
316 print calib_fac.gain[19]
317 glove_data_lock.release()
318
319 raw_input("Thumb metacarpal extension 90 degrees.")
320 glove_data_lock.acquire()
321 calib_fac.gain[0] = (90*PI/180)/(256*ord(right_s[14]) + ord(right_s[15]) - calib_fac.offset[0])
322 glove_data_lock.release()
323
324 raw_input("Thumb fully abducted.")
325 glove_data_lock.acquire()
326 calib_fac.offset[3] = 256*ord(right_s[20]) + ord(right_s[21])
327 glove_data_lock.release()
328
329 raw_input("Thumb adducted 76 degrees.")
330 glove_data_lock.acquire()
331 calib_fac.gain[3] = (76*PI/180)/(256*ord(right_s[20]) + ord(right_s[21]) - calib_fac.offset[3])
332 glove_data_lock.release()
333
334 fingerAngle = input("Palm arch, please enter acquired angle: ")
335 print fingerAngle
336 glove_data_lock.acquire()
337 calib_fac.gain[20] = (fingerAngle*PI/180)/(256*ord(right_s[52]) + ord(right_s[53]) - calib_fac.offset[20])
338 glove_data_lock.release()
339
340 file = open("generateGloveCalibration.cal","w")
341
342 file.write("VTi CyberGlove Calibration File (v2.0.0)")
343 for finger in range(6):
344 file.write("Finger " + str(finger) + ":")
345 for joint in range(4):
346 file.write(" " + str(joint) + " " + str(calib_fac.offset[finger*4 + joint]) + " " + str(calib_fac.gain[finger*4 + joint]) + " 0")
347
348 file.close()
349
350start_new_thread(read_glove,())
351main_loop()
352
353if ic:
354 #clean up
355 try:
356 ic.destroy()
357 except:
358 traceback.print_exc()
359 status = 1
360
361 sys.exit(status)
362#read_glove()
while
while(1)
Definition Scanner.cpp:721
str
std::string str(const T &t)
Definition UserAssistedSegmenterGuiWidgetController.cpp:43
cyberglove_with_calib_22dof.CalibStruct
Definition cyberglove_with_calib_22dof.py:70
cyberglove_with_calib_22dof.bcolors
Definition cyberglove_with_calib_22dof.py:24
Ice::createProperties
Ice::PropertiesPtr createProperties()
cyberglove_with_calib_22dof.calibrate_glove
calibrate_glove()
Definition cyberglove_with_calib_22dof.py:238
cyberglove_with_calib_22dof.signal_handler
signal_handler(signal, frame)
Definition cyberglove_with_calib_22dof.py:50
cyberglove_with_calib_22dof.main_loop
main_loop()
Definition cyberglove_with_calib_22dof.py:196
cyberglove_with_calib_22dof.read_glove
read_glove()
Definition cyberglove_with_calib_22dof.py:78