cyberglove_with_calib_22dof.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import serial
4 import os
5 import time
6 import signal
7 import sys
8 import tty
9 import termios
10 
11 import numpy as np
12 
13 import sys, traceback, Ice
14 import IceStorm
15 import time
16 
17 from thread import start_new_thread, allocate_lock
18 
19 os.system("clear")
20 
21 status = 0
22 ic = None
23 
24 class bcolors:
25  OKBLUE = '\033[94m'
26  OKGREEN = '\033[92m'
27  WARNING = '\033[93m'
28  FAIL = '\033[91m'
29  ENDC = '\033[0m'
30 
31 print bcolors.OKBLUE+"""
32  ______ __ ________
33  / ____/_ __/ /_ ___ _____/ ____/ /___ _ _____
34  / / / / / / __ \/ _ \/ ___/ / __/ / __ \ | / / _ \\
35 / /___/ /_/ / /_/ / __/ / / /_/ / / /_/ / |/ / __/
36 \____/\__, /_.___/\___/_/ \____/_/\____/|___/\___/
37  /____/
38 """ + bcolors.ENDC
39 
40 print 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 
44 print ""
45 print ""
46 print ""
47 
48 PI = 3.14159
49 
50 def signal_handler(signal, frame):
51  print '\nExiting...'
52  sys.exit(0)
53 signal.signal(signal.SIGINT, signal_handler)
54 
55 right_port = '/dev/ttyUSB0'
56 
57 right_ser = serial.Serial(right_port, 115200, timeout=1)
58 right_ser.write("1dw") #disable wifi
59 right_ser.write("1eu") #enable usb
60 right_ser.write("1ds") #disable sd card
61 right_ser.write("1S") #enable data streaming
62 
63 right_s = ""
64 right_cal = ["" for x in range(33)]
65 right_time = ""
66 glove_data_lock = allocate_lock()
67 show_stream = 1
68 show_cal_stream = 0
69 
70 class CalibStruct():
71  gain = np.zeros(24)
72  offset = np.zeros(24)
73 
74 calib_fac = CalibStruct()
75 #calib_fac.gain = np.zeros(24)
76 #calib_fac.offset = np.zeros(24)
77 
78 def 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 
196 def 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 
238 def 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 
350 start_new_thread(read_glove,())
351 main_loop()
352 
353 if 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()
cyberglove_with_calib_22dof.calibrate_glove
def calibrate_glove()
Definition: cyberglove_with_calib_22dof.py:238
str
std::string str(const T &t)
Definition: UserAssistedSegmenterGuiWidgetController.cpp:42
rapidxml::print
OutIt print(OutIt out, const xml_node< Ch > &node, int flags=0)
Prints XML to given output iterator.
Definition: rapidxml_print.hpp:507
cyberglove_with_calib_22dof.CalibStruct
Definition: cyberglove_with_calib_22dof.py:70
cyberglove_with_calib_22dof.read_glove
def read_glove()
Definition: cyberglove_with_calib_22dof.py:78
Ice::createProperties
Ice::PropertiesPtr createProperties()
armarx::aron::input
ReaderT::InputType & input
Definition: rw.h:19
cyberglove_with_calib_22dof.signal_handler
def signal_handler(signal, frame)
Definition: cyberglove_with_calib_22dof.py:50
armarx::CyberGloveValues
Definition: CyberGloveInterface.ice:30
cyberglove_with_calib_22dof.bcolors
Definition: cyberglove_with_calib_22dof.py:24
while
while(1)
Definition: Scanner.cpp:764
cyberglove_with_calib_22dof.main_loop
def main_loop()
Definition: cyberglove_with_calib_22dof.py:196