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