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)
