init

tests/test_control.py

@@ -0,0 +1,105 @@
+"""Tests for modular control system."""
+
+from __future__ import annotations
+
+from pathlib import Path
+
+import numpy as np
+import pytest
+
+from uavcatch.config import load_control_config, load_environment_config, load_uav_config
+from uavcatch.control import (
+  AttitudeController,
+  ControlAllocator,
+  GeometricPositionController,
+  TrackingController,
+  build_cascaded_controller,
+  circle_trajectory,
+)
+from uavcatch.control.math3d import quaternion_attitude_error
+from uavcatch.dynamics.model import QuadDynamics
+from uavcatch.dynamics.state import QuadState, euler_to_quaternion, normalize_quaternion
+from uavcatch.sim.engine import SimEngine
+
+ROOT = Path(__file__).resolve().parents[1]
+
+
+@pytest.fixture
+def env_cfg():
+  return load_environment_config(ROOT / "environment.toml")
+
+
+@pytest.fixture
+def uav_cfg():
+  return load_uav_config(ROOT / "uav.toml")
+
+
+@pytest.fixture
+def control_cfg():
+  return load_control_config(ROOT / "control.toml")
+
+
+@pytest.fixture
+def dynamics(env_cfg, uav_cfg):
+  return QuadDynamics(env_cfg, uav_cfg)
+
+
+def test_quaternion_attitude_error_zero_when_aligned() -> None:
+  q = euler_to_quaternion(0.1, -0.05, 0.2)
+  err = quaternion_attitude_error(q, q)
+  np.testing.assert_allclose(err, 0.0, atol=1e-12)
+
+
+def test_control_allocator_hover(dynamics, uav_cfg) -> None:
+  allocator = ControlAllocator(uav_cfg)
+  thrust = uav_cfg.body.mass * dynamics.env_cfg.physics.gravity
+  omega = allocator.allocate(thrust, np.zeros(3))
+  assert np.all(omega > 0.0)
+  assert np.allclose(omega, omega[0], rtol=1e-3)
+
+
+def test_cascaded_linear_hover(env_cfg, uav_cfg, dynamics, control_cfg) -> None:
+  engine = SimEngine(env_cfg, uav_cfg)
+  engine.reset()
+  controller = build_cascaded_controller(
+    control_cfg,
+    dynamics,
+    uav_cfg,
+    target_position=np.array([0.0, 0.0, 2.0]),
+  )
+  engine.run(controller, duration=5.0)
+  err = np.linalg.norm(engine.state.position - np.array([0.0, 0.0, 2.0]))
+  assert err < 0.5
+
+
+def test_geometric_circle_tracking(env_cfg, uav_cfg, dynamics, control_cfg) -> None:
+  control_cfg.controller.mode = "geometric"
+  env_cfg.simulation.duration = 12.0
+  engine = SimEngine(env_cfg, uav_cfg)
+  engine.reset()
+
+  pos_fn, yaw_fn, accel_fn = circle_trajectory(
+    center=np.zeros(3),
+    radius=2.0,
+    height=2.0,
+    period=10.0,
+  )
+  init = pos_fn(0.0)
+  controller = build_cascaded_controller(
+    control_cfg,
+    dynamics,
+    uav_cfg,
+    target_position=init.position,
+    target_velocity=init.velocity,
+    target_yaw=float(yaw_fn(0.0)),
+  )
+  tracker = TrackingController(
+    controller=controller,
+    target_fn=pos_fn,
+    yaw_fn=yaw_fn,
+    accel_fn=accel_fn,
+  )
+  engine.run(tracker, duration=12.0)
+  target = pos_fn(12.0)
+  err = np.linalg.norm(engine.state.position - target.position)
+  assert err < 1.5

tests/test_dynamics.py

