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 keyboard(self, current_state: DroneState, user_input: dict): """ Maps keyboard input to a drone state. The mapper uses fictional units and the time until next expected state is one time unit. So the steps below are basically integrations. :param current_state: the current state of the drone :param user_input: keyboard input from user :return: the expected state of the drone """ state: dict = current_state.state_dict state['Rotation'] = self.add_rotation(state['Rotation'], user_input) rotation_normalized = self.normalize_rotation(state['Rotation']) state['Angular Acceleration'] = self.angular_acceleration( state, user_input) state['Acceleration'] = self.acceleration(state['Acceleration'], user_input['Acceleration'], rotation_normalized) state['Velocity'] = add_lists(state['Velocity'], state['Acceleration']) state['Angular Velocity'] = add_lists(state['Angular Velocity'], state['Angular Acceleration']) state['Position'] = add_lists(state['Position'], state['Velocity']) return DroneState.from_dict(state)
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)