# initial_pop = create_pop_cd(100)
initial_pop = ga.create_pop_prm(10, 10)
ga.pop = initial_pop.copy()
for i in range(100):
ga.main(1.0)
# Update objects.
if i % 10 < 5:
vel = 0.05
else:
vel = -0.05
dl = np.array([[vel, 0.0], [-vel, 0.0]])
world.update_objects(dl)
ga.world_margin.update_objects(dl)
# Update current robot position.
dl = 0.1
world.update_start(ga.best_ind, dl)
ga.world_margin.update_start(ga.best_ind, dl)
for i in range(len(ga.pop)):
ga.pop[i][0] = world.start
# Create trajectory
ga.trajectory = np.vstack((ga.trajectory, world.start))
if ga.best_ind.fitness <= ga.fitness_thresh:
print('Goal.')
break
break
return self.path_list
# In[]:
if __name__ == '__main__':
# --- world class
world = World()
# --- Set frame and objects
og = ObjectGenerator(world)
og.generate_frame([-pi, pi], [-pi, pi])
og.generate_object_sample1()
og.set_object_type()
# world_margin = world.mcopy(rate=1.05)
world.update_objects([[0.5, 0.0], [-0.5, 0.0]])
# --- Set start/goal point
world.start = np.array([-3.0, -3.0])
world.goal = np.array([3.0, 3.0])
cd = CellDecomposition(world)
path_list = cd.main(10, shortcut=True)
gd = GraphDrawer(world)
# gd.draw_path(cd.L_list)
gd.draw_path(path_list)
plt.show()