cyberglove_mapper.py

Go to the documentation of this file.

#!/usr/bin/env python
#
# Copyright 2011 Shadow Robot Company Ltd.
#
# This program is free software: you can redistribute it and/or modify it
# under the terms of the GNU General Public License as published by the Free
# Software Foundation, either version 2 of the License, or (at your option)
# any later version.
#
# This program is distributed in the hope that it will be useful, but WITHOUT
# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
# FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
# more details.
#
# You should have received a copy of the GNU General Public License along
# with this program.  If not, see <http://www.gnu.org/licenses/>.
#
"""
Minimizes the mapping matrix using a simplex algorithm

@author: Ugo Cupcic
@contact: ugo@shadowrobot.com, contact@shadowrobot.com
"""

import roslib

import rospy
from scipy.optimize import fmin
from cyberglove_library import Cyberglove


class MappingConfiguration:

    def __init__(self, glove_data=[], hand_data=[], description=""):
        self.glove_data = glove_data
        self.hand_data = hand_data
        self.description = description

    def copy(self):
        tmp = MappingConfiguration(
            self.glove_data, self.hand_data, self.description)
        return tmp


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


#
# MAIN    #
#
def main():
    cyber_mapper = MappingMinimizer()

    for config in cyber_mapper.configurations:
        cyber_mapper.record(config)

    cyber_mapper.write_full_mapping()

    return 0


# start the script
if __name__ == "__main__":
    main()