45 lines
1.8 KiB
Python
45 lines
1.8 KiB
Python
import numpy as np
|
|
import pytest
|
|
from scipy.spatial.transform import Rotation
|
|
|
|
from defense_sim.entities.drone import Drone
|
|
from defense_sim.entities.projectile import Projectile
|
|
from defense_sim.physics.flight import NED_TO_ENU, FRD_TO_FLU, RotorPyDynamics, JSBSimDynamics
|
|
|
|
|
|
def test_attitude_frame_roundtrip():
|
|
rotation = Rotation.from_euler('xyz', [.2, -.1, .7]).as_matrix()
|
|
transformed = NED_TO_ENU @ rotation @ FRD_TO_FLU
|
|
assert np.linalg.det(transformed) == pytest.approx(1.)
|
|
assert np.allclose(NED_TO_ENU.T @ transformed @ FRD_TO_FLU, rotation)
|
|
|
|
|
|
def test_rotorpy_hover_and_thrust_affect_motion():
|
|
pytest.importorskip('rotorpy')
|
|
d = Drone('d', 'defense_drone', np.array([0., 0., -1.]),
|
|
target_position_ned_m=np.array([0., 0., -1.]))
|
|
dynamics = RotorPyDynamics(d)
|
|
params = dynamics.params
|
|
dynamics.state['rotor_speeds'][:] = np.sqrt(params['mass']*dynamics.vehicle.g/(4*params['k_eta']))
|
|
for _ in range(50):
|
|
dynamics.step(d, .01, np.zeros(3), True)
|
|
assert np.allclose(d.position_ned_m, [0, 0, -1], atol=.001)
|
|
assert np.linalg.norm(d.orientation_quaternion_wxyz) == pytest.approx(1.)
|
|
d.target_position_ned_m = np.array([0., 0., -2.])
|
|
for _ in range(50):
|
|
dynamics.step(d, .01, np.zeros(3), True)
|
|
assert d.position_ned_m[2] < -1.1
|
|
assert np.all(dynamics.state['rotor_speeds'] <= params['rotor_speed_max'])
|
|
|
|
|
|
def test_jsbsim_headless_si_motion_and_model_validation():
|
|
pytest.importorskip('jsbsim')
|
|
p = Projectile('aircraft', np.array([0., 0., -300.]), np.array([50., 0., 0.]))
|
|
dynamics = JSBSimDynamics(p, 'c172p')
|
|
for _ in range(10):
|
|
dynamics.step(p, .01, np.zeros(3))
|
|
assert 4 < p.position_ned_m[0] < 6
|
|
assert np.isfinite(p.velocity_ned_m_s).all()
|
|
with pytest.raises(ValueError, match='controls'):
|
|
JSBSimDynamics(p, 'c172p', {'throttle': 2.})
|