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