def __init__(self, scenario_file, gravity_dir=(-1, -1)):
self.count = 0
self.gravity_dir = gravity_dir
self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj = loadScenario(
scenario_file
)
self.impulse = None
# game_objects = self.__create_scenario__().getGameObjects()
target_obj = self.__create_scenario__().b2_objects[self.target_obj["id"]]
print target_obj.fixtures[0].massData.center
self.target_pos = target_obj.transform * target_obj.fixtures[0].massData.center
# self.target_pos = game_objects[self.target_obj['id']].mass_center
print self.target_pos
import triangle.plot as plot
import matplotlib.pyplot as plt
# from scenario_generator import Scenario_Generator as SG
from game_object import GameObject as GOBJ
import numpy as np
import networkx as nx
from triangle_utils import triangle_center
from triangle_relation import printRels, getRel
from shapely.geometry import Polygon, LineString as Line
from math import pi, acos
file_name = "mini2"
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()
"""
dic = {}
dic["vertices"] = [[0.0,0.0],[100.0,0.0],[100.0,2.0],[0.0,2.0],[77.7722702026,4.0222196579],[99.0691375732,1.34814071655],[103.008628845,32.7230033875],[81.7117614746,35.3970832825],[33.6893234253,37.2380599976],[54.1199073792,32.9810180664],[55.028678894,37.3424224854],[34.5980949402,41.5994644165],[15.1510419846,52.2749481201],[36.719417572,55.3364334106],[36.4076156616,57.5330886841],[14.8392419815,54.4716033936],[55.4564323425,53.2923240662],[73.710144043,52.1638069153],[74.098487854,58.445224762],[55.8447723389,59.5737419128],[28.52501297,28.6967639923],[30.8609046936,28.6548023224],[31.2251396179,48.9308547974],[28.8892478943,48.9728164673],[17.6961288452,28.9960212708],[19.5502109528,28.8180770874],[21.4820976257,48.9470100403],[19.6280155182,49.1249542236],[59.7772750854,35.7274398804],[60.3249855042,35.7963409424],[58.2185592651,52.5418014526],[57.6708488464,52.4729003906],[38.7034988403,39.6549720764],[41.0576400757,40.1066627502],[39.6682815552,47.3478050232],[37.3141403198,46.8961143494],[34.6390304565,54.7437553406],[53.1063194275,52.5728187561],[53.4703216553,55.6692390442],[35.0030326843,57.8401756287],[41.0,40.0],[49.0,40.0],[49.0,42.0],[41.0,42.0],[0.0,0.0],[0.0,70.0],[100.0,70.0],[100.0,0.0]]
dic["segments"] = [[0,1],[1,2],[2,3],[3,0],[4,5],[5,6],[6,7],[7,4],[8,9],[9,10],[10,11],[11,8],[12,13],[13,14],[14,15],[15,12],[16,17],[17,18],[18,19],[19,16],[20,21],[21,22],[22,23],[23,20],[24,25],[25,26],[26,27],[27,24],[28,29],[29,30],[30,31],[31,28],[32,33],[33,34],[34,35],[35,32],[36,37],[37,38],[38,39],[39,36],[40,41],[41,42],[42,43],[43,40]]
dic["holes"] = [[50.0,1.0],[90.3904495239,18.3726115227],[44.3590011597,37.2902412415],[25.7793288231,54.9040184021],[64.7774600983,55.8687744141],[29.8750762939,38.8138093948],[19.5891132355,38.9715156555],[58.9979171753,44.1346206665],[39.1858901978,43.5013885498],[44.0546760559,55.2064971924],[45.0,41.0],]
def __init__(self, scenario_file, gravity_dir=(-1, -1)):
# self.evalPath = EVAL(scenario_file)
self.gravity_dir = gravity_dir
self.evalPath = EVAL(scenario_file, gravity_dir=gravity_dir)
# create scenario
self.sigma = -0.0001
self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj = loadScenario(
scenario_file
)
self.target_obj_id = self.target_obj["id"]
self.maxd = 3
self.qualitative_paths = {}
self.qualitative_paths_actions = {}
self.zones = []
self.simulation_counter = 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
else:
while running:
self.__sim_one_step__()
if self.solved or self.__reach_end__():
break
return self.solved
if __name__=="__main__":
scenario_file = './scenarios/s5.json'
test = Scenario_Generator(*loadScenario(scenario_file), showRender=True)
#test.run()
#test.apply_velocity_and_run()
#print test.cart2pol(906.8805,0)
test.apply_impulse_and_run((189.55180966738038, 0.7484582968892486))
print test.solved
#test.apply_impulse_and_run((4517.779769295986, 0.11654142383307775))
#print test.find_contacts_with_mobile_objs()
#test.__sim_one_step__()
#current_man = test.get_current_man()
#test.run()
#print current_man.contacts[0].contact.worldManifold.points[0]
#test.evaluate((972.8,0))
#state = test.run()
#print test.objects_distance_matrix()
def __init__(self, scenario_file, gravity_dir=(-1, -1)):
print "eval using: ", scenario_file, " gravity: ", gravity_dir
self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj = loadScenario(
scenario_file
)
self.count = 0
self.first_sol = 0
self.gravity_dir = gravity_dir
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, 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 __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)
#self.target_zone = 8 # hard code for debugging s.6: 8, s6.1:12, s7:20
self.target_obj_id = self.target_obj['id']
self.maxd = 3
self.qualitative_paths = {}
self.qualitative_paths_actions = {}
self.zones = []
self.simulation_counter = 0
quali_sim = qualitative_simulation(scenario_file)
self.estimated_qualitative_paths_groups = quali_sim.run()
self.initial_zone = quali_sim.initial_zone
self.graph = quali_sim.graph
self.zones = quali_sim.zones
def __init__(self, target_region=None, O_i_rect=None, action=None, showRender=False, checksolution=False, scenario_file='s5.json'):
super(Scenario_Generator, self).__init__()
self.showRender = showRender
self.state = -1
self.min_d = 9999999
self.state_impulse = (0,0)
# Tell the framework we're going to use contacts, so keep track of them
# every Step.
self.using_contacts=True
self.beginContact = False
self.contactAID = -1
self.contactBID = -1
#current simulation round
self.round = 0
self.objID = 0
world = self.world
self.objects = []
self.game_objects = []
self.target_region = target_region
width, height, immobile_objs, mobile_objs, manipulatable_objs, target_obj = sr.loadScenario(scenario_file)
for obj in immobile_objs:
## create a simulator object
gobj = GOBJ(obj['id'])
if obj['shape'] == "box":
static_body = world.CreateStaticBody(position=obj['position'], shapes=polygonShape(box=obj['size']), angle=obj['angle'],userData=gobj)
elif obj['shape'] == "polygon":
static_body = world.CreateStaticBody(position=obj['position'], shapes=polygonShape(vertices=obj['vertices']), userData=gobj)
# store coordinates info in the screen
mass_center = static_body.fixtures[0].massData.center
vertices=[(static_body.transform*v) for v in static_body.fixtures[0].shape]
#vertices=[(v[0], SCREEN_HEIGHT-v[1]) for v in vertices]
gobj.setPosScr(vertices)
gobj.setMassCenterSrc(static_body.transform * mass_center)
self.game_objects.append(gobj)
target_id = target_obj['id']
for obj in mobile_objs:
obj_id = obj['id']
gobj = GOBJ(obj_id,role=GOBJ.MOVEABLE)
if obj['shape'] == "box":
dynamic_body = world.CreateDynamicBody(position=obj['position'], userData=gobj)
dynamic_body.CreatePolygonFixture(box=obj['size'],density=obj['density'], friction=obj['friction'])
mass_center = dynamic_body.fixtures[0].massData.center
vertices=[(dynamic_body.transform*v)for v in dynamic_body.fixtures[0].shape]
#vertices=[(v[0], SCREEN_HEIGHT-v[1]) for v in vertices]
gobj.setPosScr(vertices)
gobj.setMassCenterSrc(dynamic_body.transform * mass_center)
self.game_objects.append(gobj)
for obj in manipulatable_objs:
gobj = GOBJ(obj['id'],role=GOBJ.MAN)
if obj['shape'] == "box":
dynamic_body = world.CreateDynamicBody(position=obj['position'], userData=gobj)
dynamic_body.CreatePolygonFixture(box=obj['size'],density=obj['density'], friction=obj['friction'])
mass_center = dynamic_body.fixtures[0].massData.center
vertices=[(dynamic_body.transform*v)for v in dynamic_body.fixtures[0].shape]
#vertices=[(v[0], SCREEN_HEIGHT-v[1]) for v in vertices]
gobj.setPosScr(vertices)
gobj.setMassCenterSrc(dynamic_body.transform * mass_center)
self.game_objects.append(gobj)
'''
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)
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)
break
else:
while running:
self.__sim_one_step__()
if self.solved or self.__reach_end__():
break
return self.solved
if __name__ == "__main__":
# scenario_file = './scenarios/s3tmdmfms/4.json'
scenario_file = './scenarios/mini2p5.json'
test = Approx_Scenario_Generator(*loadScenario(scenario_file), sigma=-1, gravity_dir=(0,0), showRender=True)
#test.run()
# test.apply_velocity_and_run()
# print test.cart2pol(906.8805,0)
test.apply_impulse_and_run(((3294.3711984417114, 1.4930185179732731)))
# print test.solved
# test.apply_impulse_and_run((4517.779769295986, 0.11654142383307775))
# print test.find_contacts_with_mobile_objs()
# test.__sim_one_step__()
# current_man = test.get_current_man()
# test.run()
break
else:
while running:
self.__sim_one_step__()
if self.solved or self.__reach_end__():
break
return self.solved
if __name__ == "__main__":
# scenario_file = './scenarios/s3tmdmfms/4.json'
scenario_file = './scenarios/mini1.json'
test = Scenario_Generator(*loadScenario(scenario_file), gravity_dir=(0,0), showRender=True)
# test.run()
# test.apply_velocity_and_run()
# print test.cart2pol(906.8805,0)
test.apply_impulse_and_run((376.63431624061826, 1.7420460328417813))
traj, index = test.find_man_traj_bounce()
print traj
# print test.solved
# test.apply_impulse_and_run((4517.779769295986, 0.11654142383307775))
# print test.find_contacts_with_mobile_objs()
# test.__sim_one_step__()
from scenario_generator import Scenario_Generator as SG
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):
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)
#self.target_zone = 8 # hard code for debugging s.6: 8, s6.1:12, s7:20
self.target_obj_id = self.target_obj['id']
self.maxd = 3
self.qualitative_paths = {}
self.qualitative_paths_actions = {}
self.zones = []
self.simulation_counter = 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 __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 = []
if save_image and self.round == 1: # without round = 100, some objects will not be settled when taking screenshots
pygame.image.save(screen, 's1.png')
print ("screenshot was taken")
save_image = False
self.__sim_one_step__()
if self.solved or self.__reach_end__():
break
else:
while running:
self.__sim_one_step__()
if self.solved or self.__reach_end__():
break
return self.solved
if __name__=="__main__":
scenario_file = './scenarios/s7.json'
test = sim_object_behaviour(*loadScenario(scenario_file), showRender=True)
test.apply_impulse_obj(4, (100, 0.0),(-1,-4))
test.run()
#test.evaluate((972.8,0))
#state = test.run()
#print test.objects_distance_matrix()
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)
self.target_zone = 12 # hard code for debugging eventual_goal:8 s.6: 12, s6.1:8, s7:20
self.target_obj_id = self.target_obj['id']
self.initial_zone = 24 # hard code for debugging s.6:24, s6.1:4, s7:3
self.maxd = 3
self.qualitative_paths = {}
self.qualitative_paths_actions = {}
self.zones = []
self.graph = None
self.simulation_counter = 0
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)
self.target_zone = 12 # hard code for debugging eventual_goal:8
self.initial_zone = 24 # hard code for debugging
self.maxd = 3