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