class TestRRTSteps(unittest.TestCase): def setUp(self): # RRT 1: RRT with 1 node, in 3 dimensions self.rrt1 = RRT(start_state=(0., 0., 0.), goal_state=(1, 1, 1), dim_ranges=[(-2, 2)] * 3, step_size=0.1) # RRT 2: RRT with 2 nodes, in 2 dimensions # (start) --> (node1) self.rrt2 = RRT(start_state=(0., 0.), goal_state=(1, 1), dim_ranges=[(-2, 2), (-2, 2)], step_size=0.05) self.rrt2_node1 = self.rrt2.start.add_child(state=(0.5, 0.25)) # RRT 3: RRT with 2 nodes, in 3 dimensions self.rrt3 = RRT(start_state=(0.25, 0.25, 0.25), goal_state=(0.75, 0.75, 0.75), dim_ranges=[(0, 1)] * 3, step_size=0.1) self.rrt3_node1 = self.rrt3.start.add_child(state=(0.5, 0.5, 0.5)) self.rrt3_node2 = self.rrt3_node1.add_child(state=(0.75, 0.75, 0.7)) def test_get_random_sample(self): sample = self.rrt1._get_random_sample() lower = np.array([d[0] for d in self.rrt1.dim_ranges]) upper = np.array([d[1] for d in self.rrt1.dim_ranges]) self.assertEqual(len(sample), len(self.rrt1.dim_ranges)) self.assertTrue(np.all(sample >= lower)) self.assertTrue(np.all(sample < upper)) def test_get_nearest_neighbor(self): sample1 = np.array([0.1, 0.2, 0.3]) neighbor1 = self.rrt1._get_nearest_neighbor(sample1) self.assertIs(neighbor1, self.rrt1.start) sample2 = np.array([0.25, 0.25]) neighbor2 = self.rrt2._get_nearest_neighbor(sample2) self.assertIs(neighbor2, self.rrt2_node1) sample3 = np.array([0.7, 0.7, 0.7]) neighbor3 = self.rrt3._get_nearest_neighbor(sample3) self.assertIs(neighbor3, self.rrt3_node2) def test_extend_sample(self): sample1 = np.array([0, 0, 0.1]) sample1_node = self.rrt1._extend_sample(sample1, self.rrt1.start) self.assertIs(sample1_node.parent, self.rrt1.start) self.assertAlmostEqual(np.linalg.norm(sample1 - sample1_node.state), 0., places=3) sample2 = np.array([1, 1, 1]) sample2_ext = np.array([0.0577, 0.0577, 0.0577]) sample2_node = self.rrt1._extend_sample(sample2, self.rrt1.start) self.assertAlmostEqual(np.linalg.norm(sample2_ext - sample2_node.state), 0., places=3) def test_check_for_completion(self): complete1 = self.rrt2._check_for_completion(self.rrt2_node1) self.assertFalse(complete1) complete2 = self.rrt3._check_for_completion(self.rrt3_node2) self.assertTrue(complete2) def test_trace_path_from_start(self): path1 = self.rrt1._trace_path_from_start(self.rrt1.start) self.assertEqual(len(path1), 1) self.assertTrue(np.all(path1[0] == self.rrt1.start.state)) path2 = self.rrt2._trace_path_from_start(self.rrt2_node1) self.assertEqual(len(path2), 2) self.assertTrue(np.all(path2[0] == self.rrt2.start.state)) self.assertTrue(np.all(path2[1] == self.rrt2_node1.state)) self.rrt3_node2.children.append(self.rrt3.goal) self.rrt3.goal.parent = self.rrt3_node2 path3 = self.rrt3._trace_path_from_start() self.assertEqual(len(path3), 4) self.assertTrue(np.all(path3[0] == self.rrt3.start.state)) self.assertTrue(np.all(path3[1] == self.rrt3_node1.state)) self.assertTrue(np.all(path3[2] == self.rrt3_node2.state)) self.assertTrue(np.all(path3[3] == self.rrt3.goal.state)) def test_check_for_collision(self): sample = np.array([0.5, 0.5, 0.5]) in_collision = self.rrt1._check_for_collision(sample) self.assertFalse(in_collision)