def get_occ_grid(obstacles):
occ_grid1 = np.ones((20, 20), dtype=int)
eps = 0.025
for i in range(0, 20):
for j in range(0, 20):
if (not (helper.is_valid(
(i / 20.0 + eps, j / 20.0 + eps), obstacles))):
occ_grid1[i, j] = 0
else:
occ_grid1[i, j] = 1
return occ_grid1.ravel()
def is_trivial(start, goal, obstacles):
start = np.array(start)
goal = np.array(goal)
diff = goal - start
step = diff / EDGE_DISCRETIZATION
for i in range(EDGE_DISCRETIZATION + 1):
nodepos = start + step * i
if (not (helper.is_valid(nodepos, obstacles))):
return 0
return 1
def is_in_collision(prev_posn, curr_posn, obstacles):
start = np.array(prev_posn)
goal = np.array(curr_posn)
diff = goal - start
step = diff / EDGE_DISCRETIZATION
for i in range(EDGE_DISCRETIZATION + 1):
nodepos = start + step * i
if (not (helper.is_valid(nodepos, obstacles))):
return 1
return 0