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
