def test():
import smooth_control
# Grid format: 1:occupied, 0:empty
grid = [[0, 1, 0, 0, 0, 0],
[0, 1, 0, 1, 1, 0],
[0, 1, 0, 0, 1, 0],
[0, 1, 1, 0, 1, 0],
[0, 0, 0, 0, 1, 0]]
grid=np.array(grid,dtype=int)
start = [0, 0]
goal = [len(grid)-1, len(grid[0])-1]
path = find_path(grid, start, goal)
spath = smooth_control.smooth(path, 0.5, 0.1)
visual2D(path, spath)
err += (Perr**2)
#plt.plot(*zip(*data))
return rob.reached(goal), rob.num_collisions, N, data
def run(grid,
init,
goal,
(weight_data, weight_smooth, p_gain, d_gain),
show=True):
from search import find_path
import smooth_control
grid = np.array(grid, dtype=int)
path = find_path(grid, init, goal)
spath = smooth_control.smooth(path, weight_data, weight_smooth)
res = navigate(
grid,
init,
goal,
spath,
(p_gain, 0.0, d_gain),
)
if show:
visual2D(path, spath, res[-1])
return res
def average_run(params, grid, init, goal):
K = 10
best_err = 0.0
if not rob.check_collision(grid):
print '##### Collision ####'
err += (Perr**2)
#plt.plot(*zip(*data))
return rob.reached(goal), rob.num_collisions, N, data
def run(grid, init, goal,
(weight_data, weight_smooth, p_gain, d_gain),
show=True):
from search import find_path
import smooth_control
grid=np.array(grid,dtype=int)
path = find_path(grid, init, goal)
spath = smooth_control.smooth(path, weight_data, weight_smooth)
res = navigate(grid, init, goal, spath,
(p_gain, 0.0, d_gain),)
if show:
visual2D(path, spath, res[-1])
return res
def average_run(params, grid, init, goal):
K = 10
best_err = 0.0
for k in range(K):
ret = run(grid, init, goal, params, show=False)
if ret[0]:
best_err += ret[1] * 100 + ret[2]
else:
best_err += 99999