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 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)