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