world.type = 'cartesian' # Objects in the cartesian space og = ObjectGenerator(world) og.generate_object([[3.5, 6.5], [3.5, 3.5], [6.5, 3.5], [6.5, 6.5]]) # Robot robot2R = RobotArm(base=[0, 0], lengths=[3.0, 5.0]) robot2R.start = np.array([pi / 2, -pi / 7]) robot2R.goal = np.array([pi / 5, -pi / 7]) # C-space cspace = World() cspace.robot = robot2R cspace.generate_frame([-pi, pi], [-pi, pi]) cspace.start = cspace.robot.start cspace.goal = cspace.robot.goal cspace.generate_cspace_objects([100, 100], world.objects) cspace.generate_cspace_objects([100, 100], [world.frame]) cspace.type = 'cspace' # world.start = np.array([-3.0, -3.0]) # world.goal = np.array([3.0, 3.0]) # og = ObjectGenerator(world) # og.generate_object_sample1() ga = GeneticAlgorithm(world=cspace, NGEN=100, n_ind=100, n_elite=10, fitness_thresh=0.0, margin_on=False,
World setting ''' # --- world class world = World() # --- Set frame world.frame = np.array( [[-pi, -pi], [-pi, pi], [pi, pi], [pi, -pi], [-pi, -pi]]) # --- Set start/goal point world.start = np.array([-pi / 2, -pi / 2]) * 1.9 world.goal = np.array([pi / 2, pi / 2]) * 1.9 # --- Generate objects og = ObjectGenerator(world) world.objects = og.example() # og.generate(10, 5) # og.locate([2.0, 3.0]) # world.objects = og.world.objects # --- Generte Collision Checker cc = CollisionChecker(world, 0.1) ''' Functions '''
self.path_list.append(self.smoothed_path) if verbose: t2 = time.time() print('Generate multiple paths: {}\n'.format(t2 - t1)) if __name__ == '__main__': # --- world class world = World() world.generate_frame([-pi, pi], [-pi, pi]) # --- Set start/goal point world.start = np.array([-3.0, -3.0]) world.goal = np.array([3.0, 3.0]) # --- Set objects og = ObjectGenerator(world) og.generate_object_sample1() og.set_object_type() # ''' # --- Generate 10x10 valid roadmaps t0 = time.time() prm_list = [PRM(world, 100, 5) for i in range(10)] path_list = [] for prm in prm_list: prm.single_query(verbose=True) if prm.path_list != []:
if __name__ == '__main__': ''' World setting ''' # World world = World() world.generate_frame([-pi, pi], [-pi, pi]) world.type = 'cartesian' # Objects in the cartesian space og = ObjectGenerator(world) og.generate_object_sample1() # --- Set start/goal point world.start = np.array([-pi / 2, -pi / 2]) * 1.9 world.goal = np.array([pi / 2, pi / 2]) * 1.9 ''' Functions ''' NGEN = 1000 n_ind = 100 n_elite = 10 fitness_thresh = 0.1 verbose = True ga = GeneticAlgorithm(world, NGEN, n_ind, n_elite, fitness_thresh, verbose) rp = RealtimePlot(world, 100, dt=0.01) # --- Initial Population # initial_pop = create_pop_cd(100) initial_pop = ga.create_pop_prm(10, 10)