def test_up_with_rotation(self):
"""
Tests if the correct rotors uses more power than the opposite site.
"""
position = ZERO_VECTOR
rotation = ZERO_VECTOR
velocity = ZERO_VECTOR
velocity_ang = ZERO_VECTOR
acceleration = ZERO_VECTOR
acceleration_ang = ZERO_VECTOR
current_state = DroneState(rotation, velocity, acceleration,
acceleration_ang, position, velocity_ang)
position = [0, 0.174, 0.985]
rotation = [10, 0, 0]
velocity = [0, 0.174, 0.985]
velocity_ang = [0.174, 0, 0]
acceleration = [0, 0.174, 0.985]
acceleration_ang = [0.174, 0, 0]
future_state = DroneState(rotation, velocity, acceleration,
acceleration_ang, position, velocity_ang)
thrusts = self.calculator.calc(current_state, future_state)
print(thrusts)
self.assertGreater(thrusts[1], thrusts[3])
self.assertAlmostEqual(thrusts[0], thrusts[2])
def test_calc_simple_up(self):
"""
Tests the thrust calculation for drone from ground one meter up.
"""
position = ZERO_VECTOR
rotation = ZERO_VECTOR
velocity = ZERO_VECTOR
velocity_ang = ZERO_VECTOR
acceleration = ZERO_VECTOR
acceleration_ang = ZERO_VECTOR
current_state = DroneState(rotation, velocity, acceleration,
acceleration_ang, position, velocity_ang)
position = UP_VECTOR
velocity = UP_VECTOR
acceleration = UP_VECTOR
future_state = DroneState(rotation, velocity, acceleration,
acceleration_ang, position, velocity_ang)
needed_thrust = 21.61 # Newton
relative_thrust_per_rotor = (needed_thrust /
4) / self.calculator.max_rotor_thrust
expected_thrusts = [relative_thrust_per_rotor] * 4
thrusts = self.calculator.calc(current_state, future_state)
for expected_thrust, thrust in zip(expected_thrusts, thrusts):
self.assertAlmostEqual(expected_thrust, thrust, delta=0.001)
def test_keyboard_up_from_position(self):
"""
Tests the drone in the air but everything else zero.
"""
position = [2, 3, 4]
rotation = ZERO_VECTOR
velocity = ZERO_VECTOR
velocity_ang = ZERO_VECTOR
acceleration = ZERO_VECTOR
acceleration_ang = ZERO_VECTOR
drone_state = DroneState(rotation, velocity, acceleration,
acceleration_ang, position, velocity_ang)
user_input = ONE_UP
position = [2, 3, 5]
velocity = UP_VECTOR
acceleration = UP_VECTOR
expected_state = DroneState(rotation, velocity, acceleration,
acceleration_ang, position, velocity_ang)
future_state: DroneState = self.mapper.keyboard(
drone_state, user_input)
self.assertEqual(future_state,
expected_state,
msg=f'Actual:\n'
f'{future_state.__str__()}\n\n'
f'Expected:\n'
f'{expected_state.__str__()}')
def test_keyboard_simple_up(self):
"""
Tests one time step from drone on ground with everything zero.
"""
position = UP_VECTOR
rotation = ZERO_VECTOR
velocity = UP_VECTOR
velocity_ang = ZERO_VECTOR
acceleration = UP_VECTOR
acceleration_ang = ZERO_VECTOR
expected_state = DroneState(rotation, velocity, acceleration,
acceleration_ang, position, velocity_ang)
position = ZERO_VECTOR
velocity = ZERO_VECTOR
acceleration = ZERO_VECTOR
drone_state = DroneState(rotation, velocity, acceleration,
acceleration_ang, position, velocity_ang)
user_input = ONE_UP
future_state: DroneState = self.mapper.keyboard(
drone_state, user_input)
self.assertEqual(future_state,
expected_state,
msg=f'Actual:\n'
f'{future_state.__str__()}\n\n'
f'Expected:\n'
f'{expected_state.__str__()}')
def test_create_three_dimensional_position():
"""
Test dict generation for a three dimensional position.
"""
position = [1, 1, 1]
rotation = [1, 1, 1]
velocity = [1, 1, 1]
velocity_ang = [1, 1, 1]
acceleration = [1, 1, 1]
acceleration_ang = [1, 1, 1]
drone_state = DroneState(rotation, velocity, acceleration,
acceleration_ang, position, velocity_ang)
expected_dict = {
"Position": position,
"Rotation": rotation,
"Velocity": velocity,
"Angular Velocity": velocity_ang,
"Acceleration": acceleration,
"Angular Acceleration": acceleration_ang
}
state_dict = drone_state.state_dict
assert state_dict == expected_dict
def test_keyboard_input_exception(self):
"""
Test if exception is thrown upon wrong user input.
"""
with raises(UserInputError):
drone_state = DroneState([0, 0, 0], [0, 0, 0], [0, 0, 0],
[0, 0, 0], [0, 0, 0], [0, 0, 0])
user_input = {'velocity': [1, 1, 1]}
self.handler.keyboard_input(drone_state, user_input)
def test_rotation_from_ground(self):
"""
Tests from the standing drone to drone in air with rotation.
"""
position = ZERO_VECTOR
rotation = ZERO_VECTOR
velocity = ZERO_VECTOR
velocity_ang = ZERO_VECTOR
acceleration = ZERO_VECTOR
acceleration_ang = ZERO_VECTOR
drone_state = DroneState(rotation, velocity, acceleration,
acceleration_ang, position, velocity_ang)
position = [0, 0.174, 0.985]
rotation = [10.0, 0.0, 0.0]
velocity = [0, 0.174, 0.985]
velocity_ang = [0.174, 0.0, 0.0] # rad/s
acceleration = [0, 0.174, 0.985]
acceleration_ang = [0.174, 0.0, 0.0] # rad/s^2
expected_state = DroneState(rotation, velocity, acceleration,
acceleration_ang, position, velocity_ang)
user_input = {
"Rotation Forward": 0,
'Rotation Right': 1,
"Rotation Backward": 0,
"Rotation Left": 0,
"Acceleration": 1
}
future_state: DroneState = self.mapper.keyboard(
drone_state, user_input)
self.assertEqual(future_state,
expected_state,
msg=f'Actual:\n'
f'{future_state.__str__()}\n\n'
f'Expected:\n'
f'{expected_state.__str__()}')
def test_three_dim_pos_validation():
"""
test if invalid position causes exception
"""
with raises(DroneStateError):
position = [1, 1]
rotation = [1, 1, 1]
velocity = [1, 1, 1]
velocity_ang = [1, 1, 1]
acceleration = [1, 1, 1]
acceleration_ang = [1, 1, 1]
drone_state = DroneState(rotation, velocity, acceleration,
acceleration_ang, position, velocity_ang)
# pylint: disable=unused-variable
state = drone_state.state_dict
def setUp(self) -> None:
mass = 2
max_rotor_thrust = 20
radius = 1
self.handler = RequestHandler("Quadrocopter", mass, max_rotor_thrust,
radius)
drone_state = DroneState([0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0],
[0, 0, 0], [0, 0, 0])
user_input = {
'Rotation Forward': 0,
'Rotation Right': 0,
'Rotation Backward': 0,
'Rotation Left': 0,
'Acceleration': 1
}
expected_thrusts = [10.81 * mass / 4 / max_rotor_thrust] * 4
thrusts: list = self.handler.keyboard_input(drone_state, user_input)
self.assertEqual(expected_thrusts, thrusts)