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) scenario = ASG( self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, self.sigma, showRender=False, ) scenario.apply_impulse_and_run(action) solved = scenario.solved self.simulation_counter += 1 traj = scenario.find_man_traj() b2contacts = scenario.find_contacts_with_mobile_objs() 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 solve_with_rules_classify(self): 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: 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): # print " ", " bounce1:", bounce_pos[i], " bounce2: ", bounce_pos[i+1], self.zone_distance[(bounce_pos[i], bounce_pos[i+1])], # total_distance = total_distance + self.zone_distance[(bounce_pos[i], bounce_pos[i+1])] # total_distance = total_distance + self.zone_distance[(bounce_pos[i], bounce_pos[i+1])] * pow(r,len(bounce_pos) - i) total_distance += self.zone_distance[(bounce_pos[i], bounce_pos[i + 1])] * pow(1 + r, i) # average_distance = average_distance + total_distance/len(bounce_pos) average_distance = 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 = 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 = self.__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) # print action, ' ', path if solved: detected_sols.append(action) print "solution detected: ", action, " ", self.simulation_counter 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 += 1 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 = ASG( self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, self.sigma, showRender=False, ) scenario.apply_impulse_and_run(_action) self.simulation_counter += 1 # print "sample action: ", _action if scenario.solved: detected_sols.append(action) print "solution detected ", _action, " ", self.simulation_counter 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] 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 print "density: ", self.evalPath.eval(1000, impulse_range)