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 test_set_robot_state(generate_playground_state_list, generate_robot_list):
ps1, _ = generate_playground_state_list
r1, r2 = generate_robot_list
# Adding both robots.
ps1.add_robot(r1, 1)
ps1.add_robot(r2, 2)
# Setting different states.
ps1.set_robot_state(State([-1, -2, -3], []), 1)
ps1.set_robot_state(State([1, 1, 2], []), 2)
# Make sure that the states have updated, through the oracle, robot and
# robot states.
robot_oracle = ps1.robot_oracle
assert np.allclose(robot_oracle[1].state.data, [-1, -2, -3])
assert np.allclose(robot_oracle[2].state.data, [1, 1, 2])
assert np.allclose(ps1.get_robot(1).state.data, [-1, -2, -3])
assert np.allclose(ps1.get_robot(2).state.data, [1, 1, 2])
assert np.allclose(ps1.get_robot_state(1).data, [-1, -2, -3])
assert np.allclose(ps1.get_robot_state(2).data, [1, 1, 2])
# Trying to set a robot state against an invalid uid must throw an
# exception.
with pytest.raises(ValueError):
ps1.set_robot_state(State(3, 0), -1)
with pytest.raises(ValueError):
ps1.set_robot_state(State(3, 0), 3)
def test_normalize_state():
dubin_model = DubinModel(1)
# As the cpp side tests the actual computation, we just check that the
# normalize_state interface works.
assert dubin_model.normalize_state(robot_state=State(3, 0)) == State(3, 0)
def test_normalize_state():
tricycle_model = TricycleModel(1, 1)
# As the cpp side tests the actual computation, we just check that the
# normalize_state interface works.
assert tricycle_model.normalize_state(robot_state=State(4, 0)) == State(
4, 0)
def test_normalize_state():
ackermann_model = AckermannModel(1, 1)
# As the cpp side tests the actual computation, we just check that the
# normalize_state interface works.
assert ackermann_model.normalize_state(robot_state=State(4, 0)) == State(
4, 0)
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_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_create_like(generate_state_list):
for s in generate_state_list:
zero_state = State.create_like(state=s)
assert zero_state.size == s.size
# Make sure that if partial, they are of the same configuration.
assert zero_state.is_empty() == s.is_empty()
assert zero_state.is_pose_empty() == s.is_pose_empty()
assert zero_state.is_velocity_empty() == s.is_velocity_empty()
assert np.allclose(State.create_like(s).data, np.zeros(s.size))
def test_equality(generate_state_list):
sf1, sf2, _, _, _, sp1, sp2 = generate_state_list
assert sf1 == State([0, 0], [0, 0])
assert sf2 == State([1, 2], [3, 4])
assert sf1 != sf2
# Partial state equality.
assert sp1 == State([], [0, 0])
assert sp2 == State([4, 6, 5.9], [])
assert sp1 != sp2
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_invalid_construction():
with pytest.raises(ValueError):
_ = Trajectory(State(0, 0))
with pytest.raises(ValueError):
_ = Trajectory([])
with pytest.raises(ValueError):
_ = Trajectory([State(3, 2), State(2, 2)])
with pytest.raises(ValueError):
_ = Trajectory(np.empty([0, 0]))
with pytest.raises(ValueError):
_ = Trajectory(np.zeros([100, 2]), 1, 0)
def test_normalize_state(generate_custom_model_list):
c1, c2, c3 = generate_custom_model_list
# Making sure that the default implementation of normalize_state returns
# the given state without any changes.
state1 = State([1.0, -2.0, 0.0], [1.0, 1.0])
state2 = State(1, 1)
state3 = State([1, 2], [3, 4, 5, 6])
assert c1.normalize_state(state1) == state1
assert c2.normalize_state(state2) == state2
assert c3.normalize_state(robot_state=state3) == state3
def generate_robot_list():
r1 = Robot(DiffdriveModel(1.0, 1.0), Footprint([[0, 0]]))
r2 = Robot(
DiffdriveModel(2.0, 3.0), Footprint([[0, 0]]), State([1.0, 2.0, 3.0], [])
)
return r1, r2
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 compute_state_derivative(self, robot_state, control_input):
tmp_der = np.sum(np.multiply(robot_state.data, control_input.data))
tmp_der = tmp_der + self.a * self.b
# tmp_der is a scalar, but the derivative must be a State object.
# Copying tmp_der to each State element.
der = State([tmp_der] * self.pose_size, [tmp_der] * self.velocity_size)
return der
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_equality(generate_trajectory_list):
t1, t2, t3 = generate_trajectory_list
assert t1 == t1
assert t2 == t2
assert t3 == t3
assert t1 == Trajectory(State(2, 2))
assert t2 == Trajectory([[1, 1, 1], [2, 2, 2], [3, 3, 3]], 2, 1)
assert t1 != t2
assert t2 != t3
assert t3 != t1
assert t1 != Trajectory(State(2, 1))
assert t1 != Trajectory(State(1, 2))
assert t2 != Trajectory([[1, 1, 1], [2, 2, 2], [3, 3, 3]], 1, 2)
def test_add_knot_points(generate_trajectory_list):
t1, t2, t3 = generate_trajectory_list
t1.add_knot_points([State(2, 2), State(2, 2)], [1, 2])
t2.add_knot_points([State(2, 1), State(2, 1), State(2, 1)], [0, 2, 5])
t3.add_knot_points(knot_points=[State(3, 2), State(3, 2)],
indices=[0, 101])
assert np.allclose(t1.data, [[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0]])
assert np.allclose(
t2.data,
[[0, 0, 0], [1, 1, 1], [0, 0, 0], [2, 2, 2], [3, 3, 3], [0, 0, 0]])
assert t3[0] == State(3, 2)
assert t3[-1] == State(3, 2)
def test_getitem(generate_trajectory_list):
t1, t2, t3 = generate_trajectory_list
for t in [t1, t2, t3]:
for i in range(t.size):
assert t[i] == State(t.data[i, :t.pose_size], t.data[i,
t.pose_size:])
# Test negative indexing.
assert t[-(t.size - i)] == State(t.data[i, :t.pose_size],
t.data[i, t.pose_size:])
# Test invalid getitem
with pytest.raises(IndexError):
_ = t1[-2]
with pytest.raises(IndexError):
_ = t2[3]
with pytest.raises(IndexError):
_ = t3[200]
def test_invalid_add_knot_points_due_to_indices(generate_trajectory_list):
t1, t2, t3 = generate_trajectory_list
# Invalid indices.
# We do not allow for negative indexing here.
# Also the sizes have changed from the previous add_knot_points calls.
with pytest.raises(IndexError):
t1.add_knot_points([State(2, 2), State(2, 2)], [-1, 2])
with pytest.raises(IndexError):
t2.add_knot_points([State(2, 1), State(2, 1), State(2, 1)], [0, 2, 6])
with pytest.raises(IndexError):
t3.add_knot_points(
[State(3, 2), State(3, 2), State(3, 2)], [0, 50, 103])
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 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):
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_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_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_execute_with_temporary(generate_playground):
# Test the function after adding robots using temporarily created Robot and Pilot objects.
# This is to test for any cpp lifetime management weirdness.
playground = generate_playground
playground.add_robot(
Robot(DiffdriveModel(1.0, 1.0)),
CustomPilot([0, 0]),
IntegratorType.EULER_INTEGRATOR,
1,
)
playground.add_robot(
Robot(DiffdriveModel(1.0, 1.0),
initial_state=State([1.0, 2.0, 0.0], [])),
CustomPilot([1, 1]),
IntegratorType.MID_POINT_INTEGRATOR,
2,
)
# Make sure that the playground time is 0.
assert playground.state.time == 0.0
# Executing a playground cycle.
playground.execute()
# Test the states of the robots.
# The first pilot gives zero control inputs, hence the state must remain
# unchanged.
assert np.allclose(
playground.state.get_robot_state(1).data, [0.0, 0.0, 0.0])
# The second robot receives a constant input of [1., 1.]. As the wheel
# radius = 1 m, the robot moves forward with a velocity of 1 m/s. Hence
# it moves a total distance of dt m in the x direction (The initial angle
# is zero).
assert np.allclose(
playground.state.get_robot_state(2).data,
[1.0 + playground.spec.dt, 2.0, 0.0])
# Make sure that the playground time has updated.
assert playground.state.time == playground.spec.dt
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 generate_state_list():
# List of full states constructed in all the different ways.
# Also tests initialization.
# Full states.
sf1 = State(pose_size=2, velocity_size=2)
sf2 = State([1, 2], [3, 4])
sf3 = State((5, 6), (7, 8, 9))
sf4 = State(
data_pose=np.array([1, 1, 2]),
data_velocity=np.array([3, 5, 8, 13], dtype=np.float),
)
sf5 = State(pose=Pose([0, -1]), velocity=Velocity([-3, -7, 9]))
# Partial states.
sp1 = State(0, 2)
sp2 = State([4, 6, 5.9], [])
return sf1, sf2, sf3, sf4, sf5, sp1, sp2
HELP_PROMPT = "Type of robot to use. The available options are: \n1. ackermann\n2. diffdrive\n3. dubin\n4. tricycle"
class RobotType(Enum):
ACKERMANN = "ackermann"
DIFFDRIVE = "diffdrive"
DUBIN = "dubin"
TRICYCLE = "tricycle"
robot_bank = {
RobotType.ACKERMANN:
Robot(
kinematic_model=AckermannModel(width=1.0, length=3.0),
initial_state=State([10.0, 5.0, 0.0, np.pi / 6], []),
),
RobotType.DIFFDRIVE:
Robot(
kinematic_model=DiffdriveModel(radius=0.3, width=1.0),
initial_state=State([10.0, 5.0, 0.0], []),
),
RobotType.DUBIN:
Robot(kinematic_model=DubinModel(speed=1.0),
initial_state=State([10.0, 5.0, 0.0], [])),
RobotType.TRICYCLE:
Robot(
kinematic_model=TricycleModel(width=1.0, length=2.0),
initial_state=State([10.0, 5.0, 0.0, np.pi / 8], []),
),
}