class SolverAdjust: def __init__(self, scenario_file, perturb=False, gravity_dir=(-1,-1)): self.perturb = perturb self.gravity_dir = gravity_dir # create scenario self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj = loadScenario( scenario_file) self.scenario_file = scenario_file print 'solving scenario: ', scenario_file print 'gravity internal sim: ', gravity_dir self.evalReal = EvaluateQualiSol(scenario_file, gravity_dir) #self.evalReal = EvaluateQualiSol('./scenarios/mini2.json') #self.evalReal = EvaluateQualiSol('./scenarios/s2.json') self.objs_map = {} self.obj_min_angle_difference = {} self.first_real_sol = -1 for obj in self.immobile_objs: if perturb: perturb_obj(obj) self.objs_map[obj['id']] = obj #self.obj_min_angle_difference[obj['id']] = [100,100] self.obj_min_angle_difference[obj['id']] = [100, 0] if perturb: sps(scenario_file, 5, self.immobile_objs) self.target_obj_id = self.target_obj['id'] self.maxd = 3 self.scenario = None self.qualitative_paths = {} self.qualitative_paths_actions = {} self.zones = [] self.simulation_counter = 0 self.round = 0 quali_sim = qualitative_simulation(scenario_file) self.estimated_qualitative_paths = quali_sim.run() self.initial_zone = quali_sim.initial_zone self.graph = quali_sim.graph self.zones = quali_sim.zones self.zone_dic = {} self.zone_distance = {} for x in xrange(self.width): for y in xrange(self.height): self.zone_dic[(x, y)] = self.__find_zones(x, y) for i in xrange(len(self.zones) - 1): self.zone_distance[(i, i)] = 0 for j in xrange(i + 1, len(self.zones)): distance = self.zones[i].distance(self.zones[j]) self.zone_distance[(i, j)] = distance self.zone_distance[(j, i)] = distance self.zone_distance[(len(self.zones) - 1, len(self.zones) - 1)] = 0 def __find_zones(self, x, y): for i in xrange(len(self.zones)): if self.zones[i].contains(Point(x, y)): return i return -1 def show_scenario(self): scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, showRender=True) scenario.run() return def new_scenario(self): self.scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, gravity_dir=self.gravity_dir, showRender=False) def solve_with_rules_classify(self): self.round += 1 all_paths = [] classification = {} essential_contacts = set([]) quali_paths = [] sectors_score = [] path_by_dir = {} path_bounces = {} path_first_bounces = {} possible_impulse_ranges = [] for path_dir, path, essential_contact, bounce_pos_list in self.estimated_qualitative_paths: # print bounces_pos # heappush(all_paths, (len(bounces_pos), (path, bounces_pos))) # print path_dir comp_path = self.make_path_complete(path, self.graph) essential_contacts.add(essential_contact) if comp_path not in quali_paths: quali_paths.append(comp_path) if path_dir not in path_by_dir: path_by_dir[path_dir] = [comp_path] if len(bounce_pos_list) > 0: path_bounces[path_dir] = [bounce_pos_list] path_first_bounces[path_dir] = set([bounce_pos_list[0]]) else: path_first_bounces[path_dir] = set([]) path_bounces[path_dir] = [] else: path_by_dir[path_dir].append(comp_path) if len(bounce_pos_list) > 0: path_bounces[path_dir].append(bounce_pos_list) path_first_bounces[path_dir].add(bounce_pos_list[0]) sort_dirs = [] for path_dir in path_bounces: # calculate average bounce distance bounce_pos_list = path_bounces[path_dir] average_distance = 0 for bounce_pos in bounce_pos_list: total_distance = 0 r = 0.6 for i in xrange(len(bounce_pos) - 1): total_distance += self.zone_distance[(bounce_pos[i], bounce_pos[i + 1])] * pow(1 + r, i) average_distance += total_distance if len(bounce_pos_list) == 0: average_distance = 0 else: average_distance /= len(bounce_pos_list) print "path_dir: ", path_dir, " ", average_distance heappush(sort_dirs, (average_distance, path_dir)) ''' while all_paths: path, bounces_pos = heappop(all_paths) print path, " bounces: ", bounces_pos ''' while sort_dirs: # for path_dir in path_by_dir: distance, path_dir = heappop(sort_dirs) print "Test dir range: ", path_dir, " distance ", distance # bounces_count = 0 # for bounces_pos in path_bounces[path_dir]: # print bounces_pos # subdivide path_dir into 10 sectors # quali_paths = path_by_dir[path_dir] divided_sectors = self.divide_dir(path_dir) use_less_path = False num_iter = 15 #10 # 4 detected_sols = [] while num_iter > 0 and not use_less_path: num_iter -= 1 bounces_count = 0 for sector in divided_sectors: num_samples = 10 impulse_range = (IMPULSE_RANGE_X, sector) actions = sample_n_points_from_range(num_samples, impulse_range) # print "test sector: ", sector # print actions for action in actions: path, contacts_info, solved = self.find_qualitative_path_ptlike(action, self.initial_zone) if solved: print "solution in approx sim: ", action real_traj, real_contacts, real_contacts_objs, real_solved = self.evalReal.trialshot_real( action) detected_sols.append(action) if real_solved: #print ' real: ', self.first_real_sol, ' not perturb: ', not self.perturb if self.first_real_sol == -1: if not self.perturb: self.first_real_sol = self.simulation_counter print ' detect first real sol: ', action, self.simulation_counter exit() else: self.first_real_sol = self.evalReal.count print "solution in real evnironment!!!!!", action else: # adjust #self.adjust_approx_sim(real_traj, real_contacts, real_contacts_objs, action) self.adjust_approx_sim_qualitative_path(real_traj, real_contacts, real_contacts_objs, action) ''' real_qualitative_path = self.compute_qualitative_path(real_traj, self.initial_zone) print "solution detected: ", action, " ", self.simulation_counter print "expected qualitative path: ", path, contacts_info print "real qualitative path: ", real_qualitative_path, real_contacts ''' continue path = self.make_path_complete(path, self.graph) for first_bounces in path_first_bounces[path_dir]: if first_bounces in path: bounces_count += bounces_count for contact in contacts_info: if contact[0] in essential_contacts: path_str = str(self.make_path_complete(path, self.graph)).strip('[]') # print "path after: " ,path print "perturb_action: ", action max_mu = self.perturb_action(action, path_str, essential_contact) mu_list = np.random.normal(max_mu, 20, 100) for mu in mu_list: _action = (mu, action[1]) scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, showRender=False) scenario.apply_impulse_and_run(_action) self.simulation_counter += 1 # print "sample action: ", _action if scenario.solved: print "solution detected in approx ", _action, " ", self.simulation_counter detected_sols.append(action) real_traj, real_contacts, real_contacts_objs, real_solved = self.evalReal.trialshot_real( action) if real_solved: if self.first_real_sol == -1: if not self.perturb: self.first_real_sol = self.simulation_counter print ' detect first real sol: ', self.simulation_counter print exit() else: self.first_real_sol = self.evalReal.count print "solution in real environment !!! ", action break else: # adjust #self.adjust_approx_sim(real_traj, real_contacts, real_contacts_objs, action) self.adjust_approx_sim_qualitative_path(real_traj, real_contacts, real_contacts_objs, action) break ''' if bounces_count == 0: print "exit at: ", 10 - num_iter use_less_path = True ''' if len(detected_sols) >= 1: min_mu = 5000 max_mu = 0 min_angle = 7 max_angle = 0 for sol in detected_sols: if sol[0] > max_mu: max_mu = sol[0] if sol[0] < min_mu: min_mu = sol[0] if sol[1] > max_angle: max_angle = sol[1] if sol[1] < min_angle: min_angle = sol[1] if max_angle - max_angle < 0.1: min_angle -= 0.1 max_angle += 0.1 mu_range = (min_mu - 100, max_mu + 100) eval_impulse = [mu_range, (min_angle, max_angle)] possible_impulse_ranges.append(eval_impulse) for impulse_range in possible_impulse_ranges: print 'eval: ', impulse_range density, shots = self.evalReal.eval(1000, impulse_range) print 'density: ', density, 'first sol: ', shots , ' total shots: ', self.evalReal.count + shots if shots != -1 and self.first_real_sol == -1: self.first_real_sol = shots print 'num of trial shots: ', self.evalReal.count print 'simulation steps for finding the first sol: ', self.first_real_sol if self.first_real_sol == -1: if self.round >= 2: exit() gc.collect() self.simulation_counter = 0 self.perturb = False self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj = loadScenario( scenario_file) self.solve_with_rules_classify() if self.perturb: sps(self.scenario_file, 6, self.immobile_objs) def divide_dir(self, path_dir): num_sectors = 20 min_angle, max_angle = path_dir difference = (max_angle - min_angle) / num_sectors divided_sectors = [(min_angle + i * difference, min_angle + (i + 1) * difference) for i in xrange(num_sectors)] # print "divided sectors: ", divided_sectors return divided_sectors # return the most similar path (least edit distance) def least_distance(self, path, paths): min_d = 999 # min_path = [] for quali_path in paths: distance = editdistance.eval(path[:10], quali_path[:10]) # distance = editdistance.eval(path, quali_path) if distance < min_d: min_d = distance # min_path = quali_path # print path, " ", min_path, "distance: ", min_d return min_d def similar_path(self, path_1, paths): min_d = 999 result = path_1 for path_2 in paths: distance = editdistance.eval(path_1[:len(path_2)], path_2) if distance < min_d: min_d = distance result = path_2 if min_d < 3: return path_2 else: return None # maybe not necessary def make_path_complete(self, path, graph): new_path = [] for i in xrange(len(path) - 1): new_path.append(path[i]) if path[i + 1] == -1: break neighbors = graph.neighbors(path[i + 1]) if path[i] not in neighbors and path[i] != -1: shortest_path = nx.shortest_path(graph, source=path[i], target=path[i + 1]) # only retain the intermediate nodes (which are missing) along the path. new_path = new_path + shortest_path[1:-1] new_path.append(path[-1]) return new_path def compute_qualitative_path(self, traj, initial_zone): pre_zone = initial_zone path = [initial_zone] for traj_pt in traj: x, y = traj_pt x = int(x) y = int(y) if (x, y) not in self.zone_dic: occupied_zone = -1 else: occupied_zone = self.zone_dic[(x, y)] # if out of scope, still wait to see if it will come back, quite slow if occupied_zone == -1 or occupied_zone == pre_zone: continue path.append(occupied_zone) pre_zone = occupied_zone return path def find_qualitative_path_ptlike(self, action, initial_zone): # scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs,self.manipulatable_obj, self.target_obj, showRender=False) self.new_scenario() self.scenario.apply_impulse_and_run(action) solved = self.scenario.solved ''' if solved: scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, showRender=False) scenario.apply_impulse_and_run(action) ''' self.simulation_counter += 1 traj = self.scenario.find_man_traj() b2contacts = self.scenario.find_contacts_with_mobile_objs() # print contacts pre_zone = initial_zone path = [initial_zone] for traj_pt in traj: x, y = traj_pt x = int(x) y = int(y) if (x, y) not in self.zone_dic: occupied_zone = -1 else: occupied_zone = self.zone_dic[(x, y)] # if out of scope, still wait to see if it will come back, quite slow if occupied_zone == -1 or occupied_zone == pre_zone: continue path.append(occupied_zone) pre_zone = occupied_zone return path, b2contacts, solved def find_qualitative_path(self, traj): pre_zone = self.initial_zone path = [self.initial_zone] for traj_pt in traj: x, y = traj_pt x = int(x) y = int(y) if (x, y) not in self.zone_dic: occupied_zone = -1 else: occupied_zone = self.zone_dic[(x, y)] # if out of scope, still wait to see if it will come back, quite slow if occupied_zone == -1 or occupied_zone == pre_zone: continue path.append(occupied_zone) pre_zone = occupied_zone return path def bounce_similarity(self, es_bounces, real_bounces): bounce_differ_inx = 0 for i, ind in enumerate(real_bounces): if i == len(es_bounces): break if ind != es_bounces[i]: bounce_differ_inx = i return bounce_differ_inx return bounce_differ_inx def perturb_action(self, action, root_path, essential_contact): mu, theta = action mu_step_size = 50 num_runs = 100 max_mu = IMPULSE_RANGE_X[-1] current_mu = mu # print "perturb_action: ", action while num_runs > 0 and mu_step_size > 0.005: num_runs -= 1 action = (current_mu + mu_step_size, theta) path, contacts_info, solved = self.find_qualitative_path_ptlike(action, self.initial_zone) essential_contact_detected = False for contact in contacts_info: if contact[0] == essential_contact: essential_contact_detected = True if solved: print "solution detected when perturbing: ", action, " ", self.simulation_counter return current_mu + mu_step_size if not essential_contact_detected: continue path = str(self.make_path_complete(path, self.graph)).strip('[]') # print path, " ", action, " ", root_path in path, " max_mu: ", max_mu # if path != root_path: if root_path not in path: # qualitative turning point if max_mu > current_mu + mu_step_size: max_mu = current_mu + mu_step_size mu_step_size /= 2 else: if current_mu + mu_step_size >= IMPULSE_RANGE_X[-1]: max_mu = IMPULSE_RANGE_X[-1] break elif current_mu + mu_step_size > max_mu: max_mu = current_mu + mu_step_size current_mu += mu_step_size mu_step_size = mu_step_size + mu_step_size return max_mu def adjust_approx_sim_qualitative_path(self, real_traj, real_bounces, real_contacted_objs, action=None): es_traj, es_bounces, es_contacted_objs = self.scenario.find_man_traj_bounce() es_num_of_bounces = len(es_contacted_objs) real_num_of_bounces = len(real_contacted_objs) real_path = self.find_qualitative_path(real_traj) real_path = str(real_path).strip('[]') real_bounces_str = str(real_bounces).strip('[]') #print "es bounce: ", es_bounces, " es_contacted ", es_contacted_objs, " real bounces: ", real_bounces, " real_contacted: ", real_contacted_objs for i in xrange(es_num_of_bounces): for j in xrange(real_num_of_bounces): #if i == j and es_contacted_objs[i] == real_contacted_objs[j] and es_contacted_objs[i] != 0: if i==j and es_contacted_objs[i] == real_contacted_objs[j] and es_contacted_objs[i] != 0 and es_contacted_objs[i] in self.objs_map: #if self.obj_min_angle_difference[es_contacted_objs[i]] != [100, 0]: # already adjusted # continue es_path = self.find_qualitative_path(es_traj) es_path = str(es_path).strip('[]') es_bounces_str = str(es_bounces).strip('[]') step = 0.001 obj = self.objs_map[es_contacted_objs[i]] min_obj_angle = obj['angle'] count = 400 original_angle = obj['angle'] path_difference = editdistance.eval(es_path, real_path) #bounce_difference = editdistance.eval(es_bounces_str, real_bounces_str) bounce_similarity = self.bounce_similarity(es_bounces, real_bounces) min_path_difference = path_difference #min_bounce_difference = bounce_difference max_bounce_similarity = bounce_similarity if min_path_difference == 0: print '!!!!! angle: ', obj['angle'], ' path difference: ', path_difference, ' bounce similarity ', bounce_similarity while (path_difference > 0) and count >= 0: count -= 1 self.new_scenario() self.scenario.apply_impulse_and_run(action) _es_traj, _es_bounces, _es_contacted_objs = self.scenario.find_man_traj_bounce() _es_path = self.find_qualitative_path(_es_traj) _es_path = str(_es_path).strip('[]') _es_bounces_str = str(_es_bounces).strip('[]') path_difference = editdistance.eval(_es_path, real_path) bounce_similarity = self.bounce_similarity(_es_bounces, real_bounces) #bounce_difference = editdistance.eval(_es_bounces_str, real_bounces_str) #print _es_bounces_str, ' ||| ', real_bounces_str #print ' path difference ', path_difference, 'bounce difference: ', bounce_difference, ' angle ', obj['angle'] #print ' path difference ', path_difference, 'bounce similairty: ', bounce_similarity, ' angle ', \ obj['angle'] ''' if path_difference <= min_path_difference: if path_difference == min_path_difference: if bounce_difference < min_bounce_difference: min_bounce_difference = bounce_difference min_path_difference = path_difference min_obj_angle = obj['angle'] else: min_path_difference = path_difference min_bounce_difference = bounce_difference min_obj_angle = obj['angle'] ''' if path_difference <= min_path_difference: if path_difference == min_path_difference: if bounce_similarity > max_bounce_similarity: max_bounce_similarity = bounce_similarity min_path_difference = path_difference min_obj_angle = obj['angle'] else: min_path_difference = path_difference max_bounce_similarity = bounce_similarity min_obj_angle = obj['angle'] if count < 200: obj['angle'] += step elif count == 200: obj['angle'] = original_angle + step else: obj['angle'] -= step if min_path_difference == self.obj_min_angle_difference[obj['id']][0] and max_bounce_similarity > \ self.obj_min_angle_difference[obj['id']][1]: self.obj_min_angle_difference[obj['id']] = [min_path_difference, max_bounce_similarity] obj['angle'] = min_obj_angle elif min_path_difference < self.obj_min_angle_difference[obj['id']]: self.obj_min_angle_difference[obj['id']] = [min_path_difference, max_bounce_similarity] obj['angle'] = min_obj_angle ''' if min_path_difference == self.obj_min_angle_difference[obj['id']][0] and min_bounce_difference < self.obj_min_angle_difference[obj['id']][1]: self.obj_min_angle_difference[obj['id']] = [min_path_difference, min_bounce_difference] obj['angle'] = min_obj_angle elif min_path_difference < self.obj_min_angle_difference[obj['id']]: self.obj_min_angle_difference[obj['id']] = [min_path_difference, min_bounce_difference] obj['angle'] = min_obj_angle ''' print 'obj_id: ', obj['id'], 'adjusted angle: ', min_obj_angle, 'path_d: ', min_path_difference, 'bounce_d', max_bounce_similarity, ' final angle: ', obj['angle'], ' final difference: ', self.obj_min_angle_difference[obj['id']] else: continue return def adjust_approx_sim(self, real_traj, real_bounces, real_contacted_objs, action=None): es_traj, es_bounces, es_contacted_objs = self.scenario.find_man_traj_bounce() es_num_of_bounces = len(es_contacted_objs) real_num_of_bounces = len(real_contacted_objs) for i in xrange(es_num_of_bounces): for j in xrange(real_num_of_bounces): if i == j and es_contacted_objs[i] == real_contacted_objs[j] and es_contacted_objs[i] != 0: es_bounce_pt_inx = es_bounces[i] real_bounce_pt_inx = real_bounces[j] #if len(es_traj) - 1 < es_bounce_pt_inx + 2 or real_bounce_pt_inx + 2 > len(real_traj) - 1: # break es_bounces_pts = [es_traj[es_bounce_pt_inx - 1], es_traj[es_bounce_pt_inx], es_traj[es_bounce_pt_inx + 1]] real_bounces_pts = [real_traj[real_bounce_pt_inx - 1], real_traj[real_bounce_pt_inx], real_traj[real_bounce_pt_inx + 1]] angle_difference, translation = Adjust.find_spatial_difference(es_bounces_pts, real_bounces_pts) # step = sigma_angle step = 0.001 if angle_difference > 0: neg_flag = False else: neg_flag = True obj = self.objs_map[es_contacted_objs[i]] #obj['position'] = (obj['position'][0] + translation[0], obj['position'][1] + translation[1]) # last_obj_angle = obj['angle'] current_obj_angle = obj['angle'] last_obj_angle = obj['angle'] last_angle_difference = angle_difference min_angle_difference = 100 min_obj_angle = obj['angle'] count = 400 original_angle = obj['angle'] reversed = False if abs(angle_difference) <= 0.001: print ' !!!!!! obj angle', min_obj_angle, ' angfle difference ', angle_difference while (abs(angle_difference) > 0.001) and count >= 0: count -= 1 # update #print 'angle differ: ', angle_difference, ' obj angle: ', obj['angle'] #print "translation: ", translation #print 'current angle difference: ', angle_difference, " last angle difference", last_angle_difference, " ", obj['angle'] # add a map record the last angle difference recorded for each adjusted object ''' if not reversed: obj['angle'] = last_obj_angle + step last_obj_angle = obj['angle'] last_angle_difference = angle_difference else: obj['angle'] = last_obj_angle - step last_obj_angle = obj['angle'] last_angle_difference = angle_difference ''' ''' if angle_difference < 0: obj['angle'] = last_obj_angle + step last_obj_angle = obj['angle'] last_angle_difference = angle_difference else: obj['angle'] = last_obj_angle - step last_obj_angle = obj['angle'] last_angle_difference = angle_difference ''' ''' print 'angle difference: ', angle_difference, " current: ", current_obj_angle, " last: ", last_obj_angle, " neg flag: ", neg_flag if angle_difference < 0: if not neg_flag: obj['angle'] = (last_obj_angle + current_obj_angle)/2 last_obj_angle = current_obj_angle current_obj_angle = obj['angle'] neg_flag = True else: obj['angle'] += step last_obj_angle = current_obj_angle current_obj_angle = obj['angle'] else: if neg_flag: obj['angle'] = (last_obj_angle + current_obj_angle) / 2 last_obj_angle = current_obj_angle current_obj_angle = obj['angle'] neg_flag = False else: obj['angle'] -= step last_obj_angle = current_obj_angle current_obj_angle = obj['angle'] ''' self.new_scenario() self.scenario.apply_impulse_and_run(action) _es_traj, _es_bounces, _es_contacted_objs = self.scenario.find_man_traj_bounce() _es_bounces_pts = [_es_traj[es_bounce_pt_inx - 1], _es_traj[es_bounce_pt_inx], _es_traj[es_bounce_pt_inx + 1]] angle_difference, translation = Adjust.find_spatial_difference(_es_bounces_pts, real_bounces_pts) if abs(angle_difference) < min_angle_difference: min_angle_difference = abs(angle_difference) min_obj_angle = obj['angle'] if count < 200: obj['angle'] += step elif count == 200: obj['angle'] = original_angle + step else: obj['angle'] -= step ''' if abs(angle_difference) < min_angle_difference: min_angle_difference = abs(angle_difference) min_obj_angle = obj['angle'] #step *= 2 else: reversed = not reversed step /= 2 ''' #if abs(translation[0]) < sigma_pos and abs(translation[1]) < sigma_pos: # obj['position'] = (obj['position'][0] + translation[0], obj['position'][1] + translation[1]) if min_angle_difference < self.obj_min_angle_difference[obj['id']]: obj['angle'] = min_obj_angle self.obj_min_angle_difference[obj['id']] = min_angle_difference print 'adjusted angle: ', min_obj_angle, 'min_angle_difference: ', min_angle_difference, ' final angle: ', obj['angle'], ' final difference: ', self.obj_min_angle_difference[obj['id']] break else: continue return
# nx.draw(graph, pos, node_size=30, node_color='g', edge_color='y') ''' simple_paths = nx.all_simple_paths(graph, source=24, target=8) edges_path = [] for path in simple_paths: edges_path = edges_path + [(path[i], path[i+1]) for i in range(len(path)-1) ] nx.draw_networkx_edges(graph, pos, edgelist=edges_path, edge_color='b', arrows=True) ''' nx.draw_networkx_labels(graph, pos) plt.savefig(file_name + '.png') # use internal simulator scenario.apply_impulse_and_run((4403.079653047002, 1.5393155252923814)) es_traj, es_bounces, es_contact_obj_id = scenario.find_man_traj_bounce() # now use real environment file_name = 's2' scenario_width, scenario_height, immobile_objs, mobile_objs, manipulatable_obj, target_obj = loadScenario( './scenarios/' + file_name + '.json') scenario = SG(scenario_width, scenario_height, immobile_objs, mobile_objs, manipulatable_obj, target_obj, showRender=False) scenario.apply_impulse_and_run((4403.079653047002, 1.5393155252923814)) real_traj, real_bounces, real_contact_obj_id = scenario.find_man_traj_bounce() bounce_x_list = [] bounce_y_list = [] for inx in real_bounces: x, y = real_traj[inx] # print x, y bounce_x_list.append(x) bounce_y_list.append(y)