106 lines
2.7 KiB
Python
106 lines
2.7 KiB
Python
"""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
|