# 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,
verbose=True)
rp = RealtimePlot(cspace, 100, dt=0.01)
'r': [None, None],
'umax': [None, None],
'umin': [None, None]
}
robot2R = RobotArm(robot_parameter)
robot2R.start = np.array([0.0, 2.0])
robot2R.goal = np.array([-0.6, 1.0])
# 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_margin.objects)
cspace.generate_cspace_objects([100, 100], [world.frame])
cspace.type = 'cspace'
ga = GeneticAlgorithm(world=cspace,
NGEN=10,
n_ind=100,
n_elite=10,
fitness_thresh=3.4,
verbose=True)
initial_pop = ga.create_pop_prm(10, 10)
# ga.pop = [Individual(resampling(path, 1.0)) for path in initial_pop]
ga.pop = initial_pop.copy()
ga.main(np.inf)
print('Best Fitness: {}'.format(ga.best_ind.fitness))