00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 import rospy
00020
00021 from sr_robot_lib.etherCAT_hand_lib import EtherCAT_Hand_Lib
00022 from QtGui import QTreeWidgetItem, QTreeWidgetItemIterator, QColor, QIcon, QMessageBox
00023 from PyQt4.Qt import QTimer
00024 from PyQt4.QtCore import SIGNAL
00025
00026 import yaml
00027 try:
00028 from yaml import CLoader as Loader, CDumper as Dumper
00029 except ImportError:
00030 from yaml import Loader, Dumper
00031 import os
00032
00033 from collections import deque
00034
00035 green = QColor(153, 231, 96)
00036 orange = QColor(247, 206, 134)
00037 red = QColor(236, 178, 178)
00038
00039 class IndividualCalibration( QTreeWidgetItem ):
00040 """
00041 Calibrate a single joint by raw and calibrated values
00042 Calibrated joints will appear as green
00043 or orange if calibrations are loaded from a file
00044 """
00045 def __init__(self, joint_name,
00046 raw_value, calibrated_value,
00047 parent_widget, tree_widget,
00048 robot_lib):
00049 self.joint_name = joint_name
00050 self.raw_value = int(raw_value)
00051 self.calibrated_value = calibrated_value
00052 self.tree_widget = tree_widget
00053 self.robot_lib = robot_lib
00054
00055 QTreeWidgetItem.__init__(self, parent_widget, ["", "", str(self.raw_value), str(self.calibrated_value)])
00056
00057 for col in xrange(self.tree_widget.columnCount()):
00058 self.setBackgroundColor(col, QColor(red))
00059
00060 self.tree_widget.addTopLevelItem( self )
00061
00062 self.is_calibrated = False
00063
00064 def remove(self):
00065 self.tree_widget.remove
00066
00067 def calibrate(self):
00068 """
00069 Performs the joint calibration and sets background to green
00070 calibrate only the calibration lines, not the items for the fingers / joints / hand
00071 """
00072 self.raw_value = self.robot_lib.get_average_raw_value(self.joint_name, 100)
00073 self.setText( 2, str(self.raw_value) )
00074
00075 for col in xrange(self.tree_widget.columnCount()):
00076 if self.text(2) != "":
00077 self.setBackgroundColor(col, QColor(green))
00078
00079 self.is_calibrated = True
00080
00081 def set_is_loaded_calibration(self):
00082 """
00083 set the background to orange: those values are loaded
00084 from the file, not recalibrated
00085 """
00086 for col in xrange(self.tree_widget.columnCount()):
00087 if self.text(2) != "":
00088 self.setBackgroundColor(col, QColor(orange))
00089
00090 self.is_calibrated = True
00091
00092
00093 def get_calibration(self):
00094 return [self.raw_value, self.calibrated_value]
00095
00096 class JointCalibration( QTreeWidgetItem ):
00097 """
00098 Calibrate a single joint by calibrations list
00099 Also displays the current joint position in the GUI
00100 """
00101
00102 nb_values_to_check = 5
00103 def __init__(self, joint_name,
00104 calibrations,
00105 parent_widget, tree_widget,
00106 robot_lib):
00107 self.joint_name = joint_name
00108 self.tree_widget = tree_widget
00109 self.robot_lib = robot_lib
00110
00111 self.calibrations = []
00112
00113 self.last_raw_values = deque()
00114
00115 QTreeWidgetItem.__init__(self, parent_widget, ["", joint_name, "", ""] )
00116
00117 for calibration in calibrations:
00118 self.calibrations.append( IndividualCalibration(joint_name,
00119 calibration[0], calibration[1],
00120 self, tree_widget, robot_lib) )
00121
00122
00123 self.timer = QTimer()
00124 self.timer.start(200)
00125 tree_widget.addTopLevelItem(self)
00126 tree_widget.connect(self.timer, SIGNAL('timeout()'), self.update_joint_pos)
00127
00128 def load_joint_calibration(self, new_calibrations):
00129 for calibration in self.calibrations:
00130 self.removeChild( calibration )
00131 self.calibrations = []
00132
00133 for calibration in new_calibrations:
00134 new_calib = IndividualCalibration(self.joint_name,
00135 calibration[0], calibration[1],
00136 self, self.tree_widget, self.robot_lib)
00137 new_calib.set_is_loaded_calibration()
00138 self.calibrations.append( new_calib )
00139
00140
00141 def get_joint_calibration(self):
00142 config = []
00143 for calibration in self.calibrations:
00144 if calibration.is_calibrated:
00145 config.append( calibration.get_calibration() )
00146
00147 if len(config) <= 1:
00148
00149
00150 config = [[0, 0.0], [1, 0.0]]
00151 return [self.joint_name, config]
00152
00153 def update_joint_pos(self):
00154 """
00155 Update the joint position if there are enough nonequal values
00156 If the values are equal it can be assumed the sensor are not measuring properly
00157 """
00158 raw_value = self.robot_lib.get_raw_value( self.joint_name )
00159 self.setText( 2, str(raw_value) )
00160
00161
00162
00163 self.last_raw_values.append(raw_value)
00164 if len(self.last_raw_values) > self.nb_values_to_check:
00165 self.last_raw_values.popleft()
00166
00167
00168 all_equal = True
00169 last_data = self.last_raw_values[0]
00170 for data in self.last_raw_values:
00171 if data != last_data:
00172 all_equal = False
00173 break
00174 if all_equal == True:
00175 self.setIcon(0, QIcon( os.path.join(os.path.dirname(os.path.realpath(__file__)), '../icons/warn.gif')))
00176 self.setToolTip(0,"No noise on the data for the last " + str(self.nb_values_to_check) + " values, there could be a problem with the sensor.")
00177 else:
00178 self.setIcon(0, QIcon())
00179 self.setToolTip(0, "")
00180
00181 def on_close(self):
00182 self.timer.stop()
00183
00184
00185 class FingerCalibration( QTreeWidgetItem ):
00186 """
00187 calibrate all joints of a finger
00188 """
00189
00190 def __init__(self, finger_name,
00191 finger_joints,
00192 parent_widget, tree_widget,
00193 robot_lib):
00194
00195 QTreeWidgetItem.__init__(self, parent_widget, [finger_name, "", "", ""] )
00196
00197 self.joints = []
00198 for joint in finger_joints:
00199 self.joints.append( JointCalibration( joint_name = joint[0],
00200 calibrations = joint[1],
00201 parent_widget = self,
00202 tree_widget = tree_widget,
00203 robot_lib = robot_lib) )
00204
00205 tree_widget.addTopLevelItem(self)
00206
00207 class HandCalibration( QTreeWidgetItem ):
00208 """
00209 calibrate all joints of all fingers of a hand
00210 """
00211
00212 joint_map = {"First Finger":[ [ "FFJ1", [ [0.0, 0.0],
00213 [0.0, 22.5],
00214 [0.0, 45.0],
00215 [0.0, 67.5],
00216 [0.0, 90.0] ] ],
00217 [ "FFJ2", [ [0.0, 0.0],
00218 [0.0, 22.5],
00219 [0.0, 45.0],
00220 [0.0, 67.5],
00221 [0.0, 90.0] ] ],
00222 [ "FFJ3", [ [0.0, 0.0],
00223 [0.0, 22.5],
00224 [0.0, 45.0],
00225 [0.0, 67.5],
00226 [0.0, 90.0] ] ],
00227 [ "FFJ4", [ [0.0, -20.0],
00228 [0.0, -10.0],
00229 [0.0, 0.0],
00230 [0.0, 10.0],
00231 [0.0, 20.0] ] ] ],
00232
00233 "Middle Finger":[ [ "MFJ1", [ [0.0, 0.0],
00234 [0.0, 22.5],
00235 [0.0, 45.0],
00236 [0.0, 67.5],
00237 [0.0, 90.0] ] ],
00238 [ "MFJ2", [ [0.0, 0.0],
00239 [0.0, 22.5],
00240 [0.0, 45.0],
00241 [0.0, 67.5],
00242 [0.0, 90.0] ] ],
00243 [ "MFJ3", [ [0.0, 0.0],
00244 [0.0, 22.5],
00245 [0.0, 45.0],
00246 [0.0, 67.5],
00247 [0.0, 90.0] ] ],
00248 [ "MFJ4", [ [0.0, -20.0],
00249 [0.0, -10.0],
00250 [0.0, 0.0],
00251 [0.0, 10.0],
00252 [0.0, 20.0] ] ] ],
00253
00254 "Ring Finger":[ [ "RFJ1", [ [0.0, 0.0],
00255 [0.0, 22.5],
00256 [0.0, 45.0],
00257 [0.0, 67.5],
00258 [0.0, 90.0] ] ],
00259 [ "RFJ2", [ [0.0, 0.0],
00260 [0.0, 22.5],
00261 [0.0, 45.0],
00262 [0.0, 67.5],
00263 [0.0, 90.0] ] ],
00264 [ "RFJ3", [ [0.0, 0.0],
00265 [0.0, 22.5],
00266 [0.0, 45.0],
00267 [0.0, 67.5],
00268 [0.0, 90.0] ] ],
00269 [ "RFJ4", [ [0.0, -20.0],
00270 [0.0, -10.0],
00271 [0.0, 0.0],
00272 [0.0, 10.0],
00273 [0.0, 20.0] ] ] ],
00274
00275 "Little Finger":[ [ "LFJ1", [ [0.0, 0.0],
00276 [0.0, 22.5],
00277 [0.0, 45.0],
00278 [0.0, 67.5],
00279 [0.0, 90.0] ] ],
00280 [ "LFJ2", [ [0.0, 0.0],
00281 [0.0, 22.5],
00282 [0.0, 45.0],
00283 [0.0, 67.5],
00284 [0.0, 90.0] ] ],
00285 [ "LFJ3", [ [0.0, 0.0],
00286 [0.0, 22.5],
00287 [0.0, 45.0],
00288 [0.0, 67.5],
00289 [0.0, 90.0] ] ],
00290 [ "LFJ4", [ [0.0, -20.0],
00291 [0.0, -10.0],
00292 [0.0, 0.0],
00293 [0.0, 10.0],
00294 [0.0, 20.0] ] ],
00295 [ "LFJ5", [ [0.0, 0.0],
00296 [0.0, 22.5],
00297 [0.0, 45.0],
00298 [0.0, 67.5],
00299 [0.0, 90.0] ] ] ],
00300
00301 "Thumb":[ [ "THJ1", [ [0.0, 0.0],
00302 [0.0, 22.5],
00303 [0.0, 45.0],
00304 [0.0, 67.5],
00305 [0.0, 90.0] ] ],
00306 [ "THJ2", [ [0.0, -40.0],
00307 [0.0, -20.0],
00308 [0.0, 0.0],
00309 [0.0, 20.0],
00310 [0.0, 40.0] ] ],
00311 [ "THJ3", [ [0.0, -15.0],
00312 [0.0, 0.0],
00313 [0.0, 15.0] ] ],
00314 [ "THJ4", [ [0.0, 0.0],
00315 [0.0, 22.5],
00316 [0.0, 45.0],
00317 [0.0, 67.5] ] ],
00318 [ "THJ5", [ [0.0, -60.0],
00319 [0.0, -30.0],
00320 [0.0, 0.0],
00321 [0.0, 30.0],
00322 [0.0, 60.0] ] ] ],
00323
00324 "Wrist":[ [ "WRJ1", [ [0.0, -45.0],
00325 [0.0, -22.5],
00326 [0.0, 0.0],
00327 [0.0, 15.0],
00328 [0.0, 30.0] ] ],
00329 [ "WRJ2", [ [0.0, -30.0],
00330 [0.0, 0.0],
00331 [0.0, 10.0] ] ] ]
00332 }
00333
00334 def __init__(self,
00335 tree_widget,
00336 progress_bar,
00337 fingers = ["First Finger", "Middle Finger",
00338 "Ring Finger", "Little Finger",
00339 "Thumb", "Wrist"]):
00340 self.fingers = []
00341
00342
00343 self.is_active = True
00344
00345 QTreeWidgetItem.__init__(self, ["Hand", "", "", ""] )
00346
00347 self.robot_lib = EtherCAT_Hand_Lib()
00348 if not self.robot_lib.activate():
00349 btn_pressed = QMessageBox.warning(tree_widget, "Warning", "The EtherCAT Hand node doesn't seem to be running, or the debug topic is not being published. Do you still want to continue? The calibration will be useless.",
00350 buttons = QMessageBox.Ok | QMessageBox.Cancel)
00351
00352 if btn_pressed == QMessageBox.Cancel:
00353 self.is_active = False
00354
00355
00356 for finger in fingers:
00357 if finger in self.joint_map.keys():
00358 self.fingers.append( FingerCalibration( finger,
00359 self.joint_map[finger],
00360 self, tree_widget,
00361 self.robot_lib) )
00362
00363 else:
00364 print finger, " not found in the calibration map"
00365
00366 self.joint_0_calibration_index = 0
00367
00368 self.progress_bar = progress_bar
00369
00370 self.tree_widget = tree_widget
00371 self.tree_widget.addTopLevelItem(self)
00372 self.tree_widget.itemActivated.connect(self.calibrate_item)
00373
00374 def unregister(self):
00375 it = QTreeWidgetItemIterator( self )
00376 while it.value():
00377 try:
00378 it.value().on_close()
00379 except:
00380 pass
00381 it += 1
00382
00383 self.robot_lib.on_close()
00384
00385 def calibrate_item(self, item):
00386 try:
00387
00388 item.calibrate()
00389 except:
00390 pass
00391
00392 self.progress()
00393
00394
00395 self.tree_widget.setItemSelected( item, False )
00396 next_item = self.tree_widget.itemBelow(item)
00397 self.tree_widget.setItemSelected( next_item, True )
00398 self.tree_widget.setCurrentItem( next_item )
00399
00400 def calibrate_joint0s(self, btn_joint_0s):
00401 joint0s = [ "FFJ1", "FFJ2",
00402 "MFJ1", "MFJ2",
00403 "RFJ1", "RFJ2",
00404 "LFJ1", "LFJ2" ]
00405
00406 it = QTreeWidgetItemIterator( self )
00407 while it.value():
00408 if it.value().text(1) in joint0s:
00409 it += self.joint_0_calibration_index + 1
00410 it.value().calibrate()
00411 it += 1
00412
00413 self.joint_0_calibration_index += 1
00414 if self.joint_0_calibration_index == len( self.joint_map["First Finger"][0][1] ):
00415 self.joint_0_calibration_index = 0
00416
00417
00418 btn_joint_0s.setText( "Save all Joint 0s (angle = "+ str(self.joint_map["First Finger"][0][1][self.joint_0_calibration_index][1]) +")" )
00419
00420 self.progress()
00421
00422 def progress(self):
00423 it = QTreeWidgetItemIterator( self )
00424 nb_of_items = 0
00425 nb_of_calibrated_items = 0
00426 while it.value():
00427 it += 1
00428 try:
00429 if it.value().is_calibrated:
00430 nb_of_calibrated_items += 1
00431 nb_of_items += 1
00432 except:
00433 pass
00434
00435 self.progress_bar.setValue( int( float(nb_of_calibrated_items) / float(nb_of_items) * 100.0 ) )
00436
00437 def load(self, filepath):
00438 f = open(filepath,'r')
00439 document = ""
00440 for line in f.readlines():
00441 document += line
00442 f.close()
00443 yaml_config = yaml.load(document)
00444
00445 for joint in yaml_config["sr_calibrations"]:
00446 it = QTreeWidgetItemIterator( self )
00447 while it.value():
00448 if it.value().text(1) == joint[0]:
00449 it.value().load_joint_calibration( joint[1] )
00450 it += 1
00451
00452 def save(self, filepath):
00453 yaml_config = {}
00454
00455 joint_configs = []
00456 it = QTreeWidgetItemIterator( self )
00457 while it.value():
00458 try:
00459 joint_configs.append( it.value().get_joint_calibration() )
00460 except:
00461 pass
00462 it += 1
00463
00464
00465
00466
00467 full_config_to_write = "{\'sr_calibrations\': [\n"
00468 for joint_config in joint_configs:
00469 full_config_to_write += "[\""
00470 full_config_to_write += joint_config[0] + "\", "
00471
00472
00473 for index,calib in enumerate(joint_config[1]):
00474 joint_config[1][index][0] = float(calib[0])
00475 joint_config[1][index][1] = float(calib[1])
00476
00477 full_config_to_write += str(joint_config[1])
00478 full_config_to_write += "], \n"
00479 full_config_to_write += "]}"
00480
00481 f = open(filepath, 'w')
00482 f.write(full_config_to_write)
00483 f.close()
00484
00485 def is_calibration_complete(self):
00486 it = QTreeWidgetItemIterator( self )
00487 while it.value():
00488 try:
00489 if not it.value().is_calibrated:
00490 return False
00491 except:
00492 pass
00493 it += 1
00494
00495 return True
00496