def test_init(self):
"""Tests the __init__() function of the AttractorField"""
# testing without args:
field = AttractorField()
self.assertIsNone(field._occupancy_grid)
self.assertIsNone(field._goal)
# testing with provided occupancy_grid:
field = AttractorField(occupancy_grid=self.occupancy_grid)
self.assertTrue((field._occupancy_grid == self.occupancy_grid).all())
self.assertIsNone(field._goal)
self.assertEqual((self.N, self.M), field._grid_shape)
self.assertTrue((np.array([self.N, self.M]) == field._grid_shape_arr).all())
# testing with provided goal:
field = AttractorField(goal=self.goal)
self.assertIsNone(field._occupancy_grid)
self.assertTrue((field._goal == self.goal).all())
# testing with provided occupancy grid and goal:
field = AttractorField(occupancy_grid=self.occupancy_grid, goal=self.goal)
self.assertTrue((field._occupancy_grid == self.occupancy_grid).all())
self.assertTrue((field._goal == self.goal).all())
self.assertEqual((self.N, self.M), field._grid_shape)
self.assertTrue((np.array([self.N, self.M]) == field._grid_shape_arr).all())
self.assertIsNotNone(field._field)
def __init__(self, occupancy_grid, goal_pos, goal_ang, R, params):
"""Initializes the GradController object."""
self._occupancy_grid = occupancy_grid
self.set_new_goal(goal_pos, goal_ang)
self._R = R
# creating the attractive and repulsive gradient fields:
self._attractor = AttractorField(occupancy_grid=self._occupancy_grid,
goal=self._goal_pos)
self._repulsive = RepulsiveField(occupancy_grid=self._occupancy_grid,
R=self._R)
# setting up some params based on the json file if provided:
self._set_from_params(params)
def test_expand_pixel(self):
"""Tests the _expand_pixel method of the AttractorField"""
# if expanding again any pixels without changing their values, nothing should change:
# field1 is left as it was and field2 is modified.
field1 = AttractorField(occupancy_grid=self.occupancy_grid, goal=self.goal)
field2 = AttractorField(occupancy_grid=self.occupancy_grid, goal=self.goal)
for ind in [[1, 1], [5, 5], [3, 7], [8, 1], [8, 4]]:
field2._expand_pixel(np.array(ind))
for i in range(self.N):
for j in range(self.M):
self.assertEqual(field1._field[i, j].value, field2._field[i, j].value)
self.assertTrue((field1._field[i, j].grad == field2._field[i, j].grad).all())
### placing an obstacle and then removing it from field2: ###
occ_grid = self.occupancy_grid.copy()
occ_grid[7, 5] = 1
field1 = AttractorField(occupancy_grid=occ_grid, goal=self.goal)
field2 = AttractorField(occupancy_grid=occ_grid, goal=self.goal)
field2._field[7, 5].value = 0
field2._expand_pixel(np.array([6, 5]))
# indices where the value did not change:
for ind in [(5, 5), (6, 5), (6, 6), (7, 6), (8, 6), (9, 6), (6, 4), (7, 4), (8, 4), (9, 4), (9, 5)]:
self.assertEqual(field1._field[ind].value, field2._field[ind].value)
# indices where the gradient did not change:
for ind in [(5, 5), (6, 5), (6, 6), (7, 7), (8, 7), (9, 7), (6, 4), (7, 3), (8, 3), (9, 3)]:
self.assertEqual(field1._field[ind].value, field2._field[ind].value)
self.assertTrue((field1._field[ind].grad == field2._field[ind].grad).all())
# indices where the value has changed:
self.assertEqual(field1._field[6, 5].value - 1, field2._field[7, 5].value)
self.assertEqual(field1._field[8, 5].value + 2, field2._field[8, 5].value)
# some indices with different gradients:
self.assertTrue((field2._field[7, 5].grad == np.array([-1, 0])).all())
self.assertTrue((field2._field[8, 5].grad == np.array([-1, 0])).all())
self.assertTrue((field2._field[7, 6].grad == np.array([-1 / np.sqrt(2), -1 / np.sqrt(2)])).all())
self.assertTrue((field2._field[8, 6].grad == np.array([-1 / np.sqrt(2), -1 / np.sqrt(2)])).all())
self.assertTrue((field2._field[7, 4].grad == np.array([-1 / np.sqrt(2), 1 / np.sqrt(2)])).all())
self.assertTrue((field2._field[8, 4].grad == np.array([-1 / np.sqrt(2), 1 / np.sqrt(2)])).all())
def test_update_pixel(self):
"""Tests the _update_pixel method of the AttractorField"""
field = AttractorField(occupancy_grid=self.occupancy_grid, goal=self.goal)
# setting some pixels:
for ind in [(6, 5), (6, 6), (5, 6), (4, 6), (4, 5), (4, 4), (6, 4)]:
field._field[ind].value = -3
field._field[5, 4].value = -2
# the pixel value is already bigger:
field._field[5, 5].value = -1
new_pix = field._field[5, 5]
new_pix = field._update_pixel(new_pix)
self.assertEqual(new_pix.value, -1)
self.assertTrue((new_pix.grad == np.array([0, 0])).all())
# the pixel value has to be changed:
field._field[5, 5].value = -4
new_pix = field._field[5, 5]
new_pix = field._update_pixel(new_pix)
self.assertEqual(new_pix.value, -3)
self.assertTrue((new_pix.grad == np.array([0, -1])).all())
def test_list_expandable_indices(self):
"""Tests the _list_expandable_indices method of the AttractorField"""
field = AttractorField(occupancy_grid=self.occupancy_grid, goal=self.goal)
field._changed_indices = [np.array([0, 5]), np.array([5, 5])]
# expected list:
etalon_indices = [np.array([1, 5]), np.array([4, 5]), np.array([6, 5]), np.array([5, 4]), np.array([5, 6])]
# run function:
indices = field._list_expandable_indices()
# check if the list members are the same:
self.assertEqual(len(etalon_indices), len(indices))
for index in etalon_indices:
self.assertTrue(array_is_in_list(index, indices))
# check the order of the list:
for i, index in enumerate(indices[: -1]):
next_index = indices[i + 1]
value = field._field[index[0], index[1]].value
next_value = field._field[next_index[0], next_index[1]].value
self.assertTrue(value >= next_value)
def test_update_occupancy_grid(self):
"""Tests the update_occupancy_grid method of the AttractorField"""
# testing without original occupancy grid:
field = AttractorField(goal=self.goal)
field.update_field(self.occupancy_grid)
self.assertTrue((field._occupancy_grid == self.occupancy_grid).all())
self.assertEqual((self.N, self.M), field._grid_shape)
self.assertTrue((np.array([self.N, self.M]) == field._grid_shape_arr).all())
# testing with original occupancy grid:
field = AttractorField(occupancy_grid=self.occupancy_grid, goal=self.goal)
# test wrong shape assertion:
with self.assertRaises(AssertionError):
field.update_field(np.zeros((self.N - 1, self.M)))
# check if nothing has changed:
new_grid = self.occupancy_grid.copy()
field.update_field(new_grid)
self.assertTrue((field._occupancy_grid == new_grid).all())
with self.assertRaises(AttributeError):
a = field._changed_indices
# check if something has changed:
new_grid[5, 5] = 1
new_grid[0, 3] = 0
etalon_changes = np.sort(np.array([[5, 5], [0, 3]]), axis=0)
field.update_field(new_grid)
changes = np.sort(np.array(field._changed_indices), axis=0)
self.assertTrue((field._occupancy_grid == new_grid).all())
self.assertEqual(len(field._changed_indices), 2)
i = 0
for ind in changes:
self.assertTrue((ind == etalon_changes[i]).all())
i += 1
def test_set_new_goal(self):
"""Tests the set_new_goal method of the AttractorField"""
field = AttractorField()
# checking assertion errors:
with self.assertRaises(AssertionError):
field.set_new_goal(np.array([1, 2, 3]))
field.set_new_goal(np.array([[1, 1]]))
# checking goal setting without occupancy_grid:
field.set_new_goal(self.goal)
self.assertTrue((field._goal == self.goal).all())
self.assertIsNone(field._occupancy_grid)
# checking goal setting with occupancy grid:
field = AttractorField()
field._occupancy_grid = self.occupancy_grid
field.set_new_goal(self.goal)
self.assertTrue((field._goal == self.goal).all())
self.assertIsNotNone(field._field)
def test_init_field(self):
"""Tests the _init_field method of the AttractorField"""
# testing without args:
field = AttractorField()
with self.assertRaises(AssertionError):
field._init_field()
with self.assertRaises(AttributeError):
field._field
# testing with provided occupancy grid:
field = AttractorField(occupancy_grid=self.occupancy_grid)
with self.assertRaises(AssertionError):
field._init_field()
with self.assertRaises(AttributeError):
field._field
# testing with provided goal:
field = AttractorField(goal=self.goal)
with self.assertRaises(AssertionError):
field._init_field()
with self.assertRaises(AttributeError):
field._field
# testing with everything provided:
field = AttractorField(occupancy_grid=self.occupancy_grid, goal=self.goal)
field._init_field()
self.assertIsNotNone(field._field)
def test_update_occupancy_grid(self):
"""Tests the update_occupancy_grid method of the AttractorField"""
# testing without original occupancy grid:
field = AttractorField(goal=self.goal)
field.update_occupancy_grid(self.occupancy_grid)
self.assertTrue((field._occupancy_grid == self.occupancy_grid).all())
self.assertEqual((self.N, self.M), field._grid_shape)
self.assertTrue((np.array([self.N, self.M]) == field._grid_shape_arr).all())
# testing with original occupancy grid:
field = AttractorField(occupancy_grid=self.occupancy_grid, goal=self.goal)
# test wrong shape assertion:
with self.assertRaises(AssertionError):
field.update_occupancy_grid(np.zeros((self.N - 1, self.M)))
# check if nothing has changed:
new_grid = self.occupancy_grid.copy()
field.update_occupancy_grid(new_grid)
self.assertTrue((field._occupancy_grid == new_grid).all())
with self.assertRaises(AttributeError):
a = field._changed_indices
# check if something has changed:
new_grid[5, 5] = 1
new_grid[0, 3] = 0
etalon_changes = np.sort(np.array([[5, 5], [0, 3]]), axis=0)
field.update_occupancy_grid(new_grid)
changes = np.sort(np.array(field._changed_indices), axis=0)
self.assertTrue((field._occupancy_grid == new_grid).all())
self.assertEqual(len(field._changed_indices), 2)
i = 0
for ind in changes:
self.assertTrue((ind == etalon_changes[i]).all())
i += 1
#########################################################
# Test with changes in the occupancy grid
goal = np.array([3, 3])
occ_grid_no_obst = self.occupancy_grid.copy()
# occupancy grid with an U shaped obstacle:
occ_grid_with_obst = self.occupancy_grid.copy()
occ_grid_with_obst[6, 4: 7] = 1
occ_grid_with_obst[7, 4] = 1
occ_grid_with_obst[7, 6] = 1
# testing the insertion of new obstacle:
field1 = AttractorField(occupancy_grid=occ_grid_with_obst, goal=goal)
field2 = AttractorField(occupancy_grid=occ_grid_no_obst, goal=goal)
field2.update_occupancy_grid(occ_grid_with_obst)
# testing the values:
result_vals1 = get_values_from_field(field1._field)
result_vals2 = get_values_from_field(field2._field)
self.assertTrue((result_vals1 == result_vals2).all())
# testing the grads:
for i in range(self.N):
for j in range(self.M):
self.assertTrue((field1._field[i, j].grad == field2._field[i, j].grad).all())
# testing when the obstacle dissappears:
field1 = AttractorField(occupancy_grid=occ_grid_no_obst, goal=goal)
field2 = AttractorField(occupancy_grid=occ_grid_with_obst, goal=goal)
field2.update_occupancy_grid(occ_grid_no_obst)
# testing the values:
result_vals1 = get_values_from_field(field1._field)
result_vals2 = get_values_from_field(field2._field)
self.assertTrue((result_vals1 == result_vals2).all())
# testing the grads:
for i in range(self.N):
for j in range(self.M):
self.assertTrue((field1._field[i, j].grad == field2._field[i, j].grad).all())
class GradController:
"""Gradient field based controller."""
def __init__(self, occupancy_grid, goal_pos, goal_ang, R, params):
"""Initializes the GradController object."""
self._occupancy_grid = occupancy_grid
self.set_new_goal(goal_pos, goal_ang)
self._R = R
# creating the attractive and repulsive gradient fields:
self._attractor = AttractorField(occupancy_grid=self._occupancy_grid,
goal=self._goal_pos)
self._repulsive = RepulsiveField(occupancy_grid=self._occupancy_grid,
R=self._R)
# setting up some params based on the json file if provided:
self._set_from_params(params)
def set_new_goal(self, goal_pos, goal_ang):
"""Sets a new goal."""
self._goal_pos = goal_pos
self._goal_ang = goal_ang
self._goal_pos_is_reached = False
self._goal_ang_is_reached = False
def get_cmd_vel(self, pose):
"""Gets the cmd_vel to send to the low level controller of the robot."""
self._set_pose(pose)
if self._goal_pos_is_reached:
if self._goal_ang_is_reached:
return np.array([0, 0])
else:
print("In End mode.")
return self._get_cmd_vel_end()
if self._goal_is_visible():
print("In Direct mode.")
return self._get_cmd_vel_direct()
else:
print("In Grad mode.")
return self._get_cmd_vel_grad()
def _set_pose(self, pose):
"""sets pose related variables."""
self._pos = pose[0:2]
self._x = pose[0]
self._y = pose[1]
self._psi = pose[2]
self._i = int(np.floor(pose[0]))
self._j = int(np.floor(pose[1]))
def _goal_is_visible(self):
"""Returns true if there is no obstacle between the robot and the goal and
the robot is at least self._min_obst_dist away from the nearest obstacle.
"""
val = self._repulsive.get_val(self._i, self._j)
if (0 < val < self._min_obst_dist):
return False
else:
# carry out a raytracing:
vect = self._goal_pos - self._pos
distance = np.linalg.norm(vect)
vect = vect / distance
N = int(np.floor(distance))
point = self._pos
for _ in range(N):
point += vect
i, j = int(np.floor(point[0])), int(np.floor(point[1]))
if self._occupancy_grid[i, j] == 1:
return False
return True
def _get_cmd_vel_end(self):
"""Controller for the case when the robot has already reached the goal
position, only the orientation has to be changed.
"""
ang_diff = self._get_ang_diff(self._goal_ang, self._psi)
if abs(ang_diff) > self._ang_tolerance:
return np.array([0, self._get_ang_vel(ang_diff, self._K_end)])
else:
self._goal_ang_is_reached = True
return np.array([0, 0])
def _get_cmd_vel_direct(self):
"""Controller for the case when the goal is visible from the current
position of the robot.
"""
vect = self._goal_pos - self._pos
desired_direction = np.arctan2(vect[1], vect[0])
ang_diff = self._get_ang_diff(desired_direction, self._psi)
# calculating the desired angular velocity:
des_ang_vel = self._get_ang_vel(ang_diff, self._K_direct)
# calculating the desired translational velocity:
des_trans_vel = self._get_trans_vel(ang_diff,
self._boundar_error_direct,
self._max_error_direct)
return np.array([des_trans_vel, des_ang_vel])
def _get_cmd_vel_grad(self):
"""Controller for the case when the robot is in the influenced
area of obstacles. It uses 3 pixels from the occupancy grid:
- 1: The pixel it is in.
- 2 and 3: neares neighboring pixels.
"""
grad1 = self._attractor.get_grad(self._i, self._j) +\
self._repulsive.get_grad(self._i, self._j)
# calculating the indices of the neighbours:
#i1, j1 = np.round(self._i), np.round(self._j)
if ((self._x - self._i) > 0.5): i1 = self._i + 1
else: i1 = self._i - 1
if ((self._y - self._j) > 0.5): j1 = self._j + 1
else: j1 = self._j - 1
#if(abs(self._psi) < np.pi / 2): i1 = self._i + 1
#else: i1 = self._i - 1
#
#if(self._psi > 0): j1 = self._j + 1
#else: j1 = self._j - 1
# getting the gradients of the neighbouring cells:
if (j1 >= 0) and (j1 < self._occupancy_grid.shape[1]):
grad2 = self._attractor.get_grad(self._i, j1) +\
self._repulsive.get_grad(self._i, j1)
else:
grad2 = np.array([0, 0])
if (i1 >= 0) and (i1 < self._occupancy_grid.shape[0]):
grad3 = self._attractor.get_grad(i1, self._j) +\
self._repulsive.get_grad(i1, self._j)
else:
grad3 = np.array([0, 0])
#grad = self._norm_grad(grad1) + self._norm_grad(grad2) +\
# self._norm_grad(grad3)
grad = grad1 + grad2 + grad3
# getting the desired angular and translational velocity:
desired_direction = np.arctan2(grad[1], grad[0])
ang_diff = self._get_ang_diff(desired_direction, self._psi)
# calculating the desired angular velocity:
des_ang_vel = self._get_ang_vel(ang_diff, self._K_grad)
# calculating the desired translational velocity:
des_trans_vel = self._get_trans_vel(ang_diff, self._boundar_error_grad,
self._max_error_grad)
return np.array([des_trans_vel, des_ang_vel])
def _norm_grad(self, grad):
"""Normalizes a gradient"""
eps = 1e-6
length = np.linalg.norm(grad)
if length > eps:
return grad / length
else:
return np.array([0, 0])
def _get_ang_vel(self, ang_diff, K):
"""Gets the desired velocity based on a simple proportional
relationship with the error. It also respects the max angular velocity."""
des_ang_vel = -self._K_direct * ang_diff
if abs(des_ang_vel) > self._max_ang_vel:
des_ang_vel = np.sign(des_ang_vel) * self._max_ang_vel
return des_ang_vel
def _get_trans_vel(self, ang_diff, boundary_error, max_error):
"""Gets the desired translational velocity for the robot.
- if abs(ang_diff) < boundary_error: max velocity
- if boundary_error < abs(ang_diff) < max_error: linearly decreasing
velocity between max velocity and 0
- else: 0 translational velocity.
"""
if abs(ang_diff) < boundary_error:
return self._max_trans_vel
elif abs(ang_diff) < max_error:
ratio = (abs(ang_diff) - boundary_error) / (max_error -
boundary_error)
return self._max_trans_vel * (1 - ratio)
else:
return 0
def _get_ang_diff(self, desired, real):
"""gets the orientation difference between the desired
and the real orientation. The value is always in the range [-pi, pi]
"""
diff = real - desired
if abs(diff) < np.pi:
return diff
else:
return diff - np.sign(diff) * 2 * np.pi
@property
def goal_is_reached(self):
"""Returns true if the goal is reached."""
return self._goal_pos_is_reached and self._goal_ang_is_reached
def _set_from_params(self, params):
"""Sets up some values based on params."""
# general
self._pos_tolerance = params["GradController"]["general"][
"pos_tolerance"]
self._ang_tolerance = params["GradController"]["general"][
"ang_tolerance"]
self._max_trans_vel = params["GradController"]["general"][
"max_trans_vel"]
self._max_trans_acc = params["GradController"]["general"][
"max_trans_acc"]
self._max_ang_vel = params["GradController"]["general"]["max_ang_vel"]
self._max_ang_acc = params["GradController"]["general"]["max_ang_acc"]
# grad_mode
self._K_grad = params["GradController"]["grad_mode"]["K"]
self._boundar_error_grad = params["GradController"]["grad_mode"][
"boundary_error"]
self._max_error_grad = params["GradController"]["grad_mode"][
"max_error"]
self._grad_vel_scaling = params["GradController"]["grad_mode"][
"grad_vel_scaling"]
# direct_mode:
self._min_obst_dist = params["GradController"]["direct_mode"][
"min_obst_dist"]
self._K_direct = params["GradController"]["direct_mode"]["K"]
self._boundar_error_direct = params["GradController"]["direct_mode"][
"boundary_error"]
self._max_error_direct = params["GradController"]["direct_mode"][
"max_error"]
# end_mode:
self._K_end = params["GradController"]["end_mode"]["K_end"]