def test_create_like(generate_control_input_list):
for ci in generate_control_input_list:
assert ControlInput.create_like(control_input=ci).size == ci.size
assert np.allclose(
ControlInput.create_like(ci).data, np.zeros(ci.size))
def test_equality(generate_control_input_list):
i1, i2, i3, i4 = generate_control_input_list
assert i1 == ControlInput([0, 0])
assert i2 == ControlInput([1, 2, 3])
assert i1 != i2
def test_derivative_computation(generate_dubin_model_list):
d1, d2 = generate_dubin_model_list
# We only test that the function is called and returns properly, not the
# actual computation as that cpp side tests this.
# Test with positional arguments.
der1 = d1.compute_state_derivative(
robot_state=State([1, 2, 0], []), control_input=ControlInput(1)
)
# In this case, the control input controls the angular velocity and the
# speed i# constant. Hence the robot must move horizontally (theta=0) with
# the given speed.
assert np.allclose(der1.data, [1, 0, 0])
# Making sure that the derivative can only be computed from control inputs
# that are of the correct dimensions.
with pytest.raises(ValueError):
d1.compute_state_derivative(State(0, 3), ControlInput(1))
with pytest.raises(ValueError):
d1.compute_state_derivative(State(3, 1), ControlInput(1))
with pytest.raises(ValueError):
d2.compute_state_derivative(State(2, 0), ControlInput(1))
with pytest.raises(ValueError):
d2.compute_state_derivative(State(3, 0), ControlInput(2))
def generate_control_input_list():
# List of velocities constructed in every single way.
# This also tests initialization.
ci1 = ControlInput(size=2)
ci2 = ControlInput([1, 2, 3])
ci3 = ControlInput((4, 3, 2, 1))
ci4 = ControlInput(data=np.array([1, 1, 2, 3, 5], dtype=np.float))
return ci1, ci2, ci3, ci4
def test_derivative_computation(generate_custom_model_list):
c1, c2, c3 = generate_custom_model_list
der1 = c1.compute_state_derivative(State([1, 1, 1], [1, 1]),
ControlInput([1, 2, 3, 4, 5]))
der2 = c2.compute_state_derivative(State([1], [0]), ControlInput([-2, -1]))
# Test with positional arguments.
der3 = c3.compute_state_derivative(
robot_state=State([1, -1], [-0.1, 7, 9.5, 0]),
control_input=ControlInput([5, 0, -100, -5, 4, 0]),
)
assert np.allclose(der1.data, [15.0 + 3.45] * 5)
assert np.allclose(der2.data, [-2] * 2)
assert np.allclose(der3.data, [18 + 10] * 6)
def test_derivative_computation(generate_ackermann_model_list):
a1, a2 = generate_ackermann_model_list
# We only test that the function is called and returns properly, not the
# actual computation as the cpp side tests this.
# Test with positional arguments.
der1 = a1.compute_state_derivative(robot_state=State([1, 2, 0, 0], []),
control_input=ControlInput(2))
assert np.allclose(der1.data, [0, 0, 0, 0])
# Making sure that the derivative can only be computed from control inputs
# that are of the correct dimensions.
with pytest.raises(ValueError):
a1.compute_state_derivative(State(0, 4), ControlInput(2))
with pytest.raises(ValueError):
a1.compute_state_derivative(State(4, 1), ControlInput(2))
with pytest.raises(ValueError):
a2.compute_state_derivative(State(3, 0), ControlInput(2))
with pytest.raises(ValueError):
a2.compute_state_derivative(State(4, 0), ControlInput(3))
# It should also throw an exception if the steering angle is invalid.
with pytest.raises(ValueError):
a1.compute_state_derivative(State([0.0, 0.0, 0.0, 2 * np.pi / 3], []),
ControlInput(2))
with pytest.raises(ValueError):
a1.compute_state_derivative(
State([0.0, 0.0, np.pi / 2, -2 * np.pi / 3], []), ControlInput(2))
def test_addition(generate_control_input_list):
ci1, ci2, ci3, ci4 = generate_control_input_list
ci1 += ControlInput(2)
assert np.allclose(ci1.data, [0, 0])
res = ci2 + ControlInput(3)
assert np.allclose(res.data, ci2.data)
res = ci3 + ControlInput([0, 1, 2, 3])
assert np.allclose(res.data, [4, 4, 4, 4])
res = ci4 + ControlInput([-1, -2, -3, -4, -5])
assert np.allclose(res.data, [0, -1, -1, -1, 0])
# Test invalid addition.
with pytest.raises(ValueError):
ci1 += ControlInput(3)
with pytest.raises(ValueError):
res = ci2 + ci3
def test_subtraction(generate_control_input_list):
ci1, ci2, ci3, ci4 = generate_control_input_list
ci1 -= ControlInput(2)
assert np.allclose(ci1.data, [0, 0])
res = ci2 - ControlInput(3)
assert np.allclose(res.data, ci2.data)
res = ci3 - ControlInput([3, 2, 1, 0])
assert np.allclose(res.data, [1, 1, 1, 1])
res = ci4 - ControlInput([5, -4, 3, -2, 1])
assert np.allclose(res.data, [-4, 5, -1, 5, 4])
# Test invalid addition.
with pytest.raises(ValueError):
ci1 -= ControlInput(3)
with pytest.raises(ValueError):
res = ci2 - ci3
def test_derivative_computation(generate_tricycle_model_list):
t1, t2 = generate_tricycle_model_list
# We only test that the function is called and returns properly, not the
# actual computation as the cpp side tests this.
# Test with positional arguments.
der1 = t1.compute_state_derivative(robot_state=State([1, 2, 3, 4], []),
control_input=ControlInput(2))
assert np.allclose(der1.data, [0, 0, 0, 0])
# Making sure that the derivative can only be computed from control inputs
# that are of the correct dimensions.
with pytest.raises(ValueError):
t1.compute_state_derivative(State(0, 4), ControlInput(2))
with pytest.raises(ValueError):
t1.compute_state_derivative(State(4, 1), ControlInput(2))
with pytest.raises(ValueError):
t2.compute_state_derivative(State(3, 0), ControlInput(2))
with pytest.raises(ValueError):
t2.compute_state_derivative(State(4, 0), ControlInput(3))
def test_step_computation(generate_integrator):
euler_integrator = generate_integrator
# We don't check the actual computation as the cpp test covers this. We
# only check if the function calls and returns properly.
# Test with positional arguments.
updated_state = euler_integrator.step(robot_state=State([1, 2, 0], []),
control_input=ControlInput([1, -1]),
dt=0.1)
assert updated_state.size == 3
assert updated_state.pose.size == 3
assert updated_state.is_velocity_empty()
def test_step_computation(generate_integrator):
custom_integrator = generate_integrator
# Testing a and b
assert custom_integrator.a == 2
assert custom_integrator.b == -1
# Testing the actual computation. We make sure that the interface works.
# Test with positional arguments.
updated_state = custom_integrator.step(State([1, 2, 3, 4], []),
ControlInput([1, -1, 2, -3]), 0.1)
assert np.allclose(updated_state.data, [0.1, 0.5, 0.4, 1.1])
def test_construction(generate_integrator_list):
integrator_type_list, integrator_class_list = generate_integrator_list
kinematic_model = DiffdriveModel(1.0, 1.0)
# Making sure that the right integrator is constructed.
for integrator_type, integrator_class in zip(integrator_type_list,
integrator_class_list):
integrator = integrator_from_type(integrator_type=integrator_type,
kinematic_model=kinematic_model)
assert isinstance(integrator, integrator_class)
# Also making sure that the integrator is usable.
assert np.allclose(
integrator.step(State(3, 0), ControlInput(2), 0.05).data,
[0.0, 0.0, 0.0])
def test_construction_with_temporary(generate_integrator_list):
# Test the function with a temporarily created KinematicModel object.
# This is to test for any cpp lifetime management weirdness.
integrator_type_list, integrator_class_list = generate_integrator_list
# Making sure that the right integrator is constructed.
for integrator_type, integrator_class in zip(integrator_type_list,
integrator_class_list):
integrator = integrator_from_type(integrator_type,
DiffdriveModel(1.0, 1.0))
assert isinstance(integrator, integrator_class)
# Also making sure that the integrator is usable.
assert np.allclose(
integrator.step(State(3, 0), ControlInput(2), 0.05).data,
[0.0, 0.0, 0.0])
def test_integration(generate_integrator):
euler_integrator = generate_integrator
# We don't check the actual value, as the cpp test covers this. We only
# check if the function calls the base class implementation.
# Test with positional arguments.
updated_state = euler_integrator.integrate(
robot_state=State([1, 2, 0], []),
control_input=ControlInput([1, -1]),
time=10,
dt=0.01,
)
# Making sure that the output state dimensions and configuration is
# correct.
assert updated_state.size == 3
assert updated_state.pose.size == 3
assert updated_state.is_velocity_empty()
def compute(self):
return ControlInput(self.control_input_data)
def test_empty():
assert not ControlInput(3).is_empty()
assert ControlInput(0).is_empty()
def execute(self, playground_state, uid):
return ControlInput(self.control_input_data)
