def test_remove_knot_point(generate_trajectory_list): t1, t2, t3 = generate_trajectory_list t1.remove_knot_point(0) t2.remove_knot_point(1) set_standard_testing_random_seed() data3 = np.random.randn(100, 5) t3.remove_knot_point(index=50) # Test the overload without an index. t3.remove_knot_point() assert t1.size == 0 assert np.allclose(t1.data, np.empty([0, t1.dim])) assert np.allclose(t2.data, [[1, 1, 1], [3, 3, 3]]) assert np.allclose(t3.data, np.vstack([data3[:50], data3[51:-1]])) # Test invalid remove_knot_point. # Note that the size of each trajectory has now increased by one. # Invalid indices. with pytest.raises(IndexError): t1.remove_knot_point(0) with pytest.raises(IndexError): t2.remove_knot_point(-1) with pytest.raises(IndexError): t3.remove_knot_point(200)
def test_add_knot_point(generate_trajectory_list): t1, t2, t3 = generate_trajectory_list t1.add_knot_point(State([1, 1], [1, 1]), 0) t2.add_knot_point(knot_point=State([1.5, 1.5], [1.5]), index=1) # Test the overload without an index. set_standard_testing_random_seed() data3 = np.random.randn(100, 5) t3.add_knot_point(knot_point=State(3, 2)) assert np.allclose(t1.data, [[1, 1, 1, 1], [0, 0, 0, 0]]) assert np.allclose(t2.data, [[1, 1, 1], [1.5, 1.5, 1.5], [2, 2, 2], [3, 3, 3]]) assert np.allclose(t3.data, np.vstack([data3, [0] * 5])) # Test invalid add_knot_point. # Note that the size of each trajectory has now increased by one. # Invalid states. with pytest.raises(ValueError): t1.add_knot_point(State(2, 1), 0) with pytest.raises(ValueError): t2.add_knot_point(State(3, 1), 0) with pytest.raises(ValueError): t3.add_knot_point(State(2, 3), 0) # Invalid indices. with pytest.raises(IndexError): t1.add_knot_point(State(2, 2), -1) with pytest.raises(IndexError): t2.add_knot_point(State(2, 1), 5) with pytest.raises(IndexError): t3.add_knot_point(State(3, 2), 200)
def test_setitem(generate_trajectory_list): t1, t2, t3 = generate_trajectory_list t1[0] = State([1, 1], [1, 1]) t2[1] = State([1, 1], [1]) set_standard_testing_random_seed() data3 = np.random.randn(100, 5) for i in range(100): t3[i] = State(data3[i, :3], data3[i, 3:]) assert np.allclose(t1.data, [1, 1, 1, 1]) assert np.allclose(t2.data, [[1, 1, 1], [1, 1, 1], [3, 3, 3]]) assert np.allclose(t3.data, data3) # Test invalid setitem with pytest.raises(IndexError): t1[-2] = State(2, 2) with pytest.raises(IndexError): t2[-4] = State(2, 1) with pytest.raises(IndexError): t3[100] = State(3, 2) # Note that this check for invalid state setting is only applicable for # the python binding. In cpp, this is done through the [] operator that # returns a reference and thus no state validation can take place. with pytest.raises(ValueError): t1[0] = State(0, 0) with pytest.raises(ValueError): t2[0] = State(2, 2) with pytest.raises(ValueError): t3[0] = State(2, 2)
def test_data(generate_trajectory_list): t1, t2, t3 = generate_trajectory_list # Test getting data. # Data for t3 set_standard_testing_random_seed() data3 = np.random.randn(100, 5) assert np.allclose(t1.data, [0, 0, 0, 0]) assert np.allclose(t2.data, [[1, 1, 1], [2, 2, 2], [3, 3, 3]]) assert np.allclose(t3.data, data3) # Test setting data. t1.data = [[1, 2, 3, 4]] t2.data = np.ones([3, 3]) set_standard_testing_random_seed() data3 = np.random.randn(100, 5) t3.data = data3 assert np.allclose(t1.data, [1, 2, 3, 4]) assert np.allclose(t2.data, [[1, 1, 1], [1, 1, 1], [1, 1, 1]]) assert np.allclose(t3.data, data3) # test invalid set data. with pytest.raises(ValueError): t1.data = [[1, 2, 3]] with pytest.raises(ValueError): t2.data = np.zeros([4, 4]) with pytest.raises(ValueError): t3.data = np.zeros([100, 4])
def test_compute(generate_controller_list): c1, c2 = generate_controller_list set_standard_testing_random_seed() data = np.random.randn(6) assert np.allclose(c1.compute().data, [0]) assert np.allclose(c2.compute().data, data)
def test_world_to_canvas(): set_standard_testing_random_seed() world_coords = np.random.randn(10, 2) canvas_coords = world_to_canvas( world_coords=world_coords, resolution=0.1, canvas_size=(200, 200) ) assert canvas_coords.shape == (10, 2) assert canvas_coords.dtype == np.int32
def test_canvas_to_world_multiple_points(): set_standard_testing_random_seed() world_coords = canvas_to_world( canvas_coords=np.random.randint(200, size=(10, 2)), resolution=0.1, canvas_size=(200, 200), ) assert world_coords.shape == (10, 2) assert world_coords.dtype == np.float64
def generate_trajectory_list(): # List of trajectories constructed in different ways. t1 = Trajectory(knot_point=State(2, 2)) t2 = Trajectory(knot_points=[ State([1, 1], [1]), State([2, 2], [2]), State([3, 3], [3]) ]) set_standard_testing_random_seed() t3 = Trajectory(data=np.random.randn(100, 5), pose_size=3, velocity_size=2) return t1, t2, t3
def test_remove_knot_points(generate_trajectory_list): t1, t2, t3 = generate_trajectory_list t1.remove_knot_points([0]) t2.remove_knot_points([1, 2]) set_standard_testing_random_seed() data3 = np.random.randn(100, 5) t3.remove_knot_points(indices=[0, 1, 2, 99, 98, 97]) assert np.allclose(t1.data, np.empty([0, t1.dim])) assert np.allclose(t2.data, [[1, 1, 1]]) assert np.allclose(t3.data, data3[3:97, :])
def test_addition(generate_trajectory_list): t1, t2, t3 = generate_trajectory_list t1 += Trajectory(State([1, 1], [1, 1])) assert t1.size == 2 assert np.allclose(t1.data, [[0, 0, 0, 0], [1, 1, 1, 1]]) t2 += t2 assert t2.size == 6 assert np.allclose( t2.data, [[1, 1, 1], [2, 2, 2], [3, 3, 3], [1, 1, 1], [2, 2, 2], [3, 3, 3]]) set_standard_testing_random_seed() data3 = np.random.randn(200, 5) t4 = t3 + Trajectory(data3, 3, 2) # Make sure that t3 is unchanged. assert t3.size == 100 assert t4.size == 300 assert np.allclose(t4.data, np.vstack([t3.data, data3])) # Test invalid addition. with pytest.raises(ValueError): t1 += t2 with pytest.raises(ValueError): t2 += t3 with pytest.raises(ValueError): t3 += t1 with pytest.raises(ValueError): t1 += Trajectory(State(0, 0)) with pytest.raises(ValueError): t2 += Trajectory(State(2, 2)) with pytest.raises(ValueError): t2 += Trajectory(State(2, 2))
def generate_controller_list(): c1 = ConstantController([0]) set_standard_testing_random_seed() c2 = ConstantController(np.random.randn(6)) return c1, c2
def generate_points_list(): set_standard_testing_random_seed() points = np.random.randn(10, 2) homogeneous_points = np.random.randn(10, 3) return points, homogeneous_points
def generate_controller_list(): set_standard_testing_random_seed() c1 = CustomController(np.random.randn(3)) c2 = CustomController(np.random.randn(6)) return c1, c2