def test_run_once_success(self):
pos1 = np.array([101, 102])
pos2 = np.array([40, 20])
self.planner.args.env.cc.visible = MagicMock(return_value=True)
mocked_report_success = MagicMock()
mocked_report_fail = MagicMock()
# mock the returning sampling point
self.sampler.get_next_pos = MagicMock(
return_value=(pos1, mocked_report_success, mocked_report_fail))
# this will be run on root tree
self.planner.run_once()
self.sampler.get_next_pos.assert_called_once_with()
mocked_report_fail.assert_not_called()
mocked_report_success.assert_called_once_with(
pos=ANY, nn=ANY, rand_pos=MockNumpyEquality(pos1))
self.assertEqual(MockNumpyEquality(pos1), self.planner.nodes[-1].pos)
# reset mocks
mocked_report_success.reset_mock()
mocked_report_fail.reset_mock()
self.sampler.get_next_pos = MagicMock(
return_value=(pos2, mocked_report_success, mocked_report_fail))
# this will be run on goal root tree
self.planner.run_once()
self.sampler.get_next_pos.assert_called_once_with()
mocked_report_fail.assert_not_called()
mocked_report_success.assert_called_once_with(
pos=ANY, nn=ANY, rand_pos=MockNumpyEquality(pos2))
def test_run_once_success(self):
pos1 = np.array([345, 345])
self.planner.args.env.cc.visible = MagicMock(return_value=True)
mocked_report_success = MagicMock()
mocked_report_fail = MagicMock()
# mock the returning sampling point
self.sampler.get_next_pos = MagicMock(return_value=(
pos1,
self.planner._disjointed_trees[-1],
None,
mocked_report_success,
mocked_report_fail,
), )
self.flush_rrdt_sampler_pending_restarts(self.sampler)
num_nodes_before = len(self.planner.nodes)
self.planner.run_once()
self.assertTrue(len(self.planner.nodes) >= num_nodes_before + 1)
self.sampler.get_next_pos.assert_called_once_with()
mocked_report_fail.assert_not_called()
mocked_report_success.assert_called_once_with(
pos=MockNumpyEquality(pos1),
newnode=ANY,
)
self.assertTrue(
any(np.isclose(pos1, n.pos).all() for n in self.planner.nodes))
def test_run_once_failed(self):
pos1 = np.array([101, 102])
self.planner.args.env.cc.visible = MagicMock(return_value=False)
mocked_report_success = MagicMock()
mocked_report_fail = MagicMock()
# mock the returning sampling point
self.sampler.get_next_pos = MagicMock(
return_value=(pos1, mocked_report_success, mocked_report_fail))
self.planner.run_once()
self.sampler.get_next_pos.assert_called_once_with()
mocked_report_fail.assert_called_once_with(pos=MockNumpyEquality(pos1),
free=False)
mocked_report_success.assert_not_called()
def test_random_walk(self):
for i in range(self.sampler.num_dtrees):
desire_direction = np.random.rand(self.sampler.args.num_dim)
desire_direction /= np.linalg.norm(desire_direction)
self.sampler.p_manager.particles[i].draw_sample = MagicMock(
return_value=desire_direction)
# test if probability affects the random choice
ori_pos = self.sampler.p_manager.get_pos(i)
new_pos = self.sampler.random_walk(i)
# ensure that the returned position is in the correct direction that the
# particle reported
particle_returned_dir = new_pos - ori_pos
particle_returned_dir /= np.linalg.norm(particle_returned_dir)
self.assertEqual(
MockNumpyEquality(particle_returned_dir, almost_equal=True),
desire_direction,
)
def test_run_once_failed(self):
# PRM won't knows whether the sampled point is connectable or not before
# graph construction
pos1 = np.array([4, 2])
self.planner.args.env.cc.visible = MagicMock(return_value=False)
mocked_report_success = MagicMock()
mocked_report_fail = MagicMock()
# mock the returning sampling point
self.sampler.get_next_pos = MagicMock(
return_value=(pos1, mocked_report_success, mocked_report_fail)
)
self.planner.run_once()
self.sampler.get_next_pos.assert_called_once_with()
mocked_report_fail.assert_not_called()
mocked_report_success.assert_not_called()
self.assertEqual(MockNumpyEquality(pos1), self.planner.nodes[-1].pos)