def labelTestingDataDisplacements(user_exp_tuples, num_demos, model):
#learn model
gm_model = model #learnBestMixtureModel(user_exp_tuples, num_demos)
#write model to file
filename = "../data/gmm_params.yaml"
writeModelToYaml(gm_model, filename)
#print "learning gmm components"
#write displacement yaml files
for user_id, exp_id in user_exp_tuples:
#print "user", user_id
for demo_id in range(num_demos):
#find appropriate yaml file
yaml_file = lfd.getYamlFile(user_id, exp_id, demo_id)
#grab displacements
disp_list = lfd.getDisplacementList(yaml_file, user_id, exp_id)
mixture_labels = []
labels = gm_model.predict(np.array(disp_list))
#label displacements
for label in labels:
mixture_labels.append([int(label)])
#print "mixture labels", mixture_labels
filename = "../data/user_" + str(user_id) + "/experiment_" + str(
exp_id) + "/displacements" + str(demo_id) + ".yaml"
#print filename
stream = open(filename, 'w')
data = {}
data['objects'] = mixture_labels
#print data
yaml.dump(data, stream)
def averageDistanceToPointError(placement, user_id, exp_id, max_demos, total_demos):
sum_errors = 0.0
#for each demo find location of shape_color
for demo_id in range(max_demos, total_demos):
yaml_file = lfd.getYamlFile(user_id, exp_id, demo_id)
true_coord = lfd.getPlacementCoordinates(yaml_file, user_id, exp_id)
#print np.array(landmark_coord)
#print np.array(displacement)
#add displacement
pred_coord = np.array(placement)
#compare displacement to actual human placement
sum_errors += np.linalg.norm(pred_coord - true_coord)
return sum_errors / float(total_demos - max_demos)
def getPredictedPlacementLocation(train_data, user_id, exp_id, num_demos, test_demo_id):
thresh = 125
#learn gmm
print( "--learning gmm components--")
gmm.labelTrainingDataDisplacements(train_data, 10)
#learn groundings with training data
print( "--grounding language--")
nlp_grounder = nlp()
nlp_grounder.train(train_data)
print( "--generating placement position--")
nlp_landmark, nlp_displacement = getNLP_LfD_LandmarkDisplacementDoubleCheck(nlp_grounder, user_id, exp_id, num_demos, thresh)
yaml_file = lfd.getYamlFile(user_id, exp_id, test_demo_id)
landmark_coord = lfd.getFeatureCoordinates(nlp_landmark, yaml_file)
#print "landmark", landmark, landmark_coord
#print "displacement", disp_mus[landmark]
pos = np.round(np.array(landmark_coord) + np.array(nlp_displacement))
return pos.tolist()
def __init__(self, user_id=None, exp_id=None, demo_id=None):
self.shape_set = set()
self.color_set = set()
self.displacement_set = set()
if user_id is not None and exp_id is not None and demo_id is not None:
yaml_file = lfd.getYamlFile(user_id, exp_id, demo_id)
#print yaml_file
#get world data from yaml_file
data = lfd.getYamlData(yaml_file)
#print data
#add objects on table
for obj in data['objects']:
self.shape_set.add(obj[3])
self.color_set.add(obj[4])
#add task object
self.shape_set.add(data['task_object'][1])
self.color_set.add(data['task_object'][2])
#add displacements
displacements = getDisplacements(user_id, exp_id, demo_id)
for d in displacements:
self.displacement_set.add(d[0])
def getNLP_LfD_LandmarkDisplacement(nlp_grounder, user_id, exp_id, n_demos):
relation_offset = 11 #hard coded offset to get back to relations in range 0:8
relation, object_info = nlp_grounder.predict_goal(user_id, exp_id, n_demos)
print(len(relation), len(object_info))
print(relation)
#get lfd to use along with language or in place of language if ambiguous
lfd_shape_color, lfd_displacement = lfd.getMostLikelyLandmarkDisplacement(
user_id, exp_id, n_demos)
#check if we have one relation and one object
if len(relation) == 1 and len(object_info) == 1:
#print "grounding both"
#first check if nlp grounded landmark is actually a possible landmark
nlp_shape_color = (object_info[0][3], object_info[0][4])
yaml_file = lfd.getYamlFile(user_id, exp_id, 0)
landmark_coord = lfd.getFeatureCoordinates(nlp_shape_color, yaml_file)
if landmark_coord is None:
#print "NLP returned invalid landmark"
return lfd_shape_color, lfd_displacement
else: #use valid landmark and displacement to guess placement location
#get shape,color
nlp_shape_color = (object_info[0][3], object_info[0][4])
#get displacement from learned gmm
gmm_filename = "../data/gmm_params.yaml"
model_data = lfd.getYamlData(gmm_filename)
nlp_displacement = model_data['mu'][relation[0] - relation_offset]
return nlp_shape_color, nlp_displacement
#check if we have only one object and can ground on that
elif len(object_info) == 1:
#print "grounding on object only"
nlp_shape_color = (object_info[0][3], object_info[0][4])
nlp_displacement = lfd.getDisplacementFromLandmark(
user_id, exp_id, n_demos, nlp_shape_color)
return nlp_shape_color, nlp_displacement
#if we have one relationship and multiple items, figure out which item by using relationship
elif len(relation) == 1 and len(object_info) > 0:
#print "grounding on relation only"
#get predicted displacement based on relationship
gmm_filename = "../data/gmm_params.yaml"
model_data = lfd.getYamlData(gmm_filename)
nlp_displacement = model_data['mu'][relation[0] - relation_offset]
#get placement data and object locations
#TODO which demo should I pick from, what should n_demos be??
yaml_file = lfd.getYamlFile(user_id, exp_id, n_demos)
placement_coord = lfd.getPlacementCoordinates(yaml_file, user_id,
exp_id)
min_dist = 10000
for obj in object_info:
shape_color = (obj[3], obj[4])
landmark_coord = lfd.getFeatureCoordinates(shape_color, yaml_file)
predicted_placement = np.array(landmark_coord) + np.array(
nlp_displacement)
placement_error = np.linalg.norm(predicted_placement -
placement_coord)
if placement_error < min_dist:
min_dist = placement_error
best_landmark = shape_color
return best_landmark, nlp_displacement
#TODO case where only relationship is grounded?
#elif len(relation) == 1:
#fall back on pure lfd as a last resort
else:
#print "ambiguous grounding"
return lfd_shape_color, lfd_displacement
def getNLP_LfD_LandmarkDisplacementDoubleCheck(nlp_grounder, user_id, exp_id,
n_demos, thresh):
relation_offset = 11 #hard coded offset to get back to relations in range 0:8
relation, object_info = nlp_grounder.predict_goal(user_id, exp_id, n_demos)
#print len(relation), len(object_info)
#print relation
#get lfd to use along with language or in place of language if ambiguous
lfd_shape_color, lfd_displacement = lfd.getMostLikelyLandmarkDisplacement(
user_id, exp_id, n_demos)
#check if we have one relation and one object
grounded_prediction = False #flag to check if we think we've found a grounding
if len(relation) == 1 and len(object_info) == 1:
#print "grounding both"
#first check if nlp grounded landmark is actually a possible landmark
nlp_shape_color = (object_info[0][3], object_info[0][4])
yaml_file = lfd.getYamlFile(user_id, exp_id, 0)
landmark_coord = lfd.getFeatureCoordinates(nlp_shape_color, yaml_file)
if landmark_coord is not None:
#get shape,color
nlp_shape_color = (object_info[0][3], object_info[0][4])
#get displacement from learned gmm
gmm_filename = "../data/gmm_params.yaml"
model_data = lfd.getYamlData(gmm_filename)
nlp_displacement = model_data['mu'][relation[0] - relation_offset]
grounded_prediction = True
#check if we have only one object and can ground on that
elif len(object_info) == 1:
#print "grounding on object only"
nlp_shape_color = (object_info[0][3], object_info[0][4])
nlp_displacement = lfd.getDisplacementFromLandmark(
user_id, exp_id, n_demos, nlp_shape_color)
grounded_prediction = True
#if we have one relationship and multiple items, figure out which item by using relationship
elif len(relation) == 1 and len(object_info) > 0:
#print "grounding on relation only"
#get predicted displacement based on relationship
gmm_filename = "../data/gmm_params.yaml"
model_data = lfd.getYamlData(gmm_filename)
nlp_displacement = model_data['mu'][relation[0] - relation_offset]
#get placement data and object locations
#TODO which demo should I pick from, what should n_demos be??
yaml_file = lfd.getYamlFile(user_id, exp_id, n_demos)
placement_coord = lfd.getPlacementCoordinates(yaml_file, user_id,
exp_id)
min_dist = 10000
for obj in object_info:
shape_color = (obj[3], obj[4])
landmark_coord = lfd.getFeatureCoordinates(shape_color, yaml_file)
predicted_placement = np.array(landmark_coord) + np.array(
nlp_displacement)
placement_error = np.linalg.norm(predicted_placement -
placement_coord)
if placement_error < min_dist:
min_dist = placement_error
best_landmark = shape_color
nlp_shape_color = best_landmark
grounded_prediction = True
#TODO case where only relationship is grounded?
#elif len(relation) == 1:
#see if I have a grounded prediction or if I should go with pure lfd
if grounded_prediction:
#check if grounding prediction seems to match what's been demonstrated
ave_error = averageDistanceError(nlp_shape_color, nlp_displacement,
user_id, exp_id, 0, n_demos + 1)
#print "ave error", ave_error
if ave_error < thresh:
#print "confident in grounding"
return nlp_shape_color, nlp_displacement, relation, object_info
#fall back on pure lfd as a last resort
#print "ambiguous grounding"
return lfd_shape_color, lfd_displacement, relation, object_info
def experiment(train_data, test_data):
total_demos = 10 #total number of demos possible
max_demos = 4 #maximum number of demos given to robot to learn from (the rest are used to test generalizabiltiy)
thresh = 150
#TODO learn the gmm componenets and label the data
print("--learning gmm components--")
gmm_model = gmm.labelTrainingDataDisplacements(train_data, total_demos)
gmm.labelTestingDataDisplacements(test_data, total_demos, gmm_model)
#learn groundings with training data
print("--grounding language--")
nlp_grounder = nlp()
nlp_grounder.train(train_data)
######################
#testing code
######################
print("--testing generalization error--")
#matrix of zeros where each row is new test case and cols are ave errors for learning from 1:max_demos demonstrations
lfd_errors = np.zeros((len(test_data), max_demos))
nlp_errors = np.zeros((len(test_data), max_demos))
random_errors = np.zeros((len(test_data), max_demos))
aveplace_errors = np.zeros((len(test_data), max_demos))
lfd_correct = np.zeros((3, max_demos))
nlp_correct = np.zeros((3, max_demos))
nlp_lfd_correct = np.zeros((3, max_demos))
"""lfd_spatial_correct = np.zeros(max_demos)
lfd_object_correct = np.zeros(max_demos)
lfd_both_correct = np.zeros(max_demos)
nlp_spatial_correct = np.zeros(max_demos)
nlp_object_correct = np.zeros(max_demos)
nlp_both_correct = np.zeros(max_demos)
nlp_lfd_spatial_correct = np.zeros(max_demos)
nlp_lfd_object_correct = np.zeros(max_demos)
nlp_lfd_both_correct = np.zeros(max_demos)"""
for i in range(len(test_data)):
user_id, exp_id = test_data[i]
for n_demos in range(1, max_demos + 1):
#get best guess of landmark and displacement using pure LfD
lfd_shape_color, lfd_displacement = lfd.getMostLikelyLandmarkDisplacement(
user_id, exp_id, n_demos)
#guess landmark and displacement using NLP+LfD
nlp_lfd_shape_color, nlp_displacement, nlp_relation, nlp_shape_color = nlp_lfd.getNLP_LfD_LandmarkDisplacementDoubleCheck(
nlp_grounder, user_id, exp_id, n_demos, thresh)
world = lfd.getYamlFile(user_id, exp_id, 0)
lfd_relation = gmm_model.predict(lfd_displacement)[0]
nlp_lfd_relation = gmm_model.predict(nlp_displacement)[0]
with open(world, 'r') as f:
world_file = yaml.load(f)
landmark = world_file["objects"][0]
with open(
"../data/user_" + str(user_id) + "/experiment_" +
str(exp_id) + "/displacements0.yaml", 'r') as g:
disp_file = yaml.load(g)
disp = disp_file["objects"][0]
if len(nlp_shape_color
) == 1 and nlp_shape_color[0][3] == landmark[
3] and nlp_shape_color[0][4] == landmark[4]:
nlp_correct[0][n_demos -
1] = nlp_correct[0][n_demos - 1] + 1
if len(nlp_relation
) == 1 and nlp_relation[0] - 11 == disp[0]:
nlp_correct[2][n_demos -
1] = nlp_correct[2][n_demos - 1] + 1
if len(nlp_relation) == 1 and nlp_relation[0] - 11 == disp[0]:
nlp_correct[1][n_demos -
1] = nlp_correct[1][n_demos - 1] + 1
if lfd_shape_color[0] == landmark[3] and lfd_shape_color[
1] == landmark[4]:
lfd_correct[0][n_demos -
1] = lfd_correct[0][n_demos - 1] + 1
if lfd_relation == disp[0]:
lfd_correct[2][n_demos -
1] = lfd_correct[2][n_demos - 1] + 1
if lfd_relation == disp[0]:
lfd_correct[1][n_demos -
1] = lfd_correct[1][n_demos - 1] + 1
if nlp_lfd_shape_color[0] == landmark[
3] and nlp_lfd_shape_color[1] == landmark[4]:
nlp_lfd_correct[0][n_demos -
1] = nlp_lfd_correct[0][n_demos - 1] + 1
if nlp_lfd_relation == disp[0]:
nlp_lfd_correct[2][n_demos -
1] = nlp_lfd_correct[2][n_demos -
1] + 1
if nlp_lfd_relation == disp[0]:
nlp_lfd_correct[1][n_demos -
1] = nlp_lfd_correct[1][n_demos - 1] + 1
#guess placment randomly
rand_placement = lfd.getRandomPointOnTable(user_id, exp_id,
n_demos)
#guess placement as average of demonstrated placements
ave_placement = lfd.getMeanPlacementCoordinates(
user_id, exp_id, n_demos)
#compute accuracy over a test demo specified by demo_id
for demo_id in range(max_demos, total_demos):
#pure lfd error
lfd_errors[i, n_demos - 1] = nlp_lfd.averageDistanceError(
lfd_shape_color, lfd_displacement, user_id, exp_id,
max_demos, total_demos)
#nlp+lfd error
nlp_errors[i, n_demos - 1] = nlp_lfd.averageDistanceError(
nlp_lfd_shape_color, nlp_displacement, user_id, exp_id,
max_demos, total_demos)
#random baseline error
random_errors[i, n_demos -
1] = nlp_lfd.averageDistanceToPointError(
rand_placement, user_id, exp_id, max_demos,
total_demos)
#average placement pos baseline error
aveplace_errors[i, n_demos -
1] = nlp_lfd.averageDistanceToPointError(
ave_placement, user_id, exp_id, max_demos,
total_demos)
return (lfd_correct, nlp_correct, nlp_lfd_correct)
