test: Correct input to rotation

tests/envs/base/conftest.py

@@ -21,7 +21,14 @@
 POSITION_FROM_VELOCITY_1 = pytest.mark.parametrize(
     "pos",
     [
-        (0, 0, 0),
+        np.array((0, 0, 0)),
+    ],
+    indirect=True,
+)
+POSITION_FROM_VELOCITY_1_PB = pytest.mark.parametrize(
+    "pos",
+    [
+        np.array((0, 0, 1)),
     ],
     indirect=True,
 )
@@ -82,3 +89,25 @@
     ],
     indirect=["vec_omega"],
 )
+
+INPUT_TO_ROT = pytest.mark.parametrize(
+    "vec_omega,k_Q,ang_vel_sign",
+    [
+        [
+            (1, 0, 0, 0),
+            0,
+            (0, 1, 0),
+        ],
+        [(0, 0, 1, 0), 0, (0, -1, 0)],
+        [(0, 1, 0, 0), 0, (-1, 0, 0)],
+        [(0, 0, 0, 1), 0, (1, 0, 0)],
+        [(1, 0.9, 1, 0.9), 0.1, (0, 0, -1)],
+        [(0.9, 1, 0.9, 1), 0.1, (0, 0, 1)],
+    ],
+    indirect=["vec_omega", "k_Q"],
+)
+
+
+@pytest.fixture
+def rhr_to_lhr():
+    return np.array((-1, -1, 1))

tests/envs/base/test_linear_drone.py

@@ -2,6 +2,7 @@
 #  SPDX-License-Identifier: GPL-3.0-only
 import numpy as np
 import pytest
+from envs.base.conftest import INPUT_TO_ROT
 from envs.base.conftest import LARGE_INPUT
 from envs.base.conftest import LOW_INPUT
 from envs.base.conftest import PITCH_INPUT
@@ -79,3 +80,16 @@ def test_rpy_from_ang_vel(vec_omega, lineardroneenv, angular_velocity):
     lineardroneenv.reset()
     obs, *_ = lineardroneenv.step(vec_omega)
     assert np.allclose(np.sign(obs.rpy), np.sign(angular_velocity))
+
+
+@INPUT_TO_ROT
+def test_input_to_rot(seed, lineardroneenv, action, k_Q, ang_vel_sign):
+    """
+    Step input over a short time will give a good insight if the drone is behaving
+    as expected
+    """
+    lineardroneenv.reset(seed=seed)
+    for _ in range(5):
+        obs, *_ = lineardroneenv.step(action * 100)
+    # Drone landed within 10cm of where we expected it
+    assert np.allclose(np.sign(obs.ang_vel), ang_vel_sign)

tests/envs/base/test_nonlinear_drone.py

@@ -2,6 +2,7 @@
 #  SPDX-License-Identifier: GPL-3.0-only
 import numpy as np
 import pytest
+from envs.base.conftest import INPUT_TO_ROT
 from envs.base.conftest import LARGE_INPUT
 from envs.base.conftest import LOW_INPUT
 from envs.base.conftest import PITCH_INPUT
@@ -91,3 +92,16 @@ def test_rpy_from_ang_vel(vec_omega, nonlineardroneenv, angular_velocity):
     nonlineardroneenv.reset()
     obs, *_ = nonlineardroneenv.step(vec_omega)
     assert np.allclose(np.sign(obs.rpy), angular_velocity)
+
+
+@INPUT_TO_ROT
+def test_input_to_rot(seed, nonlineardroneenv, action, k_Q, ang_vel_sign):
+    """
+    Step input over a short time will give a good insight if the drone is behaving
+    as expected
+    """
+    nonlineardroneenv.reset(seed=seed)
+    for _ in range(5):
+        obs, *_ = nonlineardroneenv.step(action * 100)
+    # Drone landed within 10cm of where we expected it
+    assert np.allclose(np.sign(obs.ang_vel), ang_vel_sign)