@@ -0,0 +1,183 @@
+"""Tests for quadcopter dynamics and configuration."""
+
+from __future__ import annotations
+
+from pathlib import Path
+
+import mujoco
+import numpy as np
+import pytest
+
+from uavcatch.config import EnvironmentConfig, UavConfig, load_control_config, load_environment_config, load_uav_config
+from uavcatch.dynamics.model import QuadDynamics
+from uavcatch.dynamics.state import QuadState, euler_to_quaternion, normalize_quaternion
+from uavcatch.dynamics.wind import WindModel
+from uavcatch.sim.mujoco_model import _motor_xy, build_scene_mjcf
+
+ROOT = Path(__file__).resolve().parents[1]
+
+
+@pytest.fixture
+def env_cfg() -> EnvironmentConfig:
+  return load_environment_config(ROOT / "environment.toml")
+
+
+@pytest.fixture
+def uav_cfg() -> UavConfig:
+  return load_uav_config(ROOT / "uav.toml")
+
+
+@pytest.fixture
+def dynamics(env_cfg: EnvironmentConfig, uav_cfg: UavConfig) -> QuadDynamics:
+  return QuadDynamics(env_cfg, uav_cfg)
+
+
+def test_config_load_defaults(env_cfg: EnvironmentConfig, uav_cfg: UavConfig) -> None:
+  assert env_cfg.simulation.dt > 0.0
+  assert env_cfg.simulation.integrator == "rk4"
+  assert uav_cfg.body.mass == 1.3
+  assert uav_cfg.rotor.layout == "x"
+
+
+def test_control_config_load() -> None:
+  cfg = load_control_config(ROOT / "control.toml")
+  assert cfg.controller.mode in ("linear", "geometric")
+  assert cfg.position_linear.kp == 0.8
+  assert cfg.position_geometric.kp == 2.5
+  assert cfg.eval.circle.radius == 2.5
+
+
+def test_config_missing_file() -> None:
+  with pytest.raises(FileNotFoundError):
+    load_environment_config(ROOT / "missing.toml")
+
+
+def test_quaternion_normalization_after_integration(dynamics: QuadDynamics) -> None:
+  state = QuadState.from_initial(
+    position=[0.0, 0.0, 1.0],
+    velocity=[0.0, 0.0, 0.0],
+    attitude_euler=[0.1, -0.05, 0.2],
+    angular_rate=[0.5, -0.3, 0.1],
+  )
+  omega = np.full(4, dynamics.hover_omega())
+  dt = dynamics.env_cfg.simulation.dt
+
+  for _ in range(1000):
+    state = dynamics.step(state, omega, dt)
+    q_norm = np.linalg.norm(state.quaternion)
+    assert abs(q_norm - 1.0) < 1e-6
+
+
+def test_hover_acceleration_near_zero(dynamics: QuadDynamics) -> None:
+  state = QuadState.from_initial(
+    position=[0.0, 0.0, 1.0],
+    velocity=[0.0, 0.0, 0.0],
+    attitude_euler=[0.0, 0.0, 0.0],
+    angular_rate=[0.0, 0.0, 0.0],
+  )
+  omega_hover = dynamics.hover_omega()
+  omega = np.full(4, omega_hover)
+  deriv = dynamics.derivatives(state, omega)
+  assert abs(deriv[5]) < 0.5
+
+
+def test_free_fall(dynamics: QuadDynamics) -> None:
+  state = QuadState.from_initial(
+    position=[0.0, 0.0, 5.0],
+    velocity=[0.0, 0.0, 0.0],
+    attitude_euler=[0.0, 0.0, 0.0],
+    angular_rate=[0.0, 0.0, 0.0],
+  )
+  omega = np.zeros(4)
+  dt = dynamics.env_cfg.simulation.dt
+  z_prev = state.position[2]
+
+  for _ in range(200):
+    state = dynamics.step(state, omega, dt)
+    assert state.position[2] <= z_prev + 1e-9
+    z_prev = state.position[2]
+
+  assert state.position[2] < 5.0
+
+
+def test_state_vector_roundtrip() -> None:
+  q = euler_to_quaternion(0.1, 0.2, 0.3)
+  state = QuadState(
+    position=np.array([1.0, 2.0, 3.0]),
+    velocity=np.array([0.1, 0.2, 0.3]),
+    quaternion=normalize_quaternion(q),
+    angular_rate=np.array([0.01, 0.02, 0.03]),
+  )
+  restored = QuadState.from_vector(state.as_vector())
+  np.testing.assert_allclose(restored.position, state.position)
+  np.testing.assert_allclose(restored.velocity, state.velocity)
+  np.testing.assert_allclose(restored.angular_rate, state.angular_rate)
+
+
+def test_drag_opposes_motion(dynamics: QuadDynamics) -> None:
+  forward = np.array([5.0, 0.0, 0.0], dtype=np.float64)
+  force = dynamics.aerodynamic_force(forward, np.zeros(3))
+  # Drag must oppose velocity and be purely along -X here.
+  assert force[0] < 0.0
+  np.testing.assert_allclose(force[1:], 0.0, atol=1e-12)
+
+
+def test_quadratic_drag_scaling(dynamics: QuadDynamics) -> None:
+  f1 = dynamics.aerodynamic_force(np.array([1.0, 0.0, 0.0]), np.zeros(3))
+  f2 = dynamics.aerodynamic_force(np.array([2.0, 0.0, 0.0]), np.zeros(3))
+  # Quadratic drag: doubling speed quadruples the force (when linear term is 0).
+  np.testing.assert_allclose(abs(f2[0]) / abs(f1[0]), 4.0, rtol=1e-6)
+
+
+def test_drag_uses_relative_wind(dynamics: QuadDynamics) -> None:
+  wind = np.array([3.0, 0.0, 0.0], dtype=np.float64)
+  # Moving with the wind -> zero relative airspeed -> zero drag.
+  force = dynamics.aerodynamic_force(wind.copy(), wind)
+  np.testing.assert_allclose(force, 0.0, atol=1e-12)
+
+
+def test_terminal_velocity_with_drag(dynamics: QuadDynamics) -> None:
+  state = QuadState.from_initial(
+    position=[0.0, 0.0, 100.0],
+    velocity=[0.0, 0.0, 0.0],
+    attitude_euler=[0.0, 0.0, 0.0],
+    angular_rate=[0.0, 0.0, 0.0],
+  )
+  omega = np.zeros(4)
+  dt = dynamics.env_cfg.simulation.dt
+  for _ in range(5000):
+    state = dynamics.step(state, omega, dt)
+  # With quadratic drag, fall speed must saturate well below the drag-free
+  # value (g * t = 9.81 * 10 ~ 98 m/s).
+  assert abs(state.velocity[2]) < 60.0
+
+
+def test_wind_model_gust_oscillates() -> None:
+  wind = WindModel(
+    base_velocity=np.array([2.0, 0.0, 0.0]),
+    gust_amplitude=1.0,
+    gust_frequency=1.0,
+  )
+  assert wind.is_active
+  # Quarter period -> peak gust along +X direction.
+  np.testing.assert_allclose(wind.sample(0.25)[0], 3.0, atol=1e-6)
+  np.testing.assert_allclose(wind.sample(0.0)[0], 2.0, atol=1e-6)
+
+
+def test_wind_config_parsed(env_cfg: EnvironmentConfig) -> None:
+  assert env_cfg.wind.velocity == [0.0, 0.0, 0.0]
+  assert hasattr(env_cfg.wind, "gust_amplitude")
+
+
+def test_mujoco_model_motor_alignment(env_cfg: EnvironmentConfig, uav_cfg: UavConfig) -> None:
+  mjcf = build_scene_mjcf(env_cfg, uav_cfg)
+  model = mujoco.MjModel.from_xml_string(mjcf)
+  arm = uav_cfg.body.arm_length
+  expected = _motor_xy(uav_cfg.rotor.layout, arm)
+
+  for idx, (mx, my) in enumerate(expected, start=1):
+    body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, f"rotor{idx}")
+    assert body_id >= 0
+    pos = model.body_pos[body_id]
+    np.testing.assert_allclose(pos[0], mx, rtol=0, atol=1e-6)
+    np.testing.assert_allclose(pos[1], my, rtol=0, atol=1e-6)

