def testmodel_random_nolimit(path):
model = load_model(path)
# Define size of environment
x_up_bound = 60 # 120
x_low_bound = 0
y_up_bound = 60 #120
y_low_bound = 0
# x_goal = (100, 100)
#
# X_dimensions = np.array([(0, 120), (0, 120)]) # dimension of serach space
# Obstacles = np.array([(20, 20, 40, 40), (20, 60, 40, 80),
# (60, 20, 80, 40), (60, 60, 80, 80)]) # obstacles
x_goal = (30, 60) # goal location(100, 100)
X_dimensions = np.array([(x_low_bound, x_up_bound),
(y_low_bound, y_up_bound)
]) # dimension of serach space
Obstacles = np.array([(0, 0, 20, 20), (0, 40, 20, 60), (40, 0, 60, 20),
(40, 40, 60, 60)]) # obstacles
X = SearchSpace(X_dimensions, Obstacles)
x_init = (random.randrange(x_low_bound, x_up_bound),
random.randrange(y_low_bound, y_up_bound))
while is_Obstacle(x_init, Obstacles):
x_init = (random.randrange(x_low_bound, x_up_bound),
random.randrange(y_low_bound, y_up_bound))
x_position = np.zeros((1, 2))
x_position[0][0] = x_init[0]
x_position[0][1] = x_init[1]
path = list()
x_temp = (x_position[0][0], x_position[0][1])
path.append(x_temp)
for i in range(100):
y_pred = model.predict(x_position)
print(y_pred)
x_position = y_pred
position = [x_position[0][0], x_position[0][1]]
x_temp = (x_position[0][0], x_position[0][1])
path.append(x_temp)
if x_position[0][0] > x_up_bound or x_position[0][
1] > y_up_bound or x_position[0][
0] < x_low_bound or x_position[0][1] < y_low_bound:
break
print("Final position is", x_position)
plot = Plot("rrt_2d")
plot.plot_path(X, path)
plot.plot_obstacles(X, Obstacles)
plot.plot_start(X, x_init)
plot.plot_goal(X, x_goal)
plot.draw(auto_open=True)
del model
def obplot(obstacles, goal1):
X_dimensions = np.array([(0, EnviromBoundx), (0, EnviromBoundy)]) # dimensions of Search Space
# obstacles
Obstacles = np.array([(ob[0][0], ob[0][1], ob[2][0], ob[2][1]) for ob in obstacles])
X = SearchSpace(X_dimensions, Obstacles)
path1 = [[0, 0], goal1]
plot = Plot("obst_env")
plot.plot_path(X, path1)
plot.plot_obstacles(X, Obstacles)
# plot.plot_start(X, x_init)
# plot.plot_goal(X, x_goal)
plot.draw(auto_open=True)
def RRT_arm(obstacles, goalxy, thetas):
if goalxy[0] > EnviromBoundx or goalxy[1] > EnviromBoundy:
print("We cannot reach that bound.")
return False
X_dimensions = np.array([(0, EnviromBoundx),
(0, EnviromBoundy)]) # dimensions of Search Space
# obstacles
Obstacles = np.array([(ob[0], 0, ob[1], ob[2]) for ob in obstacles])
print(Obstacles)
# Obstacles = np.array([(20, 20, 40, 40), (20, 60, 40, 80), (60, 20, 80, 40), (60, 60, 80, 80)])
x_init = (initialx, initialy) # starting location
x_goal = (goalxy[0], goalxy[1]) # goal location
Q = np.array([(1, 1)]) # length of tree edges
r = l1 / 10 # length of smallest edge to check for intersection with obstacles
max_samples = 1024 # max number of samples to take before timing out
prc = 0.2 # probability of checking for a connection to goal
# create search space
X = SearchSpace(X_dimensions, Obstacles)
# create rrt_search
rrt = RRT(X, Q, x_init, x_goal, max_samples, r, prc)
path = rrt.rrt_search()
thetas_c = thetas.copy()
i = 0
for point in path:
i = i + 1
print(point)
thetas_c[1], thetas_c[2], thetas_c[3] = iv.iter(
thetas_c[1], thetas_c[2], thetas_c[3], point[0], point[1])
for ob in Obstacles:
if obt.ArmStatusCheck(thetas, ob):
return False
print("The", i, "th Angles is:", thetas_c, "when grabber go to", point)
# plot
plot = Plot("rrt_2d")
plot.plot_tree(X, rrt.trees)
if path is not None:
plot.plot_path(X, path)
plot.plot_obstacles(X, Obstacles)
plot.plot_start(X, x_init)
plot.plot_goal(X, x_goal)
plot.draw(auto_open=True)
return True
def testmodel_fromzero(path, xinit, yinit):
x_up_bound = 60 # 120
x_low_bound = 0
y_up_bound = 60 # 120
y_low_bound = 0
model = load_model(path)
x_init = (xinit, yinit)
x_goal = (30, 60) # goal location(100, 100)
X_dimensions = np.array([(x_low_bound, x_up_bound),
(y_low_bound, y_up_bound)
]) # dimension of serach space
Obstacles = np.array([(0, 0, 20, 20), (0, 40, 20, 60), (40, 0, 60, 20),
(40, 40, 60, 60)]) # obstacles
X = SearchSpace(X_dimensions, Obstacles)
x_position = np.zeros((1, 2))
x_position[0][0] = xinit
x_position[0][1] = yinit
path = list()
x_temp = (x_position[0][0], x_position[0][1])
path.append(x_temp)
for i in range(100):
# print(x_position)
y_pred = model.predict(x_position)
print(y_pred)
# breakpoint()
x_position = y_pred
position = [x_position[0][0], x_position[0][1]]
x_temp = (x_position[0][0], x_position[0][1])
path.append(x_temp)
if x_position[0][0] > x_up_bound or x_position[0][
1] > y_up_bound or x_position[0][
0] < x_low_bound or x_position[0][1] < y_low_bound:
break
print("Final position is", x_position)
plot = Plot("rrt_2d")
plot.plot_path(X, path)
plot.plot_obstacles(X, Obstacles)
plot.plot_start(X, x_init)
plot.plot_goal(X, x_goal)
plot.draw(auto_open=True)
del model
x_goal = (100, 100) # goal location
X_dimensions = np.array([(0, 120), (0, 120)]) # dimension of serach space
Obstacles = np.array([(20, 20, 40, 40), (20, 60, 40, 80), (60, 20, 80, 40),
(60, 60, 80, 80)]) # obstacles
Q = np.array([(8, 4)]) # length of tree edges
r = 1 # length of smallest edge to check for intersection with obstacles
max_samples = 1024 # max number of samples to take before timing out
prc = 0.1 # probability of checking for a connection to goal
max_iteration = 100000 #100000
data_size = 100000 #100000
x = np.zeros((data_size, 2))
y = np.zeros((data_size, 2))
data_position = 0
X = SearchSpace(X_dimensions, Obstacles)
for i in range(max_iteration):
# create rrt_search
rrt = RRT(X, Q, x_init, x_goal, max_samples, r, prc)
path = rrt.rrt_search()
for k in range(len(path) - 1):
x[data_position] = path[k]
y[data_position] = path[k + 1]
data_position += 1
if data_position >= data_size:
break
if data_position >= data_size:
break
print("Data Collected")