def test_init(self):
"""Tests the __init__ function of the GradController"""
controller = GradController(occupancy_grid=self.occupancy_grid,
goal_pos=self.goal_pos,
goal_ang=self.goal_ang,
R=self.R,
params=self.params)
self.assertTrue((controller._occupancy_grid == self.occupancy_grid).all())
self.assertTrue((controller._goal_pos == self.goal_pos).all())
self.assertEqual(controller._goal_ang, self.goal_ang)
self.assertEqual(controller._R, self.R)
self.assertIsInstance(controller._attractor, AttractorField)
self.assertIsInstance(controller._repulsive, RepulsiveField)
# params which are set from the json file:
self.assertEqual(controller._pos_tolerance, 0.1)
self.assertEqual(controller._ang_tolerance, 0.2)
self.assertEqual(controller._max_trans_vel, 1)
self.assertEqual(controller._max_trans_acc, 1)
self.assertEqual(controller._max_ang_vel, 1.5708)
self.assertEqual(controller._max_ang_acc, 1.5708)
self.assertEqual(controller._K_grad, 0.5)
self.assertEqual(controller._boundar_error_grad, 0.2)
self.assertEqual(controller._max_error_grad, 1.0472)
self.assertEqual(controller._grad_vel_scaling, False)
self.assertEqual(controller._K_direct, 0.5)
self.assertEqual(controller._boundar_error_direct, 0.2)
self.assertEqual(controller._max_error_direct, 1.0472)
self.assertEqual(controller._K_end, 0.5)
def test_get_ang_vel(self):
"""Tests the _get_ang_vel function of the GradController"""
controller = GradController(occupancy_grid=self.occupancy_grid,
goal_pos=self.goal_pos,
goal_ang=self.goal_ang,
R=self.R,
params=self.params)
controller._max_ang_vel = 0.1
out = controller._get_ang_vel(0.16, 0.5)
self.assertEqual(out, -0.08)
self.assertFalse(controller._goal_ang_is_reached)
out = controller._get_ang_vel(-0.16, 0.5)
self.assertEqual(out, 0.08)
self.assertFalse(controller._goal_ang_is_reached)
out = controller._get_ang_vel(0.3, 0.5)
self.assertEqual(out, -0.1)
self.assertFalse(controller._goal_ang_is_reached)
out = controller._get_ang_vel(-0.3, 0.5)
self.assertEqual(out, 0.1)
self.assertFalse(controller._goal_ang_is_reached)
def test_set_pose(self):
"""Tests the _set_pose function of the GradController"""
controller = GradController(occupancy_grid=self.occupancy_grid,
goal_pos=self.goal_pos,
goal_ang=self.goal_ang,
R=self.R,
params=self.params)
pose = np.array([4.3, 8.7, -0.3])
controller._set_pose(pose)
self.assertTrue((controller._pos == np.array([4.3, 8.7])).all())
self.assertEqual(controller._x, 4.3)
self.assertEqual(controller._y, 8.7)
self.assertEqual(controller._psi, -0.3)
self.assertEqual(controller._i, 4)
self.assertEqual(controller._j, 8)
def test_goal_is_visible(self):
"""Tests the _goal_is_visible function of the GradController"""
controller = GradController(occupancy_grid=self.occupancy_grid,
goal_pos=self.goal_pos,
goal_ang=self.goal_ang,
R=self.R,
params=self.params)
# there is an obstacle between the position and the goal:
controller._occupancy_grid[6, 5] = 1
pose = np.array([8.3, 5.6, 0.3])
controller._set_pose(pose)
self.assertFalse(controller._goal_is_visible())
# there is no obstacle between the position and the goal:
pose = np.array([5.6, 8.3, 0.3])
controller._set_pose(pose)
with open(sim_param_file) as f:
sim_params = json.load(f)
# generating occupancy grid:
occ_grid = set_up_occ_grid(params=sim_params)
# Environment:
env = PointRobotEnv(occ_grid=occ_grid,
Ts=params["general"]["Ts"],
sim_params=sim_params
)
# High level controller:
grad_controller = GradController(occupancy_grid=occ_grid,
goal_pos=np.array([sim_params["goal"]["x"], sim_params["goal"]["y"]]),
goal_ang=sim_params["goal"]["psi"],
R=params["GradController"]["general"]["R"],
params=params
)
# Low level controller:
low_level_controller = LowLevelController(params=params)
# main simulation loop:
collision = False
state = env.get_state()
while (not grad_controller.goal_is_reached) and (not collision):
start_time = time.time()
# getting command velocities from the high level controller:
pose = np.array([state[0], state[1], state[3]])
cmd_vel = grad_controller.get_cmd_vel(pose)
def test_get_trans_vel(self):
"""Tests the _get_trans_vel function of the GradController"""
controller = GradController(occupancy_grid=self.occupancy_grid,
goal_pos=self.goal_pos,
goal_ang=self.goal_ang,
R=self.R,
params=self.params)
out = controller._get_trans_vel(0.19, 0.2, 0.6)
self.assertTrue(np.isclose(out, controller._max_trans_vel))
out = controller._get_trans_vel(-0.19, 0.2, 0.6)
self.assertTrue(np.isclose(out, controller._max_trans_vel))
out = controller._get_trans_vel(0.2, 0.2, 0.6)
self.assertTrue(np.isclose(out, controller._max_trans_vel))
out = controller._get_trans_vel(-0.2, 0.2, 0.6)
self.assertTrue(np.isclose(out, controller._max_trans_vel))
out = controller._get_trans_vel(0.4, 0.2, 0.6)
self.assertTrue(np.isclose(out, controller._max_trans_vel / 2))
out = controller._get_trans_vel(-0.4, 0.2, 0.6)
self.assertTrue(np.isclose(out, controller._max_trans_vel / 2))
out = controller._get_trans_vel(0.6, 0.2, 0.6)
self.assertEqual(out, 0)
out = controller._get_trans_vel(-0.6, 0.2, 0.6)
self.assertEqual(out, 0)
out = controller._get_trans_vel(0.61, 0.2, 0.6)
self.assertEqual(out, 0)
out = controller._get_trans_vel(-0.61, 0.2, 0.6)
self.assertEqual(out, 0)
def test_get_cmd_vel_end(self):
"""Tests the _get_cmd_vel_end function of the GradController"""
controller = GradController(occupancy_grid=self.occupancy_grid,
goal_pos=self.goal_pos,
goal_ang=self.goal_ang,
R=self.R,
params=self.params)
controller._max_ang_vel = 0.1
controller._ang_tolerance = 0.05
pose = np.array([0, 0, 0.16])
controller._set_pose(pose)
out = controller._get_cmd_vel_end()
self.assertTrue((out == np.array([0, -0.08])).all())
self.assertFalse(controller._goal_ang_is_reached)
pose = np.array([0, 0, -0.16])
controller._set_pose(pose)
out = controller._get_cmd_vel_end()
self.assertTrue((out == np.array([0, 0.08])).all())
self.assertFalse(controller._goal_ang_is_reached)
pose = np.array([0, 0, 1.6])
controller._set_pose(pose)
out = controller._get_cmd_vel_end()
self.assertTrue((out == np.array([0, -0.1])).all())
self.assertFalse(controller._goal_ang_is_reached)
pose = np.array([0, 0, -1.6])
controller._set_pose(pose)
out = controller._get_cmd_vel_end()
self.assertTrue((out == np.array([0, 0.1])).all())
self.assertFalse(controller._goal_ang_is_reached)
pose = np.array([0, 0, 0.04])
controller._set_pose(pose)
out = controller._get_cmd_vel_end()
self.assertTrue((out == np.array([0, 0])).all())
self.assertTrue(controller._goal_ang_is_reached)
pose = np.array([0, 0, -0.04])
controller._set_pose(pose)
out = controller._get_cmd_vel_end()
self.assertTrue((out == np.array([0, 0])).all())
self.assertTrue(controller._goal_ang_is_reached)
def test_get_ang_diff(self):
"""Tests the _get_ang_diff function of the GradController"""
controller = GradController(occupancy_grid=self.occupancy_grid,
goal_pos=self.goal_pos,
goal_ang=self.goal_ang,
R=self.R,
params=self.params)
pi = np.pi
desired, real = 5. / 6. * pi, 1. / 6. * pi
self.assertTrue(np.isclose(controller._get_ang_diff(desired, real), -4. / 6. * pi))
desired, real = 1. / 6. * pi, 5. / 6. * pi
self.assertTrue(np.isclose(controller._get_ang_diff(desired, real), 4. / 6. * pi))
desired, real = -5. / 6. * pi, -1. / 6. * pi
self.assertTrue(np.isclose(controller._get_ang_diff(desired, real), 4. / 6. * pi))
desired, real = -1. / 6. * pi, -5. / 6. * pi
self.assertTrue(np.isclose(controller._get_ang_diff(desired, real), -4. / 6. * pi))
desired, real = 1. / 6. * pi, -3. / 6. * pi
self.assertTrue(np.isclose(controller._get_ang_diff(desired, real), -4. / 6. * pi))
desired, real = -1. / 6. * pi, 3. / 6. * pi
self.assertTrue(np.isclose(controller._get_ang_diff(desired, real), 4. / 6. * pi))
desired, real = 5. / 6. * pi, -3. / 6. * pi
self.assertTrue(np.isclose(controller._get_ang_diff(desired, real), 4. / 6. * pi))
desired, real = -5. / 6. * pi, 3. / 6. * pi
self.assertTrue(np.isclose(controller._get_ang_diff(desired, real), -4. / 6. * pi))
desired, real = 3. / 6. * pi, -5. / 6. * pi
self.assertTrue(np.isclose(controller._get_ang_diff(desired, real), 4. / 6. * pi))
desired, real = -3. / 6. * pi, 5. / 6. * pi
self.assertTrue(np.isclose(controller._get_ang_diff(desired, real), -4. / 6. * pi))