tests/test_eval.py

@@ -0,0 +1,72 @@
+"""Tests for control evaluation metrics."""
+
+from __future__ import annotations
+
+import numpy as np
+
+from uavcatch.eval.metrics import compute_tracking_metrics, evaluate_maneuver, _step_response_metrics
+from uavcatch.control.pid import circle_trajectory_with_takeoff
+from uavcatch.eval.maneuvers import circle_track, step_position
+
+
+def test_tracking_rmse() -> None:
+  err = np.array([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 0.0]])
+  m = compute_tracking_metrics(err)
+  assert abs(m.rmse[0] - np.sqrt(1 / 3)) < 1e-9
+  assert m.rmse_3d > 0.0
+
+
+def test_step_rise_and_settle() -> None:
+  t = np.linspace(0, 10, 1000)
+  y = np.where(t < 1.0, 0.0, 1.0 - np.exp(-(t - 1.0) / 0.5))
+  m = _step_response_metrics(t, y, step_time=1.0, y0=0.0, yf=1.0)
+  assert m.rise_time is not None
+  assert m.settling_time is not None
+  assert m.overshoot_pct < 5.0
+
+
+def test_evaluate_maneuver_step_spec() -> None:
+  spec = step_position(
+    np.array([0.0, 0.0, 2.0]),
+    np.array([0.0, 0.0, 3.0]),
+    step_time=2.0,
+    duration=10.0,
+    axis=2,
+  )
+  n = 200
+  time = np.linspace(0, 10, n)
+  z = np.where(time < 2.0, 2.0, 2.0 + 0.9 * (1 - np.exp(-(time - 2.0) / 1.0)))
+  data = {
+    "time": time,
+    "position": np.column_stack([np.zeros(n), np.zeros(n), z]),
+    "tracking_error": np.column_stack([np.zeros(n), np.zeros(n), 3.0 - z]),
+    "euler": np.zeros((n, 3)),
+    "omega": np.full((n, 4), 550.0),
+  }
+  metrics = evaluate_maneuver(spec, data, omega_max=838.0, mode="linear")
+  assert metrics.step is not None
+  assert metrics.tracking.rmse_3d < 0.6
+
+
+def test_circle_takeoff_phase() -> None:
+  spec = circle_track(takeoff_time=2.0, ground_z=0.0, height=2.0, radius=2.5)
+  assert spec.initial_position == (0.0, 0.0, 0.05)
+
+  t0 = spec.target_fn(0.0)
+  assert t0.position[2] == 0.05
+
+  t1 = spec.target_fn(1.0)
+  assert 0.05 < t1.position[2] < 2.0
+
+  t2 = spec.target_fn(2.0)
+  np.testing.assert_allclose(t2.position, [2.5, 0.0, 2.0], atol=1e-9)
+
+  pos_fn, _, _ = circle_trajectory_with_takeoff(
+    center=np.zeros(3),
+    radius=2.5,
+    height=2.0,
+    period=10.0,
+    takeoff_time=2.0,
+  )
+  t3 = pos_fn(3.0)
+  assert t3.position[0] > 2.0