def __init__(self, verbose=1): rospy.init_node("cyberglove_mapper_minimizer") #connect to the cyberglove self.cyberglove = Cyberglove() self.verbose = verbose glove_sensors = [ "G_ThumbRotate", "G_ThumbMPJ", "G_ThumbIJ", "G_ThumbAb", "G_IndexMPJ", "G_IndexPIJ", "G_IndexDIJ", "G_MiddleMPJ", "G_MiddlePIJ", "G_MiddleDIJ", "G_MiddleIndexAb", "G_RingMPJ", "G_RingPIJ", "G_RingDIJ", "G_RingMiddleAb", "G_PinkieMPJ", "G_PinkiePIJ", "G_PinkieDIJ", "G_PinkieRingAb", "G_PalmArch", "G_WristPitch", "G_WristYaw" ] index = 0 self.glove_name_index_map = {} for sensor in glove_sensors: self.glove_name_index_map[sensor] = index index += 1 # fill the table containing all the joints hand_joints = [ "TH1", "TH2", "TH3", "TH4", "TH5", "FF0", "FF3", "FF4", "MF0", "MF3", "MF4", "RF0", "RF3", "RF4", "LF0", "LF3", "LF4", "LF5", "WR1", "WR2" ] index = 0 self.hand_name_index_map = {} for sensor in hand_joints: self.hand_name_index_map[sensor] = index index += 1 self.thumb_glove = { "G_ThumbRotate": 0, "G_ThumbMPJ": 1, "G_ThumbAb": 2 } self.thumb_hand = {"TH2": 0, "TH4": 1, "TH5": 2} self.configurations = [] self.initialise_configurations() self.simplex_iteration_index = 0 if (self.verbose == 1): for conf in self.configurations: print "-------" print conf.description print conf.glove_data print conf.hand_data print "-------" print "" self.minerror = 1000000 self.maxerror = 0
def __init__(self, verbose = 1): rospy.init_node("cyberglove_mapper_minimizer") #connect to the cyberglove self.cyberglove = Cyberglove() self.verbose = verbose glove_sensors = ["G_ThumbRotate", "G_ThumbMPJ", "G_ThumbIJ", "G_ThumbAb", "G_IndexMPJ", "G_IndexPIJ", "G_IndexDIJ", "G_MiddleMPJ", "G_MiddlePIJ", "G_MiddleDIJ", "G_MiddleIndexAb", "G_RingMPJ", "G_RingPIJ", "G_RingDIJ", "G_RingMiddleAb", "G_PinkieMPJ", "G_PinkiePIJ", "G_PinkieDIJ", "G_PinkieRingAb", "G_PalmArch", "G_WristPitch", "G_WristYaw"] index = 0 self.glove_name_index_map = {} for sensor in glove_sensors: self.glove_name_index_map[sensor] = index index += 1 # fill the table containing all the joints hand_joints = ["TH1","TH2","TH3","TH4","TH5","FF0","FF3","FF4","MF0","MF3","MF4","RF0","RF3","RF4","LF0", "LF3","LF4","LF5","WR1","WR2"] index = 0 self.hand_name_index_map = {} for sensor in hand_joints: self.hand_name_index_map[sensor] = index index += 1 self.thumb_glove = { "G_ThumbRotate": 0, "G_ThumbMPJ": 1, "G_ThumbAb": 2 } self.thumb_hand = { "TH2": 0, "TH4": 1, "TH5": 2 } self.configurations = [] self.initialise_configurations() self.simplex_iteration_index = 0 if(self.verbose == 1): for conf in self.configurations: print "-------" print conf.description print conf.glove_data print conf.hand_data print "-------" print "" self.minerror = 1000000 self.maxerror = 0
def __init__(self, description_function=default_description): """ Initialize some class variables: a table containing the calibration steps, a connection to the cyberglove library and a description function for the calibration steps @param description_function: specify a function you want to use to describe the calibration steps ( text / pictures / animation / ... ). Must take a joint name as parameter. """ self.calibration_steps = [] self.cyberglove = Cyberglove() # fill the table containing all the joints self.joints = {} for name in self.cyberglove.get_joints_names(): self.joints[name] = Calibration() self.get_calibration_steps() if description_function == None: description_function = do_nothing self.description_function = description_function
def __init__(self, description_function = default_description): """ Initialize some class variables: a table containing the calibration steps, a connection to the cyberglove library and a description function for the calibration steps @param description_function: specify a function you want to use to describe the calibration steps ( text / pictures / animation / ... ). Must take a joint name as parameter. """ self.calibration_steps = [] self.cyberglove = Cyberglove() # fill the table containing all the joints self.joints = {} for name in self.cyberglove.get_joints_names(): self.joints[name] = Calibration() self.get_calibration_steps() if description_function == None: description_function = do_nothing self.description_function = description_function
class CybergloveCalibrer: """ A utility to calibrate the cyberglove. """ def __init__(self, description_function=default_description): """ Initialize some class variables: a table containing the calibration steps, a connection to the cyberglove library and a description function for the calibration steps @param description_function: specify a function you want to use to describe the calibration steps ( text / pictures / animation / ... ). Must take a joint name as parameter. """ self.calibration_steps = [] self.cyberglove = Cyberglove() # fill the table containing all the joints self.joints = {} for name in self.cyberglove.get_joints_names(): self.joints[name] = Calibration() self.get_calibration_steps() if description_function == None: description_function = do_nothing self.description_function = description_function def get_calibration_steps(self): """ Read the calibration steps from the xml file. @return: 0 when the values were read. """ #first step: calibrate 0s, 3s and TH4 joints1 = [ Joint("G_IndexMPJ", 0, 90), Joint("G_IndexPIJ", 0, 90), Joint("G_IndexDIJ", 0, 90), Joint("G_MiddleMPJ", 0, 90), Joint("G_MiddlePIJ", 0, 90), Joint("G_MiddleDIJ", 0, 90), Joint("G_RingMPJ", 0, 90), Joint("G_RingPIJ", 0, 90), Joint("G_RingDIJ", 0, 90), Joint("G_PinkieMPJ", 0, 90), Joint("G_PinkiePIJ", 0, 90), Joint("G_PinkieDIJ", 0, 90), Joint("G_ThumbAb", 50, 0) ] self.calibration_steps.append( CalibrationStep( step_name="Joints 0s, 3s and thumb abduction (THJ4)", step_description=[ "Hand flat on a table, fingers joined, thumb opened", "Hand forming a fist, thumb closed" ], joints=joints1)) #second step: calibrate 4s, TH1, TH2, TH5 and WR2 joints2 = [ Joint("G_ThumbIJ", 90, 0), Joint("G_ThumbMPJ", 30, -30), Joint("G_MiddleIndexAb", 0, 50), Joint("G_RingMiddleAb", 0, 50), Joint("G_PinkieRingAb", 0, 50), Joint("G_WristYaw", 10, -30) ] self.calibration_steps.append( CalibrationStep( step_name="Joints 4s + TH1, 2, and WR2", step_description=[ "Hand flat fingers joined, thumb completely curled under the palm, wrist 2 bent to the left", "Hand flat fingers apart, thumb completely opened, wrist bent 2 to the right" ], joints=joints2)) #third step: calibrate TH5 joints3 = [Joint("G_ThumbRotate", 60, -60)] self.calibration_steps.append( CalibrationStep(step_name="Joint TH5", step_description=[ "thumb completely under the palm", "thumb completely opened, curled over the palm" ], joints=joints3)) #fourth step: calibrate LF5 and WR1 joints4 = [Joint("G_PalmArch", 0, 40), Joint("G_WristPitch", -30, 40)] self.calibration_steps.append( CalibrationStep(step_name="LF5 and WR1", step_description=[ "Hand flat, wrist 1 bent backward", "Palm curled (LFJ5), wrist 1 bent forward" ], joints=joints4)) return 0 def do_step_min(self, index): """ Run the given step of the calibration, gets the min values. @param index: the index of the step in the calibration file @return: 0 when the values were read. """ joints = self.calibration_steps[index].joints # display the description self.description_function( self.calibration_steps[index].step_description[0], 0) for joint in joints: name = joint.name #read the min values self.joints[name].raw_min = self.cyberglove.read_raw_average_value( name) self.joints[name].calibrated_min = joint.min #still needs to read the max before fully calibrated self.joints[name].is_calibrated += 0.5 return 0 def do_step_max(self, index): """ Run the given step of the calibration, gets the max values. As this method is called after the do_step_min() method, we don't display the description @param index: the index of the step in the calibration file @return: 0 when the values were read. """ joints = self.calibration_steps[index].joints # display the description self.description_function( self.calibration_steps[index].step_description[1], 0) for joint in joints: name = joint.name #read the max values self.joints[name].raw_max = self.cyberglove.read_raw_average_value( name) self.joints[name].calibrated_max = joint.max #still needs to read the max before fully calibrated self.joints[name].is_calibrated += 0.5 return 0 def is_step_done(self, joint_name): """ Check if the joint is calibrated. @param joint_name: the name of the joint @return: 1 if the joint has already been calibrated. """ return self.joints[joint_name].is_calibrated def all_steps_done(self): """ Check if all the steps were processed. @return: True if all the steps were processed. """ for calib in self.joints.values(): if calib.is_calibrated != 1: return False return True def reorder_calibration(self): """ Reorder the calibration: set the raw_min to the min raw_value """ for name in self.joints.keys(): if self.joints[name].raw_min > self.joints[name].raw_max: tmp_raw = self.joints[name].raw_min tmp_cal = self.joints[name].calibrated_min self.joints[name].raw_min = self.joints[name].raw_max self.joints[name].calibrated_min = self.joints[ name].calibrated_max self.joints[name].raw_max = tmp_raw self.joints[name].calibrated_max = tmp_cal def write_calibration_file(self, filepath): """ Checks if all the steps were processed by calling self.all_steps_done() Reorder the calibration Then writes the whole calibration to a given file. @param filepath: Where to write the calibration @return: 0 if the file has been written, -1 if the calibration is not finished yet, -2 if other error """ #Where all the steps processed? if not self.all_steps_done(): return -1 #reorder the calibration self.reorder_calibration() ############### # Write to an xml file ############### #store the text in a table text = [] text.append("<?xml version=\"1.0\" ?>") text.append("<Cyberglove_calibration>") for name in self.joints: #joint name text.append("<Joint name=\"" + name + "\">") cal = self.joints[name] #min value text.append("<calib raw_value=\"" + str(cal.raw_min) + "\" calibrated_value=\"" + str(cal.calibrated_min) + "\"/>") #max value text.append("<calib raw_value=\"" + str(cal.raw_max) + "\" calibrated_value=\"" + str(cal.calibrated_max) + "\"/>") text.append("</Joint>") text.append("</Cyberglove_calibration>") #write the text to a file try: output = open(filepath, "w") for line in text: output.write(line + "\n") output.close() except: return -2 return 0 def load_calib(self, filename): if filename == "": return -1 rospy.wait_for_service('/cyberglove/calibration') try: calib = rospy.ServiceProxy('cyberglove/calibration', CalibrationSrv) path = filename.encode("iso-8859-1") resp = calib(path) return resp.state except rospy.ServiceException, e: print 'Failed to call start service' return -2
class CybergloveCalibrer: """ A utility to calibrate the cyberglove. """ def __init__(self, description_function = default_description): """ Initialize some class variables: a table containing the calibration steps, a connection to the cyberglove library and a description function for the calibration steps @param description_function: specify a function you want to use to describe the calibration steps ( text / pictures / animation / ... ). Must take a joint name as parameter. """ self.calibration_steps = [] self.cyberglove = Cyberglove() # fill the table containing all the joints self.joints = {} for name in self.cyberglove.get_joints_names(): self.joints[name] = Calibration() self.get_calibration_steps() if description_function == None: description_function = do_nothing self.description_function = description_function def get_calibration_steps(self): """ Read the calibration steps from the xml file. @return: 0 when the values were read. """ #first step: calibrate 0s, 3s and TH4 joints1 = [Joint("G_IndexMPJ", 0, 90), Joint("G_IndexPIJ", 0, 90), Joint("G_IndexDIJ", 0, 90), Joint("G_MiddleMPJ", 0, 90), Joint("G_MiddlePIJ", 0, 90), Joint("G_MiddleDIJ", 0, 90), Joint("G_RingMPJ", 0, 90), Joint("G_RingPIJ", 0, 90), Joint("G_RingDIJ", 0, 90), Joint("G_PinkieMPJ", 0, 90), Joint("G_PinkiePIJ", 0, 90), Joint("G_PinkieDIJ", 0, 90), Joint("G_ThumbAb", 50, 0) ] self.calibration_steps.append(CalibrationStep(step_name="Joints 0s, 3s and thumb abduction (THJ4)", step_description = ["Hand flat on a table, fingers joined, thumb opened", "Hand forming a fist, thumb closed"], joints = joints1 )) #second step: calibrate 4s, TH1, TH2, TH5 and WR2 joints2 = [Joint("G_ThumbIJ", 90, 0), Joint("G_ThumbMPJ", 30, -30), Joint("G_MiddleIndexAb", 0, 50), Joint("G_RingMiddleAb", 0, 50), Joint("G_PinkieRingAb", 0, 50), Joint("G_WristYaw", 10, -30)] self.calibration_steps.append(CalibrationStep(step_name="Joints 4s + TH1, 2, and WR2", step_description = ["Hand flat fingers joined, thumb completely curled under the palm, wrist 2 bent to the left", "Hand flat fingers apart, thumb completely opened, wrist bent 2 to the right"], joints = joints2 )) #third step: calibrate TH5 joints3 = [Joint("G_ThumbRotate", 60, -60)] self.calibration_steps.append(CalibrationStep(step_name="Joint TH5", step_description = ["thumb completely under the palm", "thumb completely opened, curled over the palm"], joints = joints3 )) #fourth step: calibrate LF5 and WR1 joints4 = [Joint("G_PalmArch", 0, 40), Joint("G_WristPitch", -30, 40)] self.calibration_steps.append(CalibrationStep(step_name="LF5 and WR1", step_description = ["Hand flat, wrist 1 bent backward", "Palm curled (LFJ5), wrist 1 bent forward"], joints = joints4 )) return 0 def do_step_min(self, index): """ Run the given step of the calibration, gets the min values. @param index: the index of the step in the calibration file @return: 0 when the values were read. """ joints = self.calibration_steps[index].joints # display the description self.description_function(self.calibration_steps[index].step_description[0], 0) for joint in joints: name = joint.name #read the min values self.joints[name].raw_min = self.cyberglove.read_raw_average_value(name) self.joints[name].calibrated_min = joint.min #still needs to read the max before fully calibrated self.joints[name].is_calibrated += 0.5 return 0 def do_step_max(self, index): """ Run the given step of the calibration, gets the max values. As this method is called after the do_step_min() method, we don't display the description @param index: the index of the step in the calibration file @return: 0 when the values were read. """ joints = self.calibration_steps[index].joints # display the description self.description_function(self.calibration_steps[index].step_description[1], 0) for joint in joints: name = joint.name #read the max values self.joints[name].raw_max = self.cyberglove.read_raw_average_value(name) self.joints[name].calibrated_max = joint.max #still needs to read the max before fully calibrated self.joints[name].is_calibrated += 0.5 return 0 def is_step_done(self, joint_name): """ Check if the joint is calibrated. @param joint_name: the name of the joint @return: 1 if the joint has already been calibrated. """ return self.joints[joint_name].is_calibrated def all_steps_done(self): """ Check if all the steps were processed. @return: True if all the steps were processed. """ for calib in self.joints.values(): if calib.is_calibrated != 1: return False return True def reorder_calibration(self): """ Reorder the calibration: set the raw_min to the min raw_value """ for name in self.joints.keys(): if self.joints[name].raw_min > self.joints[name].raw_max: tmp_raw = self.joints[name].raw_min tmp_cal = self.joints[name].calibrated_min self.joints[name].raw_min = self.joints[name].raw_max self.joints[name].calibrated_min = self.joints[name].calibrated_max self.joints[name].raw_max = tmp_raw self.joints[name].calibrated_max = tmp_cal def write_calibration_file(self, filepath): """ Checks if all the steps were processed by calling self.all_steps_done() Reorder the calibration Then writes the whole calibration to a given file. @param filepath: Where to write the calibration @return: 0 if the file has been written, -1 if the calibration is not finished yet, -2 if other error """ #Where all the steps processed? if not self.all_steps_done(): return -1 #reorder the calibration self.reorder_calibration() ############### # Write to an xml file ############### #store the text in a table text = [] text.append("<?xml version=\"1.0\" ?>") text.append("<Cyberglove_calibration>") for name in self.joints: #joint name text.append("<Joint name=\""+name+"\">") cal = self.joints[name] #min value text.append("<calib raw_value=\""+str(cal.raw_min) +"\" calibrated_value=\"" +str(cal.calibrated_min)+"\"/>") #max value text.append("<calib raw_value=\""+str(cal.raw_max) +"\" calibrated_value=\"" +str(cal.calibrated_max)+"\"/>") text.append("</Joint>") text.append("</Cyberglove_calibration>") #write the text to a file try: output = open(filepath, "w") for line in text: output.write(line+"\n") output.close() except: return -2 return 0 def load_calib(self, filename): if filename == "": return -1 rospy.wait_for_service('/cyberglove/calibration') try: calib = rospy.ServiceProxy('cyberglove/calibration', CalibrationSrv) path = filename.encode("iso-8859-1") resp = calib(path) return resp.state except rospy.ServiceException, e: print 'Failed to call start service' return -2
class MappingMinimizer: def __init__(self, verbose=1): rospy.init_node("cyberglove_mapper_minimizer") #connect to the cyberglove self.cyberglove = Cyberglove() self.verbose = verbose glove_sensors = [ "G_ThumbRotate", "G_ThumbMPJ", "G_ThumbIJ", "G_ThumbAb", "G_IndexMPJ", "G_IndexPIJ", "G_IndexDIJ", "G_MiddleMPJ", "G_MiddlePIJ", "G_MiddleDIJ", "G_MiddleIndexAb", "G_RingMPJ", "G_RingPIJ", "G_RingDIJ", "G_RingMiddleAb", "G_PinkieMPJ", "G_PinkiePIJ", "G_PinkieDIJ", "G_PinkieRingAb", "G_PalmArch", "G_WristPitch", "G_WristYaw" ] index = 0 self.glove_name_index_map = {} for sensor in glove_sensors: self.glove_name_index_map[sensor] = index index += 1 # fill the table containing all the joints hand_joints = [ "TH1", "TH2", "TH3", "TH4", "TH5", "FF0", "FF3", "FF4", "MF0", "MF3", "MF4", "RF0", "RF3", "RF4", "LF0", "LF3", "LF4", "LF5", "WR1", "WR2" ] index = 0 self.hand_name_index_map = {} for sensor in hand_joints: self.hand_name_index_map[sensor] = index index += 1 self.thumb_glove = { "G_ThumbRotate": 0, "G_ThumbMPJ": 1, "G_ThumbAb": 2 } self.thumb_hand = {"TH2": 0, "TH4": 1, "TH5": 2} self.configurations = [] self.initialise_configurations() self.simplex_iteration_index = 0 if (self.verbose == 1): for conf in self.configurations: print "-------" print conf.description print conf.glove_data print conf.hand_data print "-------" print "" self.minerror = 1000000 self.maxerror = 0 def initialise_configurations(self): configuration = MappingConfiguration() ### # First configuration configuration.description = "All fingers and thumb are curved maximum in a punch form ... " configuration.glove_data = [] #corresponding data for the hand configuration.hand_data = [[30.0, 52.0, 5.0], [30.0, 43.0, 0.0], [30.0, 68.0, 16.0]] self.configurations.append(configuration.copy()) ### # Second configuration configuration.description = "Thumb touching first finger tip ... " configuration.glove_data = [] #corresponding data for the hand configuration.hand_data = [[30.0, 54.0, -5.0], [19.0, 58.0, 28.0]] self.configurations.append(configuration.copy()) ### # Third configuration configuration.description = "Thumb touching middle finger tip ... " configuration.glove_data = [] #corresponding data for the hand configuration.hand_data = [[30.0, 64.0, 9.0], [1.0, 68.0, 29.0]] self.configurations.append(configuration.copy()) ### # Fourth configuration configuration.description = "Thumb touching ring finger tip ... " configuration.glove_data = [] #corresponding data for the hand configuration.hand_data = [[30.0, 75.0, 19.0], [1.0, 75.0, 42.0]] self.configurations.append(configuration.copy()) ### # Fifth configuration configuration.description = "Thumb touching little finger tip ... " configuration.glove_data = [] #corresponding data for the hand configuration.hand_data = [[30.0, 75.0, 3.0], [1.0, 75.0, 35.0]] self.configurations.append(configuration.copy()) ### # Sixth configuration configuration.description = "Thumb touching all the finger tips ... " configuration.glove_data = [] #corresponding data for the hand configuration.hand_data = [[-30.0, 75.0, 60.0], [-7.0, 63.0, 60.0]] self.configurations.append(configuration.copy()) ### # Seventh configuration configuration.description = "hand flat, thumb relaxed along the fingers " configuration.glove_data = [] #corresponding data for the hand configuration.hand_data = [[0.0, 0.0, -60.0]] self.configurations.append(configuration.copy()) ### # Eigth configuration configuration.description = "hand flat, thumb opened " configuration.glove_data = [] #corresponding data for the hand configuration.hand_data = [[0.0, 75.0, -60.0]] self.configurations.append(configuration.copy()) def evaluation_function(self, matrix): """ The error is computed by adding the square error for each configuration. """ error = 0 #rebuild the matrix as a matrix, not a vector (the implementation of the simplex in scipy works on vectors matrix_tmp = [] for i in range(len(self.thumb_glove)): line = [] for j in range(len(self.thumb_hand)): line.append(matrix[i * len(self.thumb_hand) + j]) matrix_tmp.append(line) matrix = matrix_tmp computed_vector_hand = [] for config in self.configurations: #get the hand vectors from the glove data for vec_glove in config.glove_data: computed_vector_hand.append(self.multiply(vec_glove, matrix)) #compute the square error for vec_comp, vec_hand in zip(computed_vector_hand, config.hand_data): for computed_hand_data in vec_comp: for hand_data in vec_hand: error += (computed_hand_data - hand_data) * (computed_hand_data - hand_data) if self.verbose == 1: print "error: " + str(error) if error < self.minerror: self.minerror = error if error > self.maxerror: self.maxerror = error return error def multiply(self, vector_glove, matrix): """ multiply the vector by the matrix. Returns a vector. """ vector_hand = [] index_col = 0 print vector_glove print matrix for glove_data in vector_glove: data = 0 for line in matrix: data += (glove_data * line[index_col]) vector_hand.append(data) index_col += 1 return vector_hand def callback(self, xk): """ Function called at each iteration """ self.simplex_iteration_index += 1 if self.simplex_iteration_index % 50 == 0: print "-------------------------" print "iteration number: " + str(self.simplex_iteration_index) #print xk def minimize(self): start = [] for i in range(0, len(self.thumb_hand)): line = [] for j in range(0, len(self.thumb_glove)): line.append(0.1) start.append(line) xopt = fmin(self.evaluation_function, start, callback=self.callback, maxiter=5000) #rebuild the result as a matrix, not a vector (the implementation of the simplex in scipy works on vectors output = [] for i in range(len(self.thumb_glove)): line = [] for j in range(len(self.thumb_hand)): line.append(xopt[i * len(self.thumb_hand) + j]) output.append(line) print "min error: " + str(self.minerror) print "max error: " + str(self.maxerror) return output def write_full_mapping(self, output_path="../../../param/GloveToHandMappings"): """ Writes the mapping matrix to a file: the glove values are the lines, the hand values are the columns """ #initialise the matrix with 0s mapping_matrix = [] for i in range(0, len(self.glove_name_index_map)): line = [] for j in range(0, len(self.hand_name_index_map)): line.append(0.0) mapping_matrix.append(line) # TH3 is always 0, RF4 as well #fill the matrix with the known values + th1 (all except the thumb) mapping_matrix[self.glove_name_index_map["G_IndexDIJ"]][ self.hand_name_index_map["FF0"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_IndexPIJ"]][ self.hand_name_index_map["FF0"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_IndexMPJ"]][ self.hand_name_index_map["FF3"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_MiddleIndexAb"]][ self.hand_name_index_map["FF4"]] = -1.0 mapping_matrix[self.glove_name_index_map["G_MiddleDIJ"]][ self.hand_name_index_map["MF0"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_MiddlePIJ"]][ self.hand_name_index_map["MF0"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_MiddleMPJ"]][ self.hand_name_index_map["MF3"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_RingMiddleAb"]][ self.hand_name_index_map["MF4"]] = -1.0 mapping_matrix[self.glove_name_index_map["G_RingDIJ"]][ self.hand_name_index_map["RF0"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_RingPIJ"]][ self.hand_name_index_map["RF0"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_RingMPJ"]][ self.hand_name_index_map["RF3"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_PinkieDIJ"]][ self.hand_name_index_map["LF0"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_PinkiePIJ"]][ self.hand_name_index_map["LF0"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_PinkieMPJ"]][ self.hand_name_index_map["LF3"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_PinkieRingAb"]][ self.hand_name_index_map["LF4"]] = -1.0 mapping_matrix[self.glove_name_index_map["G_PalmArch"]][ self.hand_name_index_map["LF5"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_WristPitch"]][ self.hand_name_index_map["WR1"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_WristYaw"]][ self.hand_name_index_map["WR2"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_ThumbIJ"]][ self.hand_name_index_map["TH1"]] = 1.0 #mapping_matrix[self.glove_name_index_map["G_ThumbAb"]][self.hand_name_index_map["TH4"]] = 1.0 #mapping_matrix[self.glove_name_index_map["G_ThumbRotate"]][self.hand_name_index_map["TH5"]] = 1.0 #mapping_matrix[self.glove_name_index_map["G_ThumbMPJ"]][self.hand_name_index_map["TH2"]] = 1.0 #compute the best mapping for the thumb except th1 thumb_mapping = self.minimize() #fill the matrix with the thumb values computed with the simplex for glove_name in self.thumb_glove.keys(): for hand_name in self.thumb_hand.keys(): #indexes in the computed thumb mapping matrix tmp_index_glove = self.thumb_glove[glove_name] tmp_index_hand = self.thumb_hand[hand_name] #retrieve the mapping value for those sensors mapping_value = thumb_mapping[tmp_index_glove][tmp_index_hand] #indexes for the whole mapping matrix final_index_glove = self.glove_name_index_map[glove_name] final_index_hand = self.hand_name_index_map[hand_name] #update matrix mapping_matrix[final_index_glove][ final_index_hand] = mapping_value #write the matrix to a file file = open(output_path, "w") for line in mapping_matrix: for col in line: file.write(" " + str(col)) file.write("\n") file.close() def record(self, config): for i in range(0, 2): raw_input(str(i) + ": " + config.description) vector_data = [0] * len(self.thumb_glove) for sensor_name in self.thumb_glove.keys(): data = self.cyberglove.read_calibrated_average_value( sensor_name) vector_data[self.thumb_glove[sensor_name]] = data config.glove_data.append(vector_data) if (self.verbose == 1): print "read values:" print config.glove_data
class MappingMinimizer: def __init__(self, verbose = 1): rospy.init_node("cyberglove_mapper_minimizer") #connect to the cyberglove self.cyberglove = Cyberglove() self.verbose = verbose glove_sensors = ["G_ThumbRotate", "G_ThumbMPJ", "G_ThumbIJ", "G_ThumbAb", "G_IndexMPJ", "G_IndexPIJ", "G_IndexDIJ", "G_MiddleMPJ", "G_MiddlePIJ", "G_MiddleDIJ", "G_MiddleIndexAb", "G_RingMPJ", "G_RingPIJ", "G_RingDIJ", "G_RingMiddleAb", "G_PinkieMPJ", "G_PinkiePIJ", "G_PinkieDIJ", "G_PinkieRingAb", "G_PalmArch", "G_WristPitch", "G_WristYaw"] index = 0 self.glove_name_index_map = {} for sensor in glove_sensors: self.glove_name_index_map[sensor] = index index += 1 # fill the table containing all the joints hand_joints = ["TH1","TH2","TH3","TH4","TH5","FF0","FF3","FF4","MF0","MF3","MF4","RF0","RF3","RF4","LF0", "LF3","LF4","LF5","WR1","WR2"] index = 0 self.hand_name_index_map = {} for sensor in hand_joints: self.hand_name_index_map[sensor] = index index += 1 self.thumb_glove = { "G_ThumbRotate": 0, "G_ThumbMPJ": 1, "G_ThumbAb": 2 } self.thumb_hand = { "TH2": 0, "TH4": 1, "TH5": 2 } self.configurations = [] self.initialise_configurations() self.simplex_iteration_index = 0 if(self.verbose == 1): for conf in self.configurations: print "-------" print conf.description print conf.glove_data print conf.hand_data print "-------" print "" self.minerror = 1000000 self.maxerror = 0 def initialise_configurations(self): configuration = MappingConfiguration() ### # First configuration configuration.description = "All fingers and thumb are curved maximum in a punch form ... " configuration.glove_data = [] #corresponding data for the hand configuration.hand_data = [[30.0, 52.0, 5.0], [30.0, 43.0, 0.0], [30.0, 68.0, 16.0] ] self.configurations.append(configuration.copy()) ### # Second configuration configuration.description = "Thumb touching first finger tip ... " configuration.glove_data = [] #corresponding data for the hand configuration.hand_data = [[30.0, 54.0, -5.0], [19.0, 58.0, 28.0]] self.configurations.append(configuration.copy()) ### # Third configuration configuration.description = "Thumb touching middle finger tip ... " configuration.glove_data = [] #corresponding data for the hand configuration.hand_data = [[30.0, 64.0, 9.0], [1.0, 68.0, 29.0]] self.configurations.append(configuration.copy()) ### # Fourth configuration configuration.description = "Thumb touching ring finger tip ... " configuration.glove_data = [] #corresponding data for the hand configuration.hand_data = [[30.0, 75.0, 19.0], [1.0, 75.0, 42.0]] self.configurations.append(configuration.copy()) ### # Fifth configuration configuration.description = "Thumb touching little finger tip ... " configuration.glove_data = [] #corresponding data for the hand configuration.hand_data = [[30.0, 75.0, 3.0], [1.0, 75.0, 35.0]] self.configurations.append(configuration.copy()) ### # Sixth configuration configuration.description = "Thumb touching all the finger tips ... " configuration.glove_data = [] #corresponding data for the hand configuration.hand_data = [[-30.0, 75.0, 60.0], [-7.0, 63.0, 60.0]] self.configurations.append(configuration.copy()) ### # Seventh configuration configuration.description = "hand flat, thumb relaxed along the fingers " configuration.glove_data = [] #corresponding data for the hand configuration.hand_data = [[0.0, 0.0, -60.0]] self.configurations.append(configuration.copy()) ### # Eigth configuration configuration.description = "hand flat, thumb opened " configuration.glove_data = [] #corresponding data for the hand configuration.hand_data = [[0.0, 75.0, -60.0]] self.configurations.append(configuration.copy()) def evaluation_function(self, matrix): """ The error is computed by adding the square error for each configuration. """ error = 0 #rebuild the matrix as a matrix, not a vector (the implementation of the simplex in scipy works on vectors matrix_tmp = [] for i in range(len(self.thumb_glove)): line = [] for j in range(len(self.thumb_hand)): line.append(matrix[i*len(self.thumb_hand)+j]) matrix_tmp.append(line) matrix = matrix_tmp computed_vector_hand = [] for config in self.configurations: #get the hand vectors from the glove data for vec_glove in config.glove_data: computed_vector_hand.append(self.multiply(vec_glove, matrix)) #compute the square error for vec_comp, vec_hand in zip(computed_vector_hand, config.hand_data): for computed_hand_data in vec_comp: for hand_data in vec_hand: error += (computed_hand_data-hand_data)*(computed_hand_data-hand_data) if self.verbose == 1: print "error: "+str(error) if error < self.minerror: self.minerror = error if error > self.maxerror: self.maxerror = error return error def multiply(self, vector_glove, matrix): """ multiply the vector by the matrix. Returns a vector. """ vector_hand = [] index_col = 0 print vector_glove print matrix for glove_data in vector_glove: data = 0 for line in matrix: data += (glove_data*line[index_col]) vector_hand.append(data) index_col += 1 return vector_hand def callback(self, xk): """ Function called at each iteration """ self.simplex_iteration_index += 1 if self.simplex_iteration_index % 50 == 0: print "-------------------------" print "iteration number: "+ str(self.simplex_iteration_index) #print xk def minimize(self): start = [] for i in range(0, len(self.thumb_hand)): line = [] for j in range(0, len(self.thumb_glove)): line.append(0.1) start.append(line) xopt = fmin(self.evaluation_function, start, callback=self.callback, maxiter = 5000) #rebuild the result as a matrix, not a vector (the implementation of the simplex in scipy works on vectors output = [] for i in range(len(self.thumb_glove)): line = [] for j in range(len(self.thumb_hand)): line.append(xopt[i*len(self.thumb_hand)+j]) output.append(line) print "min error: "+ str(self.minerror) print "max error: "+ str(self.maxerror) return output def write_full_mapping(self, output_path = "../../../param/GloveToHandMappings"): """ Writes the mapping matrix to a file: the glove values are the lines, the hand values are the columns """ #initialise the matrix with 0s mapping_matrix = [] for i in range(0, len(self.glove_name_index_map)): line = [] for j in range(0, len(self.hand_name_index_map)): line.append(0.0) mapping_matrix.append(line) # TH3 is always 0, RF4 as well #fill the matrix with the known values + th1 (all except the thumb) mapping_matrix[self.glove_name_index_map["G_IndexDIJ"]][self.hand_name_index_map["FF0"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_IndexPIJ"]][self.hand_name_index_map["FF0"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_IndexMPJ"]][self.hand_name_index_map["FF3"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_MiddleIndexAb"]][self.hand_name_index_map["FF4"]] = -1.0 mapping_matrix[self.glove_name_index_map["G_MiddleDIJ"]][self.hand_name_index_map["MF0"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_MiddlePIJ"]][self.hand_name_index_map["MF0"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_MiddleMPJ"]][self.hand_name_index_map["MF3"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_RingMiddleAb"]][self.hand_name_index_map["MF4"]] = -1.0 mapping_matrix[self.glove_name_index_map["G_RingDIJ"]][self.hand_name_index_map["RF0"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_RingPIJ"]][self.hand_name_index_map["RF0"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_RingMPJ"]][self.hand_name_index_map["RF3"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_PinkieDIJ"]][self.hand_name_index_map["LF0"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_PinkiePIJ"]][self.hand_name_index_map["LF0"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_PinkieMPJ"]][self.hand_name_index_map["LF3"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_PinkieRingAb"]][self.hand_name_index_map["LF4"]] = -1.0 mapping_matrix[self.glove_name_index_map["G_PalmArch"]][self.hand_name_index_map["LF5"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_WristPitch"]][self.hand_name_index_map["WR1"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_WristYaw"]][self.hand_name_index_map["WR2"]] = 1.0 mapping_matrix[self.glove_name_index_map["G_ThumbIJ"]][self.hand_name_index_map["TH1"]] = 1.0 #mapping_matrix[self.glove_name_index_map["G_ThumbAb"]][self.hand_name_index_map["TH4"]] = 1.0 #mapping_matrix[self.glove_name_index_map["G_ThumbRotate"]][self.hand_name_index_map["TH5"]] = 1.0 #mapping_matrix[self.glove_name_index_map["G_ThumbMPJ"]][self.hand_name_index_map["TH2"]] = 1.0 #compute the best mapping for the thumb except th1 thumb_mapping = self.minimize() #fill the matrix with the thumb values computed with the simplex for glove_name in self.thumb_glove.keys(): for hand_name in self.thumb_hand.keys(): #indexes in the computed thumb mapping matrix tmp_index_glove = self.thumb_glove[glove_name] tmp_index_hand = self.thumb_hand[hand_name] #retrieve the mapping value for those sensors mapping_value = thumb_mapping[tmp_index_glove][tmp_index_hand] #indexes for the whole mapping matrix final_index_glove = self.glove_name_index_map[glove_name] final_index_hand = self.hand_name_index_map[hand_name] #update matrix mapping_matrix[final_index_glove][final_index_hand] = mapping_value #write the matrix to a file file = open(output_path, "w") for line in mapping_matrix: for col in line: file.write(" "+str(col) ) file.write("\n") file.close() def record(self, config): for i in range(0, 2): raw_input(str(i) + ": " + config.description) vector_data = [0] * len(self.thumb_glove) for sensor_name in self.thumb_glove.keys(): data = self.cyberglove.read_calibrated_average_value(sensor_name) vector_data[self.thumb_glove[sensor_name]] = data config.glove_data.append(vector_data) if(self.verbose == 1): print "read values:" print config.glove_data