def find_qualitative_path_ptlike(self, action, zones, initial_zone):
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)
traj = scenario.find_man_traj()
b2contacts = scenario.find_contacts_with_mobile_objs()
#print contacts
pre_zone = initial_zone
path = [initial_zone]
for traj_pt in traj:
occupied_zone = -1
man_position = Point(traj_pt)
for i in xrange(len(zones)):
if zones[i].contains(man_position):
occupied_zone = i
break
# 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
'''
if occupied_zone == -1:
# reach end zone
path.append(-1)
break
elif occupied_zone == pre_zone:
continue
'''
path.append(occupied_zone)
pre_zone = occupied_zone
return path, b2contacts
def explore_qualitative_path(self, qualitative_path, direction_range):
print "explore path: ", qualitative_path
impulse_range = (IMPULSE_RANGE_X, direction_range)
num_sampled_points = 100
actions = self.__sample_n_points_from_range(num_sampled_points, impulse_range)
for action in actions:
#path, contacts_info = self.find_qualitative_path_ptlike(action, self.zones, self.initial_zone)
path = self.find_qualitative_path_rich(action, self.zones, self.initial_zone)
print "sampled path: ", path
#path = str(self.make_path_complete(path, self.graph)).strip('[]')
if qualitative_path not in path:
continue
else:
#print "perturb_action: ", action
max_mu = self.perturb_action(action, qualitative_path)
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 = self.simulation_counter + 1
print "sample action: ", _action
if scenario.solved:
print "solution detected ", _action, " ", self.simulation_counter
return _action
return -1
def evaluate(self, action, zones, graph):
scenario = Scenario_Generator(
self.width,
self.height,
self.immobile_objs,
self.mobile_objs,
self.manipulatable_obj,
self.target_obj,
showRender=False,
)
game_objects = scenario.getGameObjects()
end_position, shape = scenario.evaluate(action)
radius = shape.radius
end_position = Point(end_position)
circular_region_ball = end_position.buffer(radius)
occupied_zones = []
for i in xrange(len(zones)):
if zones[i].intersects(circular_region_ball):
occupied_zones.append(i)
if len(occupied_zones) == 0:
return len(zones) # set to the maximum length
min_d = 9999
for occupied_zone in occupied_zones:
length = nx.shortest_path_length(graph, source=occupied_zone, target=self.target_zone)
if length < min_d:
min_d = length
return min_d
def __apply__action__(self, impulse):
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=True,
)
scenario.apply_impulse_and_run(impulse)
def explore_path(self, sampled_actions, root_path):
print "explore path: ", root_path
num_sampled_points = 100
solution = -1
for action in sampled_actions:
max_mu = self.perturb_action(action, root_path)
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 = self.simulation_counter + 1
if scenario.solved:
print "solved !!!!!!!!!!!!!!: ", self.simulation_counter
return _action
return solution
def test_old_evaluate(self, action):
scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, showRender=False)
game_objects = scenario.getGameObjects()
graph, zones = triangulate(game_objects, self.width, self.height)
end_position, shape = scenario.evaluate(action)
end_position = Point(end_position)
last_zone = -1
for i in xrange(len(zones)):
if zones[i].contains(end_position):
last_zone = i
break
if last_zone == -1:
return len(zones) # set to the maximum length
score = nx.shortest_path_length(graph, source=i, target=self.target_zone)
return score
def find_qualitative_path_extended(self, velocity, zones, man_obj_id):
scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, showRender=False)
scenario.current_man_id = man_obj_id
scenario.apply_velocity_and_run(*velocity)
traj = scenario.find_man_traj()
b2contacts = scenario.find_contacts_with_mobile_objs()
initial_zone = -1
pre_zone = initial_zone
path = [initial_zone]
for traj_pt in traj:
occupied_zone = -1
man_position = Point(traj_pt)
for i in xrange(len(zones)):
if zones[i].contains(man_position):
occupied_zone = i
break
# 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
def explore_qualitative_path(self, qualitative_path, direction_range, essential_contact):
print "explore path: ", qualitative_path, " essential_contact with: ", essential_contact, " direction range: ", direction_range
impulse_range = (IMPULSE_RANGE_X, direction_range)
num_sampled_points = 100
actions = self.__sample_n_points_from_range(num_sampled_points, impulse_range)
for action in actions:
path, contacts_info = self.find_qualitative_path_ptlike(action, self.zones, self.initial_zone)
#print contacts_info
#print action, path, " essential_contact: ", essential_contact, " : ", contacts_info
essential_contact_detected = False
for contact in contacts_info:
if contact[0] == essential_contact:
essential_contact_detected = True
if contact[0] == self.target_obj_id:
print "solution detected ", action, " ", self.simulation_counter, str(self.make_path_complete(path, self.graph)).strip('[]')
if not essential_contact_detected:
continue
#print " path before: ", path
path = str(self.make_path_complete(path, self.graph)).strip('[]')
#print "path after: " ,path
print "perturb_action: ", action
max_mu = self.perturb_action(action, path, 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 = self.simulation_counter + 1
#print "sample action: ", _action
if scenario.solved:
print "solution detected ", _action, " ", self.simulation_counter
return _action
return -1
def find_qualitative_path_rich(self, action, zones, initial_zone):
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)
traj = scenario.find_man_traj()
num_zones = len(zones)
initial_time, x,y, touching = traj[0]
# ball must touch the surface
last_touching_time = initial_time # beginning of the last touching
last_state_time = initial_time
last_touching_state = True # False: not touched, True, touched
pre_zone = initial_zone
last_motion_type = 0
quali_path = ''
for traj_pt in traj:
time, x, y, touching = traj_pt
man_position = Point(x,y)
for i in xrange(num_zones):
if zones[i].contains(man_position):
occupied_zone = i
break
if occupied_zone == pre_zone:
if last_touching_state == touching:
continue
else:
# previously touch, currently not
if last_touching_state:
time_duration = time - last_touching_time
if time_duration > 30: #SLIDING
motion_type = 4
else:
motion_type = 8 # BOUCING
else:
motion_type = 2 # FLYING
last_touching_time = time
last_motion_type = last_motion_type | motion_type
last_motion_type = motion_type
last_touching_state = touching
else:
if time - last_state_time < 10:
continue
if last_motion_type == 0:
if last_touching_state:
time_duration = time - last_touching_time
if time_duration > 30:
last_motion_type = 4
else:
last_motion_type = 8
else:
last_motion_type = 2
quali_path = quali_path + str(pre_zone) + "*" +str(last_motion_type) + ","
last_touching_state = touching
pre_zone = occupied_zone
last_state_time = time
last_motion_type = 0
if touching:
last_touching_time = time
return quali_path
def evaluate_with_velocity(self, velocity, object_id, zones, graph):
scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, showRender=False)
end_position, shape = scenario.evaluate_with_velocity(velocity, object_id)
def test_action(self, action):
scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, showRender=True)
scenario.apply_impulse_obj(self.manipulatable_obj['id'], action)
scenario.run()
def solve_with_rules_classify(self):
all_paths = []
classification = {}
essential_contacts = set([])
quali_paths = []
sectors_score = []
for qualitative_paths_group in self.estimated_qualitative_paths_groups:
path_by_dir = {}
processed_paths = []
direction, estimated_qualitative_paths = qualitative_paths_group
for path_dir, path, essential_contact in estimated_qualitative_paths:
comp_path = self.make_path_complete(path, self.graph)
if comp_path not in quali_paths:
quali_paths.append(comp_path)
essential_contacts.add(essential_contact)
if (path_dir[0], comp_path) not in processed_paths:
processed_paths.append(comp_path)
if path_dir[0] not in path_by_dir:
path_by_dir[path_dir[0]] = [comp_path]
else:
path_by_dir[path_dir[0]].append(comp_path)
for path_dir in path_by_dir:
# subdivide path_dir into 10 sectors
#quali_paths = path_by_dir[path_dir]
divided_sectors = self.divide_dir(path_dir)
for sector in divided_sectors:
total_distance = 0
min_distance = 999
num_samples = 20
impulse_range = (IMPULSE_RANGE_X, sector)
actions = self.__sample_n_points_from_range(num_samples, impulse_range)
for action in actions:
path, contacts_info = self.find_qualitative_path_ptlike(action, self.zones, self.initial_zone)
essential_contact_detected = False
path = self.make_path_complete(path, self.graph)
for contact in contacts_info:
if contact[0] in essential_contacts:
if contact[0] == self.target_obj_id:
print "solution detected ", action, " ", self.simulation_counter, str(self.make_path_complete(path, self.graph)).strip('[]')
else:
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 = self.simulation_counter + 1
#print "sample action: ", _action
if scenario.solved:
print "solution detected ", _action, " ", self.simulation_counter
distance = self.least_distance(path, quali_paths)
if distance < min_distance:
min_distance = distance
total_distance = total_distance + distance
heappush(sectors_score, (total_distance, sector))
def solve_with_rules_classify(self):
all_paths = []
classification = {}
essential_contacts = set([])
quali_paths = []
sectors_score = []
path_by_dir = {}
path_bounces = {}
path_first_bounces = {}
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):
#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 = 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 = 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
while num_iter > 0 and not use_less_path:
num_iter = 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.zones, self.initial_zone)
if solved:
print "solution detected: ", action, " ", self.simulation_counter
continue
essential_contact_detected = False
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 + 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.apply_impulse_and_run(_action)
self.simulation_counter = self.simulation_counter + 1
#print "sample action: ", _action
if scenario.solved:
print "solution detected ", _action, " ", self.simulation_counter
if bounces_count == 0:
print "exit at: ", 10 - num_iter
use_less_path = True
def __init__(self, scenario_file):
# create scenario
self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj = loadScenario(scenario_file)
scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, showRender=False)
game_objects = scenario.getGameObjects()
self.graph, self.edges_index, self.edges_dir, self.edges_by_tri, self.vertices_of_edges, self.is_edge_surface, self.tri_by_surface, self.surface_rel_dic, self.tri_neighbor_by_edge, self.edges_by_object_id, self.objects_id_by_edge, self.zones = triangulate_advanced(game_objects, self.width, self.height)
self.initial_zone = -1
initial_obj = scenario.b2_objects[self.manipulatable_obj['id']]
initial_position = Point(initial_obj.position)
for i in xrange(len(self.zones)):
if self.zones[i].contains(initial_position):
self.initial_zone = i
break
self.target_zones = []
target_edges = self.edges_by_object_id[self.target_obj['id']]
for edge in target_edges:
#print "edge: ", edge
if edge in self.edges_index:
self.target_zones.append(self.tri_by_surface[edge])
initial_edge = None
for edge in self.edges_by_tri[self.initial_zone]:
if self.is_on_surface(initial_obj, edge):
initial_edge = edge
break
#self.target_zone = 8
self.initial_obj_id = self.manipulatable_obj['id']
self.target_obj_id = self.target_obj['id']
initial_motion = "FLYING"
self.initial_directions = []
self.initial_states = []
#path = simple_paths.next()
for target_zone in self.target_zones:
simple_paths = nx.all_simple_paths(self.graph, source=self.initial_zone, target=target_zone)
for path in simple_paths:
initial_target_edge = self.graph.get_edge_data(path[0], path[1])['rel']
initial_direction = self.edges_dir[(self.edges_index[initial_edge], self.edges_index[initial_target_edge])]
if initial_direction not in self.initial_directions:
self.initial_directions.append(initial_direction)
initial_state = (path[0], initial_edge, initial_direction, initial_motion, None, self.initial_obj_id)
self.initial_states.append(initial_state)
#print target_edge
self.root_paths = {}
self.all_paths = []
def __init__(self, scenario_file):
# create scenario
self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj = loadScenario(scenario_file)
scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, showRender=False)
game_objects = scenario.getGameObjects()
self.b2_objects = scenario.b2_objects
## draw triangulation
# tri = [] may cause segmentation fault
tri = None
###
self.graph, self.edges_index, self.edges_dir, self.edges_by_tri, self.vertices_of_edges, self.is_edge_surface, self.tri_by_surface, self.surface_rel_dic, self.tri_neighbor_by_edge, self.edges_by_object_id, self.objects_id_by_edge, self.edges_length, self.zones = triangulate_advanced(game_objects, self.width, self.height, tri)
#triangulate_advanced(game_objects, self.width, self.height, tri)
#triangulate_advanced(game_objects, self.width, self.height, tri)
#dt(self.graph, tri, self.zones)
self.initial_zone = -1
initial_obj = scenario.b2_objects[self.manipulatable_obj['id']]
self.object_size = 16
initial_position = Point(initial_obj.position)
for i in xrange(len(self.zones)):
if self.zones[i].contains(initial_position):
self.initial_zone = i
break
self.target_zones = []
target_edges = self.edges_by_object_id[self.target_obj['id']]
for edge in target_edges:
#print "edge: ", edge
if edge in self.edges_index:
self.target_zones.append(self.tri_by_surface[edge])
self.initial_edge = None
min_d = 99999
for edge in self.edges_by_tri[self.initial_zone]:
flag, distance = self.is_on_surface(initial_obj, edge)
if flag:
self.initial_edge = edge
break
else:
if min_d > distance:
min_d = distance
self.initial_edge = edge
#self.target_zone = 8
self.initial_obj_id = self.manipulatable_obj['id']
self.target_obj_id = self.target_obj['id']
initial_motion = "FLYING"
self.initial_directions = []
self.initial_states = []
#path = simple_paths.next()
self.initial_position = self.b2_objects[self.manipulatable_obj['id']].position
for target_zone in self.target_zones:
simple_paths = nx.all_simple_paths(self.graph, source=self.initial_zone, target=target_zone)
for path in simple_paths:
initial_target_edge = self.graph.get_edge_data(path[0], path[1])['rel']
#initial_direction = self.edges_dir[(self.edges_index[initial_edge], self.edges_index[initial_target_edge])]
########################## DEBUG added #######################
initial_direction = self.compute_initial_direction_exact_position(self.initial_position, self.vertices_of_edges[initial_target_edge])
if initial_direction[0] < 0:
initial_direction = (0, initial_direction[1])
############################ ###################################################
if initial_direction not in self.initial_directions:
self.initial_directions.append(initial_direction)
### state = (current zone id, current edge index set, direction:[(),()], motion_type, collision happened, direction before updated, current obj id)
initial_state = (path[0], self.initial_edge, initial_direction, initial_motion, False , None, self.initial_obj_id)
self.initial_states.append(initial_state)
### check if can slide:
if self.is_edge_surface[self.initial_edge] == 1:
connected_surfaces = self.surface_rel_dic[self.initial_edge]
for surface_info in connected_surfaces:
connected_surface, on_surface_direction, exiting_direction, intersection_angle = surface_info
if on_surface_direction[0] <= initial_direction[1] and on_surface_direction[0] >=initial_direction[0]:
initial_state = (path[0], self.initial_edge, on_surface_direction, SLIDING, False, None, self.initial_obj_id)
#print "add initial state ", initial_state
#self.initial_states.append(initial_state)
'''
if self.is_edge_surface[self.initial_edge] == 1:
connected_surfaces = self.surface_rel_dic[self.initial_edge]
for surface_info in connected_surfaces:
connected_surface, on_surface_direction, exiting_direction, intersection_angle = surface_info
if self.tri_by_surface[connected_surface] == path[1]:
initial_state = (path[0], self.initial_edge, on_surface_direction, SLIDING, False, None, self.initial_obj_id )
# add event
self.initial_states.append(initial_state)
'''
##### Can remove later
#print self.edges_by_object_id[self.target_obj_id]
self.root_paths = {}
self.all_paths = []
from game_object import GameObject as GOBJ
#from path_finding import createGraph
from triangulation import triangulate, triangulate_advanced
import numpy as np
import networkx as nx
from triangle_utils import triangle_center
from triangle_relation import printRels
from scenario_reader import loadScenario
#scenario_file = './scenarios/s3.json'
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=True)
## get objects
game_objects = scenario.getGameObjects()
tri = []
graph, edges_index, edges_dirs, edges_by_tri, vertices_of_edges, edges_surface_dic, tri_by_surface, surface_rel_dic, tri_neighbor_by_edge, edges_by_object_id, objects_id_by_edge, zones = triangulate_advanced(game_objects, scenario_width, scenario_height, tri)
plot.plot(plt.axes(),**(tri[0]))
################### Arrange graph according to the position of triangles #########
#triangles_by_vertices = tri["vertices"][tri["triangles"]]
pos = {}
for index, triangle in enumerate(zones):
pos[index] = (triangle.centroid.x, triangle.centroid.y)
def solve_with_rules_classify(self):
all_paths = []
classification = {}
essential_contacts = set([])
quali_paths = []
sectors_score = []
path_by_dir = {}
path_bounces = {}
for path_dir, path, essential_contact, bounces_pos 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]
path_bounces[path_dir] = [bounces_pos]
else:
path_by_dir[path_dir].append(comp_path)
path_bounces[path_dir].append(bounces_pos)
'''
while all_paths:
path, bounces_pos = heappop(all_paths)
print path, " bounces: ", bounces_pos
'''
for path_dir in path_by_dir:
print "Test dir range: ", path_dir
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)
for sector in divided_sectors:
total_distance = 0
min_distance = 999
num_samples = 40
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.zones, self.initial_zone)
if solved:
print "solution detected: ", action, " ", self.simulation_counter
continue
essential_contact_detected = False
path = self.make_path_complete(path, self.graph)
for contact in contacts_info:
############### ERROR, Target object will not be in contacts ############################
'''
if contact[0] == self.target_obj_id:
print "solution detected ", action, " ", self.simulation_counter, str(self.make_path_complete(path, self.graph)).strip('[]')
'''
#####################################################################################
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 = self.simulation_counter + 1
#print "sample action: ", _action
if scenario.solved:
print "solution detected ", _action, " ", self.simulation_counter
distance = self.least_distance(path, quali_paths)
if distance < min_distance:
min_distance = distance
total_distance = total_distance + distance
heappush(sectors_score, (total_distance, sector))
