This commit is contained in:
robinson committed 2026-07-03 04:19:44 +08:00
commit e901fa9579
72 files changed
+3715

No files matched your search

+2
View File
@@ -0,0 +1,2 @@
# Static snapshot for reference only.
# Runtime visualization uses procedural MJCF from uavcatch.sim.mujoco_model.
+2
View File
@@ -0,0 +1,2 @@
# Static snapshot for reference only.
# Runtime visualization uses procedural MJCF from uavcatch.sim.mujoco_model.
+54
View File
@@ -0,0 +1,54 @@
[controller]
# linear: 世界系位置 PID + 姿态内环
# geometric: SE(3) 几何位置控制 + 四元数姿态内环
mode = "geometric"
max_tilt = 0.35 # rad,仅 linear 外环限幅
[position.linear]
kp = 0.8
ki = 0.2
kd = 0.9
[position.geometric]
kp = 2.5
ki = 0.4
kd = 2.0
[attitude.linear]
kp = 8.0
ki = 0.5
kd = 0.6
[attitude.geometric]
kp = 10.0
ki = 0.8
kd = 0.8
[eval]
initial_height = 2.0
[eval.hover]
duration = 8.0
[eval.step]
step_time = 2.0
duration = 12.0
step_z = 3.0
step_x = 3.0
step_y = 2.0
[eval.takeoff_forward]
ground_z = 0.0
hover_z = 2.0
forward_x = 4.0
takeoff_time = 3.0
hover_time = 2.0
forward_time = 6.0
[eval.circle]
ground_z = 0.0
takeoff_time = 2.0
radius = 2.5
height = 2.0
period = 10.0
duration = 20.0
+26
View File
@@ -0,0 +1,26 @@
[scene]
# 场景边界(米),以原点为中心
size_x = 20.0
size_y = 20.0
size_z = 10.0
ground_z = 0.0
[physics]
gravity = 9.81
air_density = 1.225
[simulation]
dt = 0.003
duration = 10.0
integrator = "rk4"
[viewer]
enable = true
fps = 60
camera_distance = 8.0
[wind]
# 环境风(世界系, m/s)。drag 基于相对空速 v_rel = 机体速度 - 风速 计算
velocity = [0.0, 0.0, 0.0] # 恒定风速 [vx, vy, vz]
gust_amplitude = 0.0 # 阵风幅值(m/s),沿风向叠加正弦扰动
gust_frequency = 0.0 # 阵风频率(Hz)
+320
View File
@@ -0,0 +1,320 @@
#!/usr/bin/env python3
"""Control system performance evaluation: maneuvers + time-domain metrics."""
from __future__ import annotations
import argparse
import sys
from pathlib import Path
import numpy as np
ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path:
sys.path.insert(0, str(ROOT / "src"))
from uavcatch.config import load_control_config, load_environment_config, load_uav_config
from uavcatch.control import TrackingController
from uavcatch.control.factory import build_cascaded_controller
from uavcatch.eval import (
SimRecorder,
default_maneuver_suite,
evaluate_maneuver,
plot_maneuver_report,
plot_summary_comparison,
)
from uavcatch.eval.maneuvers import ManeuverSpec
from uavcatch.sim.engine import SimEngine
def parse_args() -> argparse.Namespace:
parser = argparse.ArgumentParser(description="Evaluate cascaded flight controller performance")
parser.add_argument("--environment", type=Path, default=ROOT / "environment.toml")
parser.add_argument("--uav", type=Path, default=ROOT / "uav.toml")
parser.add_argument("--control", type=Path, default=ROOT / "control.toml")
parser.add_argument(
"--output",
type=Path,
default=ROOT / "output" / "eval_control",
help="Directory for plots",
)
parser.add_argument("--show", action="store_true", help="Show matplotlib windows")
parser.add_argument(
"--maneuver",
type=str,
default="all",
help="Run one maneuver by name, or 'all'",
)
return parser.parse_args()
def run_maneuver(
spec: ManeuverSpec,
env_cfg,
uav_cfg,
control_cfg,
) -> tuple[dict[str, np.ndarray], object]:
env_cfg.simulation.duration = spec.duration
if spec.initial_position is not None:
uav_cfg.initial.position = list(spec.initial_position)
engine = SimEngine(env_cfg=env_cfg, uav_cfg=uav_cfg)
engine.reset()
init = spec.target_fn(0.0)
if hasattr(init, "position"):
init_pos, init_vel = init.position, init.velocity
else:
init_pos, init_vel = np.asarray(init), np.zeros(3)
controller = build_cascaded_controller(
control_cfg,
engine.dynamics,
uav_cfg,
target_position=init_pos,
target_velocity=init_vel,
)
controller.reset()
tracker = TrackingController(
controller=controller,
target_fn=spec.target_fn,
yaw_fn=spec.yaw_fn,
accel_fn=spec.accel_fn,
)
recorder = SimRecorder()
def on_step(state, t, omega):
tgt_pos, tgt_vel = recorder.resolve_target(spec.target_fn, t)
recorder.record(t, state, tgt_pos, tgt_vel, omega)
engine.run(tracker, duration=spec.duration, on_step=on_step)
return recorder.as_arrays(), engine
def print_metrics_table(metrics_list: list) -> None:
print("\n" + "=" * 72)
print(f"{'Maneuver':<22} {'Mode':<10} {'RMSE3D':>8} {'MaxErr':>8} {'Sat%':>6}")
print("-" * 72)
for m in metrics_list:
print(
f"{m.name:<22} {m.mode:<10} "
f"{m.tracking.rmse_3d:8.4f} {m.tracking.max_error_3d:8.4f} "
f"{m.control.saturation_ratio * 100:6.1f}"
)
print("=" * 72)
for m in metrics_list:
if m.step is not None:
rise = f"{m.step.rise_time:.3f}s" if m.step.rise_time is not None else "N/A"
settle = f"{m.step.settling_time:.3f}s" if m.step.settling_time is not None else "N/A"
print(
f" [{m.name}] step {m.step.axis}: rise={rise}, settle={settle}, "
f"overshoot={m.step.overshoot_pct:.1f}%, ss_err={m.step.steady_state_error:.4f} m"
)
def main() -> int:
args = parse_args()
env_cfg = load_environment_config(args.environment)
uav_cfg = load_uav_config(args.uav)
control_cfg = load_control_config(args.control)
uav_cfg.initial.position = [0.0, 0.0, control_cfg.eval.initial_height]
uav_cfg.initial.velocity = [0.0, 0.0, 0.0]
suite = default_maneuver_suite(control_cfg.eval)
if args.maneuver != "all":
suite = [s for s in suite if s.name == args.maneuver]
if not suite:
names = ", ".join(s.name for s in default_maneuver_suite(control_cfg.eval))
print(f"Unknown maneuver '{args.maneuver}'. Available: {names}")
return 1
mode = control_cfg.controller.mode
print(f"Controller mode: {mode} (from {args.control})")
all_metrics = []
for spec in suite:
print(f"Running: {spec.name} - {spec.description}")
data, engine = run_maneuver(spec, env_cfg, uav_cfg, control_cfg)
metrics = evaluate_maneuver(
spec,
data,
omega_max=uav_cfg.rotor.omega_max,
mode=mode,
)
all_metrics.append(metrics)
out_file = args.output / f"{spec.name}.png"
plot_maneuver_report(data, metrics, save_path=out_file, show=args.show)
import matplotlib.pyplot as plt
plt.close("all")
print(f" -> saved {out_file}")
summary_path = args.output / "summary.png"
plot_summary_comparison(all_metrics, save_path=summary_path, show=args.show)
print(f"\nSummary plot: {summary_path}")
print_metrics_table(all_metrics)
return 0
if __name__ == "__main__":
raise SystemExit(main())
+121
View File
@@ -0,0 +1,121 @@
#!/usr/bin/env python3␍
"""SE(3) geometric control demo: circular trajectory tracking."""␍
␍
from __future__ import annotations␍
␍
import argparse␍
import sys␍
from pathlib import Path␍
␍
import numpy as np␍
␍
ROOT = Path(__file__).resolve().parents[1]␍
if str(ROOT) not in sys.path:␍
sys.path.insert(0, str(ROOT / "src"))␍
␍
from uavcatch.config import load_control_config, load_environment_config, load_uav_config␍
from uavcatch.control import TrackingController, circle_trajectory␍
from uavcatch.control.factory import build_cascaded_controller␍
from uavcatch.dynamics.state import QuadState␍
from uavcatch.sim.engine import SimEngine␍
from uavcatch.sim.mujoco_viz import MuJoCoViewer␍
␍
␍
def parse_args() -> argparse.Namespace:␍
parser = argparse.ArgumentParser(␍
description="Geometric SE(3) circular trajectory tracking demo",␍
)␍
parser.add_argument("--environment", type=Path, default=ROOT / "environment.toml")␍
parser.add_argument("--uav", type=Path, default=ROOT / "uav.toml")␍
parser.add_argument("--control", type=Path, default=ROOT / "control.toml")␍
parser.add_argument("--radius", type=float, default=3.0, help="Circle radius (m)")␍
parser.add_argument("--height", type=float, default=2.0, help="Flight height (m)")␍
parser.add_argument("--period", type=float, default=12.0, help="One lap period (s)")␍
parser.add_argument("--duration", type=float, default=24.0, help="Simulation duration (s)")␍
parser.add_argument("--no-viewer", action="store_true")␍
parser.add_argument(␍
"--speed",␍
type=float,␍
default=1.0,␍
help="Playback speed (1.0 = real time, <1 slow motion)",␍
)␍
return parser.parse_args()␍
␍
␍
def main() -> int:␍
args = parse_args()␍
env_cfg = load_environment_config(args.environment)␍
uav_cfg = load_uav_config(args.uav)␍
control_cfg = load_control_config(args.control)␍
env_cfg.simulation.duration = args.duration␍
␍
if control_cfg.controller.mode != "geometric":␍
print(␍
f"Warning: control.toml mode is '{control_cfg.controller.mode}'; "␍
"circle tracking works best with mode = \"geometric\"."␍
)␍
␍
engine = SimEngine(env_cfg=env_cfg, uav_cfg=uav_cfg)␍
engine.reset()␍
␍
center = np.array([0.0, 0.0, 0.0], dtype=np.float64)␍
pos_fn, yaw_fn, accel_fn = circle_trajectory(␍
center=center,␍
radius=args.radius,␍
height=args.height,␍
period=args.period,␍
)␍
init_target = pos_fn(0.0)␍
controller = build_cascaded_controller(␍
control_cfg,␍
engine.dynamics,␍
uav_cfg,␍
target_position=init_target.position,␍
target_velocity=init_target.velocity,␍
target_yaw=float(yaw_fn(0.0)),␍
)␍
tracker = TrackingController(␍
controller=controller,␍
target_fn=pos_fn,␍
yaw_fn=yaw_fn,␍
accel_fn=accel_fn,␍
)␍
␍
duration = args.duration␍
dt = env_cfg.simulation.dt␍
use_viewer = env_cfg.viewer.enable and not args.no_viewer␍
␍
if use_viewer:␍
with MuJoCoViewer(env_cfg, uav_cfg=uav_cfg) as viewer:␍
def on_step(state: QuadState, t: float, omega: np.ndarray) -> None:␍
viewer.render_realtime(state, omega, dt, speed=args.speed)␍
if not viewer.is_running():␍
raise KeyboardInterrupt␍
␍
try:␍
engine.run(tracker, duration=duration, on_step=on_step)␍
except KeyboardInterrupt:␍
pass␍
else:␍
engine.run(tracker, duration=duration)␍
␍
final = engine.state␍
final_target = pos_fn(duration)␍
pos_err = float(np.linalg.norm(final.position - final_target.position))␍
print(f"Simulation finished at t={engine.time:.2f}s")␍
print(f"Final position: {final.position}")␍
print(f"Target position: {final_target.position}")␍
print(f"Position error: {pos_err:.4f} m")␍
print(f"Final velocity: {final.velocity}")␍
␍
if pos_err > 1.0:␍
print("Warning: circular tracking error exceeds 1.0 m")␍
return 1␍
return 0␍
␍
␍
if __name__ == "__main__":␍
try:␍
raise SystemExit(main())␍
except KeyboardInterrupt:␍
raise SystemExit(130) from None␍
+121
View File
@@ -0,0 +1,121 @@
#!/usr/bin/env python3
"""Hover demo: PID-controlled single quadcopter in MuJoCo viewer."""
from __future__ import annotations
import argparse
import sys
from pathlib import Path
import numpy as np
ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path:
sys.path.insert(0, str(ROOT / "src"))
from uavcatch.config import load_control_config, load_environment_config, load_uav_config
from uavcatch.control.pid import HoverPIDController
from uavcatch.dynamics.state import QuadState
from uavcatch.sim.engine import SimEngine
from uavcatch.sim.mujoco_viz import MuJoCoViewer
def parse_args() -> argparse.Namespace:
parser = argparse.ArgumentParser(description="Single-UAV hover simulation demo")
parser.add_argument(
"--environment",
type=Path,
default=ROOT / "environment.toml",
help="Path to environment.toml",
)
parser.add_argument(
"--uav",
type=Path,
default=ROOT / "uav.toml",
help="Path to uav.toml",
)
parser.add_argument(
"--control",
type=Path,
default=ROOT / "control.toml",
help="Path to control.toml",
)
parser.add_argument(
"--target-z",
type=float,
default=2.0,
help="Target hover height (m)",
)
parser.add_argument(
"--no-viewer",
action="store_true",
help="Run without MuJoCo viewer",
)
parser.add_argument(
"--speed",
type=float,
default=1.0,
help="Playback speed (1.0 = real time, <1 slow motion)",
)
return parser.parse_args()
def main() -> int:
args = parse_args()
env_cfg = load_environment_config(args.environment)
uav_cfg = load_uav_config(args.uav)
control_cfg = load_control_config(args.control)
engine = SimEngine(env_cfg=env_cfg, uav_cfg=uav_cfg)
engine.reset()
target = np.array([0.0, 0.0, args.target_z], dtype=np.float64)
controller = HoverPIDController(
dynamics=engine.dynamics,
uav_cfg=uav_cfg,
target_position=target,
control_cfg=control_cfg,
)
duration = env_cfg.simulation.duration
dt = env_cfg.simulation.dt
use_viewer = env_cfg.viewer.enable and not args.no_viewer
if use_viewer:
with MuJoCoViewer(env_cfg, uav_cfg=uav_cfg) as viewer:
last_state: QuadState | None = None
last_omega: np.ndarray | None = None
def on_step(state: QuadState, t: float, omega: np.ndarray) -> None:
nonlocal last_state, last_omega
last_state = state
last_omega = omega
viewer.render_realtime(state, omega, dt, speed=args.speed)
if not viewer.is_running():
raise KeyboardInterrupt
try:
history = engine.run(controller, duration=duration, on_step=on_step)
except KeyboardInterrupt:
history = []
else:
history = engine.run(controller, duration=duration)
final = engine.state
pos_err = np.linalg.norm(final.position - target)
print(f"Simulation finished at t={engine.time:.2f}s")
print(f"Final position: {final.position}")
print(f"Target position: {target}")
print(f"Position error: {pos_err:.4f} m")
print(f"Final velocity: {final.velocity}")
if pos_err > 0.1:
print("Warning: position error exceeds 0.1 m threshold")
return 1
return 0
if __name__ == "__main__":
try:
raise SystemExit(main())
except KeyboardInterrupt:
raise SystemExit(130) from None
+119
View File
@@ -0,0 +1,119 @@
#!/usr/bin/env python3
"""Take off from ground, hover, then fly forward along +X."""
from __future__ import annotations
import argparse
import sys
from pathlib import Path
import numpy as np
ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path:
sys.path.insert(0, str(ROOT / "src"))
from uavcatch.config import load_control_config, load_environment_config, load_uav_config
from uavcatch.control.pid import (
HoverPIDController,
TrackingPIDController,
phased_takeoff_hover_forward,
)
from uavcatch.dynamics.state import QuadState
from uavcatch.sim.engine import SimEngine
from uavcatch.sim.mujoco_viz import MuJoCoViewer
def parse_args() -> argparse.Namespace:
parser = argparse.ArgumentParser(
description="Ground takeoff -> hover -> forward flight demo",
)
parser.add_argument("--environment", type=Path, default=ROOT / "environment.toml")
parser.add_argument("--uav", type=Path, default=ROOT / "uav.toml")
parser.add_argument("--control", type=Path, default=ROOT / "control.toml")
parser.add_argument("--hover-z", type=float, default=2.0, help="Hover height (m)")
parser.add_argument("--forward-x", type=float, default=5.0, help="Forward distance (m)")
parser.add_argument("--takeoff-time", type=float, default=4.0, help="Takeoff phase (s)")
parser.add_argument("--hover-time", type=float, default=3.0, help="Hover phase (s)")
parser.add_argument("--forward-time", type=float, default=8.0, help="Forward phase (s)")
parser.add_argument("--no-viewer", action="store_true")
parser.add_argument(
"--speed",
type=float,
default=1.0,
help="Playback speed (1.0 = real time, <1 slow motion)",
)
return parser.parse_args()
def main() -> int:
args = parse_args()
env_cfg = load_environment_config(args.environment)
uav_cfg = load_uav_config(args.uav)
control_cfg = load_control_config(args.control)
ground_z = env_cfg.scene.ground_z
uav_cfg.initial.position = [0.0, 0.0, ground_z + 0.05]
uav_cfg.initial.velocity = [0.0, 0.0, 0.0]
uav_cfg.initial.attitude_euler = [0.0, 0.0, 0.0]
uav_cfg.initial.angular_rate = [0.0, 0.0, 0.0]
duration = args.takeoff_time + args.hover_time + args.forward_time
env_cfg.simulation.duration = duration
engine = SimEngine(env_cfg=env_cfg, uav_cfg=uav_cfg)
engine.reset()
target_fn = phased_takeoff_hover_forward(
ground_z=ground_z,
hover_z=args.hover_z,
forward_x=args.forward_x,
takeoff_duration=args.takeoff_time,
hover_duration=args.hover_time,
forward_duration=args.forward_time,
)
hover_pid = HoverPIDController(
dynamics=engine.dynamics,
uav_cfg=uav_cfg,
target_position=target_fn(0.0).position,
control_cfg=control_cfg,
)
controller = TrackingPIDController(pid=hover_pid, target_fn=target_fn)
final_target = target_fn(duration).position
dt = env_cfg.simulation.dt
use_viewer = env_cfg.viewer.enable and not args.no_viewer
if use_viewer:
with MuJoCoViewer(env_cfg, uav_cfg=uav_cfg) as viewer:
def on_step(state: QuadState, t: float, omega: np.ndarray) -> None:
viewer.render_realtime(state, omega, dt, speed=args.speed)
if not viewer.is_running():
raise KeyboardInterrupt
try:
engine.run(controller, duration=duration, on_step=on_step)
except KeyboardInterrupt:
pass
else:
engine.run(controller, duration=duration)
final = engine.state
pos_err = np.linalg.norm(final.position - final_target)
print(f"Simulation finished at t={engine.time:.2f}s")
print(f"Final position: {final.position}")
print(f"Expected target: {final_target}")
print(f"Position error: {pos_err:.4f} m")
print(f"Final velocity: {final.velocity}")
if pos_err > 0.5:
print("Warning: final tracking error exceeds 0.5 m")
return 1
return 0
if __name__ == "__main__":
try:
raise SystemExit(main())
except KeyboardInterrupt:
raise SystemExit(130) from None
Binary file not shown.

After

Width:  |  Height:  |  Size: 211 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 37 KiB

+27
View File
@@ -0,0 +1,27 @@
[build-system]
requires = ["setuptools>=61"]
build-backend = "setuptools.build_meta"
[project]
name = "uavcatch"
version = "0.1.0"
description = "Multi-UAV pursuit simulation with explicit quadcopter dynamics"
requires-python = ">=3.10"
dependencies = [
"numpy>=1.24",
"mujoco>=3.0",
"scipy>=1.10",
"matplotlib>=3.7",
"tomli>=2.0.1; python_version < '3.11'",
]
[project.optional-dependencies]
dev = [
"pytest>=7.0",
]
[tool.setuptools.packages.find]
where = ["src"]
[tool.pytest.ini_options]
testpaths = ["tests"]
+6
View File
@@ -0,0 +1,6 @@
numpy>=1.24
mujoco>=3.0
scipy>=1.10
matplotlib>=3.7
tomli>=2.0.1; python_version < '3.11'
pytest>=7.0
+11
View File
@@ -0,0 +1,11 @@
Metadata-Version: 2.4
Name: uavcatch
Version: 0.1.0
Summary: Multi-UAV pursuit simulation with explicit quadcopter dynamics
Requires-Python: >=3.10
Requires-Dist: numpy>=1.24
Requires-Dist: mujoco>=3.0
Requires-Dist: scipy>=1.10
Requires-Dist: tomli>=2.0.1; python_version < "3.11"
Provides-Extra: dev
Requires-Dist: pytest>=7.0; extra == "dev"
+17
View File
@@ -0,0 +1,17 @@
pyproject.toml
src/uavcatch/__init__.py
src/uavcatch/config.py
src/uavcatch.egg-info/PKG-INFO
src/uavcatch.egg-info/SOURCES.txt
src/uavcatch.egg-info/dependency_links.txt
src/uavcatch.egg-info/requires.txt
src/uavcatch.egg-info/top_level.txt
src/uavcatch/control/__init__.py
src/uavcatch/control/pid.py
src/uavcatch/dynamics/__init__.py
src/uavcatch/dynamics/model.py
src/uavcatch/dynamics/state.py
src/uavcatch/sim/__init__.py
src/uavcatch/sim/engine.py
src/uavcatch/sim/mujoco_viz.py
tests/test_dynamics.py
@@ -0,0 +1 @@
+9
View File
@@ -0,0 +1,9 @@
numpy>=1.24
mujoco>=3.0
scipy>=1.10
[:python_version < "3.11"]
tomli>=2.0.1
[dev]
pytest>=7.0
+1
View File
@@ -0,0 +1 @@
uavcatch
+31
View File
@@ -0,0 +1,31 @@
"""UAVCatch: multi-UAV pursuit simulation with explicit quadcopter dynamics."""
from __future__ import annotations
from uavcatch.config import (
ControlConfig,
EnvironmentConfig,
UavConfig,
load_control_config,
load_environment_config,
load_uav_config,
)
from uavcatch.dynamics import QuadDynamics, QuadState, WindModel
from uavcatch.sim import MuJoCoViewer, SimEngine
__version__ = "0.1.0"
__all__ = [
"ControlConfig",
"EnvironmentConfig",
"UavConfig",
"QuadDynamics",
"QuadState",
"WindModel",
"SimEngine",
"MuJoCoViewer",
"load_control_config",
"load_environment_config",
"load_uav_config",
"__version__",
]
Binary file not shown.
Binary file not shown.
+382
View File
@@ -0,0 +1,382 @@
"""Load project settings from TOML configuration files."""
from __future__ import annotations
import sys
from dataclasses import dataclass, field
from pathlib import Path
from typing import Any, Literal
IntegratorKind = Literal["euler", "rk4"]
RotorLayout = Literal["x", "plus"]
ControlMode = Literal["linear", "geometric"]
def _load_toml(path: Path) -> dict[str, Any]:
data = path.read_bytes()
if sys.version_info >= (3, 11):
import tomllib
return tomllib.loads(data.decode("utf-8"))
import tomli
return tomli.loads(data.decode("utf-8"))
@dataclass
class SceneConfig:
size_x: float = 20.0
size_y: float = 20.0
size_z: float = 10.0
ground_z: float = 0.0
@dataclass
class PhysicsConfig:
gravity: float = 9.81
air_density: float = 1.225
@dataclass
class SimulationConfig:
dt: float = 0.002
duration: float = 10.0
integrator: IntegratorKind = "rk4"
@dataclass
class ViewerConfig:
enable: bool = True
fps: int = 60
camera_distance: float = 8.0
@dataclass
class WindConfig:
"""Ambient wind field (world frame)."""
velocity: list[float] = field(default_factory=lambda: [0.0, 0.0, 0.0])
gust_amplitude: float = 0.0
gust_frequency: float = 0.0
@dataclass
class EnvironmentConfig:
scene: SceneConfig = field(default_factory=SceneConfig)
physics: PhysicsConfig = field(default_factory=PhysicsConfig)
simulation: SimulationConfig = field(default_factory=SimulationConfig)
viewer: ViewerConfig = field(default_factory=ViewerConfig)
wind: WindConfig = field(default_factory=WindConfig)
@classmethod
def from_dict(cls, data: dict[str, Any]) -> EnvironmentConfig:
return EnvironmentConfig(
scene=_parse_scene(data.get("scene", {})),
physics=_parse_physics(data.get("physics", {})),
simulation=_parse_simulation(data.get("simulation", {})),
viewer=_parse_viewer(data.get("viewer", {})),
wind=_parse_wind(data.get("wind", {})),
)
@dataclass
class BodyConfig:
mass: float = 1.0
arm_length: float = 0.25
Ixx: float = 0.01
Iyy: float = 0.01
Izz: float = 0.02
@dataclass
class RotorConfig:
kf: float = 8.11e-6
km: float = 2.03e-7
omega_min: float = 0.0
omega_max: float = 838.0
layout: RotorLayout = "x"
@dataclass
class InitialConfig:
position: list[float] = field(default_factory=lambda: [0.0, 0.0, 1.0])
velocity: list[float] = field(default_factory=lambda: [0.0, 0.0, 0.0])
attitude_euler: list[float] = field(default_factory=lambda: [0.0, 0.0, 0.0])
angular_rate: list[float] = field(default_factory=lambda: [0.0, 0.0, 0.0])
@dataclass
class DragConfig:
"""Aerodynamic drag acting on the air-relative velocity.
Total translational drag force (world frame):
F = -linear * v_rel - 0.5 * rho * cd * area * |v_rel| * v_rel
where v_rel = v_body - v_wind, rho = physics.air_density.
"""
linear: float = 0.0
angular: float = 0.05
cd: float = 0.8
area: float = 0.1
@dataclass
class UavConfig:
body: BodyConfig = field(default_factory=BodyConfig)
rotor: RotorConfig = field(default_factory=RotorConfig)
initial: InitialConfig = field(default_factory=InitialConfig)
drag: DragConfig = field(default_factory=DragConfig)
@classmethod
def from_dict(cls, data: dict[str, Any]) -> UavConfig:
return UavConfig(
body=_parse_body(data.get("body", {})),
rotor=_parse_rotor(data.get("rotor", {})),
initial=_parse_initial(data.get("initial", {})),
drag=_parse_drag(data.get("drag", {})),
)
def _parse_scene(d: dict[str, Any]) -> SceneConfig:
return SceneConfig(
size_x=float(d.get("size_x", 20.0)),
size_y=float(d.get("size_y", 20.0)),
size_z=float(d.get("size_z", 10.0)),
ground_z=float(d.get("ground_z", 0.0)),
)
def _parse_physics(d: dict[str, Any]) -> PhysicsConfig:
return PhysicsConfig(
gravity=float(d.get("gravity", 9.81)),
air_density=float(d.get("air_density", 1.225)),
)
def _parse_simulation(d: dict[str, Any]) -> SimulationConfig:
integrator = d.get("integrator", "rk4")
if integrator not in ("euler", "rk4"):
integrator = "rk4"
return SimulationConfig(
dt=float(d.get("dt", 0.002)),
duration=float(d.get("duration", 10.0)),
integrator=integrator,
)
def _parse_viewer(d: dict[str, Any]) -> ViewerConfig:
return ViewerConfig(
enable=bool(d.get("enable", True)),
fps=int(d.get("fps", 60)),
camera_distance=float(d.get("camera_distance", 8.0)),
)
def _parse_wind(d: dict[str, Any]) -> WindConfig:
velocity = list(d.get("velocity", [0.0, 0.0, 0.0]))
return WindConfig(
velocity=[float(v) for v in velocity],
gust_amplitude=float(d.get("gust_amplitude", 0.0)),
gust_frequency=float(d.get("gust_frequency", 0.0)),
)
def _parse_body(d: dict[str, Any]) -> BodyConfig:
return BodyConfig(
mass=float(d.get("mass", 1.0)),
arm_length=float(d.get("arm_length", 0.25)),
Ixx=float(d.get("Ixx", 0.01)),
Iyy=float(d.get("Iyy", 0.01)),
Izz=float(d.get("Izz", 0.02)),
)
def _parse_rotor(d: dict[str, Any]) -> RotorConfig:
layout = d.get("layout", "x")
if layout not in ("x", "plus"):
layout = "x"
return RotorConfig(
kf=float(d.get("kf", 3.16e-10)),
km=float(d.get("km", 7.94e-12)),
omega_min=float(d.get("omega_min", 0.0)),
omega_max=float(d.get("omega_max", 838.0)),
layout=layout,
)
def _parse_initial(d: dict[str, Any]) -> InitialConfig:
return InitialConfig(
position=list(d.get("position", [0.0, 0.0, 1.0])),
velocity=list(d.get("velocity", [0.0, 0.0, 0.0])),
attitude_euler=list(d.get("attitude_euler", [0.0, 0.0, 0.0])),
angular_rate=list(d.get("angular_rate", [0.0, 0.0, 0.0])),
)
def _parse_drag(d: dict[str, Any]) -> DragConfig:
return DragConfig(
linear=float(d.get("linear", 0.0)),
angular=float(d.get("angular", 0.05)),
cd=float(d.get("cd", 0.8)),
area=float(d.get("area", 0.1)),
)
def load_environment_config(path: str | Path = "environment.toml") -> EnvironmentConfig:
config_path = Path(path)
if not config_path.is_file():
raise FileNotFoundError(f"Environment config not found: {config_path}")
return EnvironmentConfig.from_dict(_load_toml(config_path))
def load_uav_config(path: str | Path = "uav.toml") -> UavConfig:
config_path = Path(path)
if not config_path.is_file():
raise FileNotFoundError(f"UAV config not found: {config_path}")
return UavConfig.from_dict(_load_toml(config_path))
@dataclass
class GainsConfig:
kp: float = 0.8
ki: float = 0.2
kd: float = 0.9
@dataclass
class ControllerSection:
mode: ControlMode = "linear"
max_tilt: float = 0.35
@dataclass
class EvalHoverConfig:
duration: float = 8.0
@dataclass
class EvalStepConfig:
step_time: float = 2.0
duration: float = 12.0
step_z: float = 3.0
step_x: float = 3.0
step_y: float = 2.0
@dataclass
class EvalTakeoffForwardConfig:
ground_z: float = 0.0
hover_z: float = 2.0
forward_x: float = 4.0
takeoff_time: float = 3.0
hover_time: float = 2.0
forward_time: float = 6.0
@dataclass
class EvalCircleConfig:
ground_z: float = 0.0
takeoff_time: float = 2.0
radius: float = 2.5
height: float = 2.0
period: float = 10.0
duration: float = 20.0
@dataclass
class EvalConfig:
initial_height: float = 2.0
hover: EvalHoverConfig = field(default_factory=EvalHoverConfig)
step: EvalStepConfig = field(default_factory=EvalStepConfig)
takeoff_forward: EvalTakeoffForwardConfig = field(default_factory=EvalTakeoffForwardConfig)
circle: EvalCircleConfig = field(default_factory=EvalCircleConfig)
@dataclass
class ControlConfig:
controller: ControllerSection = field(default_factory=ControllerSection)
position_linear: GainsConfig = field(default_factory=lambda: GainsConfig(kp=0.8, ki=0.2, kd=0.9))
position_geometric: GainsConfig = field(
default_factory=lambda: GainsConfig(kp=2.5, ki=0.4, kd=2.0)
)
attitude_linear: GainsConfig = field(
default_factory=lambda: GainsConfig(kp=8.0, ki=0.5, kd=0.6)
)
attitude_geometric: GainsConfig = field(
default_factory=lambda: GainsConfig(kp=10.0, ki=0.8, kd=0.8)
)
eval: EvalConfig = field(default_factory=EvalConfig)
@classmethod
def from_dict(cls, data: dict[str, Any]) -> ControlConfig:
return ControlConfig(
controller=_parse_controller(data.get("controller", {})),
position_linear=_parse_gains(data.get("position", {}).get("linear", {}), 0.8, 0.2, 0.9),
position_geometric=_parse_gains(
data.get("position", {}).get("geometric", {}), 2.5, 0.4, 2.0
),
attitude_linear=_parse_gains(data.get("attitude", {}).get("linear", {}), 8.0, 0.5, 0.6),
attitude_geometric=_parse_gains(
data.get("attitude", {}).get("geometric", {}), 10.0, 0.8, 0.8
),
eval=_parse_eval(data.get("eval", {})),
)
def _parse_gains(d: dict[str, Any], kp: float, ki: float, kd: float) -> GainsConfig:
return GainsConfig(
kp=float(d.get("kp", kp)),
ki=float(d.get("ki", ki)),
kd=float(d.get("kd", kd)),
)
def _parse_controller(d: dict[str, Any]) -> ControllerSection:
mode = d.get("mode", "linear")
if mode not in ("linear", "geometric"):
mode = "linear"
return ControllerSection(
mode=mode,
max_tilt=float(d.get("max_tilt", 0.35)),
)
def _parse_eval(d: dict[str, Any]) -> EvalConfig:
hover_d = d.get("hover", {})
step_d = d.get("step", {})
takeoff_d = d.get("takeoff_forward", {})
circle_d = d.get("circle", {})
return EvalConfig(
initial_height=float(d.get("initial_height", 2.0)),
hover=EvalHoverConfig(duration=float(hover_d.get("duration", 8.0))),
step=EvalStepConfig(
step_time=float(step_d.get("step_time", 2.0)),
duration=float(step_d.get("duration", 12.0)),
step_z=float(step_d.get("step_z", 3.0)),
step_x=float(step_d.get("step_x", 3.0)),
step_y=float(step_d.get("step_y", 2.0)),
),
takeoff_forward=EvalTakeoffForwardConfig(
ground_z=float(takeoff_d.get("ground_z", 0.0)),
hover_z=float(takeoff_d.get("hover_z", 2.0)),
forward_x=float(takeoff_d.get("forward_x", 4.0)),
takeoff_time=float(takeoff_d.get("takeoff_time", 3.0)),
hover_time=float(takeoff_d.get("hover_time", 2.0)),
forward_time=float(takeoff_d.get("forward_time", 6.0)),
),
circle=EvalCircleConfig(
ground_z=float(circle_d.get("ground_z", 0.0)),
takeoff_time=float(circle_d.get("takeoff_time", 2.0)),
radius=float(circle_d.get("radius", 2.5)),
height=float(circle_d.get("height", 2.0)),
period=float(circle_d.get("period", 10.0)),
duration=float(circle_d.get("duration", 20.0)),
),
)
def load_control_config(path: str | Path = "control.toml") -> ControlConfig:
config_path = Path(path)
if not config_path.is_file():
raise FileNotFoundError(f"Control config not found: {config_path}")
return ControlConfig.from_dict(_load_toml(config_path))
+30
View File
@@ -0,0 +1,30 @@
from uavcatch.control.allocator import ControlAllocator
from uavcatch.control.attitude import AttitudeController
from uavcatch.control.cascade import CascadedController, TrackingController
from uavcatch.control.factory import build_cascaded_controller
from uavcatch.control.pid import (
HoverPIDController,
TrackingPIDController,
circle_trajectory,
phased_takeoff_hover_forward,
smoothstep,
)
from uavcatch.control.position import GeometricPositionController, LinearPositionController
from uavcatch.control.types import PIDGains, PhaseTarget
__all__ = [
"ControlAllocator",
"AttitudeController",
"CascadedController",
"TrackingController",
"build_cascaded_controller",
"GeometricPositionController",
"LinearPositionController",
"HoverPIDController",
"TrackingPIDController",
"PIDGains",
"PhaseTarget",
"circle_trajectory",
"phased_takeoff_hover_forward",
"smoothstep",
]
Binary file not shown.
+46
View File
@@ -0,0 +1,46 @@
"""Motor mixing / control allocation."""
from __future__ import annotations
from dataclasses import dataclass, field
import numpy as np
from uavcatch.config import UavConfig
@dataclass
class ControlAllocator:
"""Map total thrust and body torques to per-rotor squared speeds."""
uav_cfg: UavConfig
_mixing_matrix: np.ndarray = field(init=False)
def __post_init__(self) -> None:
self._mixing_matrix = np.array(
[
[1.0, 1.0, 1.0, 1.0],
[0.0, -1.0, 0.0, 1.0],
[-1.0, 0.0, 1.0, 0.0],
[1.0, -1.0, 1.0, -1.0],
],
dtype=np.float64,
)
def allocate(self, thrust: float, torque: np.ndarray) -> np.ndarray:
kf = self.uav_cfg.rotor.kf
km = self.uav_cfg.rotor.km
arm = self.uav_cfg.body.arm_length
tx, ty, tz = np.asarray(torque, dtype=np.float64)
if self.uav_cfg.rotor.layout == "plus":
tx, ty = ty, tx
rhs = np.array(
[thrust / kf, tx / (kf * arm), ty / (kf * arm), tz / km],
dtype=np.float64,
)
sq = np.linalg.solve(self._mixing_matrix, rhs)
max_sq = self.uav_cfg.rotor.omega_max ** 2
sq = np.clip(sq, 0.0, max_sq)
return np.sqrt(sq)
+46
View File
@@ -0,0 +1,46 @@
"""Quaternion-based attitude controller."""
from __future__ import annotations
from dataclasses import dataclass, field
import numpy as np
from uavcatch.control.math3d import quaternion_attitude_error
from uavcatch.control.types import AttitudeCommand, PIDGains
from uavcatch.dynamics.state import QuadState, normalize_quaternion
@dataclass
class AttitudeController:
"""Inner-loop attitude control using quaternion error and PID."""
gains: PIDGains = field(default_factory=lambda: PIDGains(kp=8.0, ki=0.5, kd=0.6))
_integral: np.ndarray = field(default_factory=lambda: np.zeros(3, dtype=np.float64))
_integral_limit: float = 0.5
def reset(self) -> None:
self._integral[:] = 0.0
def compute(
self,
state: QuadState,
desired_quaternion: np.ndarray,
desired_angular_rate: np.ndarray | None = None,
dt: float = 0.002,
) -> AttitudeCommand:
q_des = normalize_quaternion(desired_quaternion)
e_att = quaternion_attitude_error(q_des, state.quaternion)
self._integral += e_att * dt
self._integral = np.clip(self._integral, -self._integral_limit, self._integral_limit)
if desired_angular_rate is None:
desired_angular_rate = np.zeros(3, dtype=np.float64)
rate_err = state.angular_rate - desired_angular_rate
torque = (
-self.gains.kp * e_att
+ self.gains.ki * self._integral
- self.gains.kd * rate_err
)
return AttitudeCommand(torque=torque.astype(np.float64))
+148
View File
@@ -0,0 +1,148 @@
"""Cascaded 6-DOF flight controller."""
from __future__ import annotations
from dataclasses import dataclass, field
from typing import Literal
import numpy as np
from uavcatch.config import UavConfig
from uavcatch.control.allocator import ControlAllocator
from uavcatch.control.attitude import AttitudeController
from uavcatch.control.position import GeometricPositionController, LinearPositionController
from uavcatch.control.types import PIDGains, PhaseTarget, TargetFn
from uavcatch.dynamics.model import QuadDynamics
from uavcatch.dynamics.state import QuadState
PositionMode = Literal["linear", "geometric"]
@dataclass
class CascadedController:
"""Modular cascade: position -> attitude (quaternion) -> motor allocation."""
dynamics: QuadDynamics
uav_cfg: UavConfig
mode: PositionMode = "linear"
target_position: np.ndarray = field(default_factory=lambda: np.zeros(3))
target_velocity: np.ndarray = field(default_factory=lambda: np.zeros(3))
target_yaw: float = 0.0
target_acceleration: np.ndarray | None = None
pos_gains: PIDGains = field(
default_factory=lambda: PIDGains(kp=0.8, ki=0.2, kd=0.9)
)
att_gains: PIDGains = field(
default_factory=lambda: PIDGains(kp=8.0, ki=0.5, kd=0.6)
)
max_tilt: float = 0.35
allocator: ControlAllocator = field(init=False)
attitude: AttitudeController = field(init=False)
_position_linear: LinearPositionController = field(init=False)
_position_geometric: GeometricPositionController = field(init=False)
def __post_init__(self) -> None:
mass = self.uav_cfg.body.mass
gravity = self.dynamics.env_cfg.physics.gravity
max_thrust = 4.0 * self.uav_cfg.rotor.kf * self.uav_cfg.rotor.omega_max ** 2
self.allocator = ControlAllocator(self.uav_cfg)
self.attitude = AttitudeController(gains=self.att_gains)
self._position_linear = LinearPositionController(
mass=mass,
gravity=gravity,
max_thrust=max_thrust,
gains=self.pos_gains,
max_tilt=self.max_tilt,
)
self._position_geometric = GeometricPositionController(
mass=mass,
gravity=gravity,
max_thrust=max_thrust,
gains=self.pos_gains,
)
def reset(self) -> None:
self.attitude.reset()
self._position_linear.reset()
self._position_geometric.reset()
def __call__(self, state: QuadState, t: float) -> np.ndarray:
del t
dt = self.dynamics.env_cfg.simulation.dt
return self.compute(state, dt)
def compute(self, state: QuadState, dt: float | None = None) -> np.ndarray:
if dt is None:
dt = self.dynamics.env_cfg.simulation.dt
if self.mode == "geometric":
pos_cmd = self._position_geometric.compute(
state,
self.target_position,
self.target_velocity,
self.target_yaw,
dt,
target_acceleration=self.target_acceleration,
)
else:
pos_cmd = self._position_linear.compute(
state,
self.target_position,
self.target_velocity,
self.target_yaw,
dt,
)
att_cmd = self.attitude.compute(
state,
pos_cmd.desired_quaternion,
pos_cmd.desired_angular_rate,
dt,
)
omega = self.allocator.allocate(pos_cmd.thrust, att_cmd.torque)
return self.dynamics.clip_omega(omega)
@dataclass
class TrackingController:
"""Time-varying setpoint wrapper for CascadedController."""
controller: CascadedController
target_fn: TargetFn
yaw_fn: TargetFn | None = None
accel_fn: TargetFn | None = None
_prev_t: float = field(default=-1.0, init=False)
_prev_pos: np.ndarray | None = field(default=None, init=False)
def __call__(self, state: QuadState, t: float) -> np.ndarray:
target = self.target_fn(t)
if isinstance(target, PhaseTarget):
pos = np.asarray(target.position, dtype=np.float64)
vel = np.asarray(target.velocity, dtype=np.float64)
else:
pos = np.asarray(target, dtype=np.float64)
vel = self._estimate_target_velocity(pos, t)
self.controller.target_position = pos
self.controller.target_velocity = vel
if self.yaw_fn is not None:
yaw = self.yaw_fn(t)
self.controller.target_yaw = float(yaw if np.isscalar(yaw) else yaw[0])
if self.accel_fn is not None:
accel = self.accel_fn(t)
self.controller.target_acceleration = np.asarray(accel, dtype=np.float64)
else:
self.controller.target_acceleration = None
return self.controller(state, t)
def _estimate_target_velocity(self, pos: np.ndarray, t: float) -> np.ndarray:
if self._prev_pos is None or self._prev_t < 0.0:
self._prev_pos = pos.copy()
self._prev_t = t
return np.zeros(3, dtype=np.float64)
dt = max(t - self._prev_t, 1e-9)
vel = (pos - self._prev_pos) / dt
self._prev_pos = pos.copy()
self._prev_t = t
return vel.astype(np.float64)
+49
View File
@@ -0,0 +1,49 @@
"""Build controllers from control.toml configuration."""
from __future__ import annotations
import numpy as np
from uavcatch.config import ControlConfig, UavConfig
from uavcatch.control.cascade import CascadedController
from uavcatch.control.types import PIDGains
from uavcatch.dynamics.model import QuadDynamics
def gains_from_control(control_cfg: ControlConfig) -> tuple[PIDGains, PIDGains]:
if control_cfg.controller.mode == "geometric":
pos = control_cfg.position_geometric
att = control_cfg.attitude_geometric
else:
pos = control_cfg.position_linear
att = control_cfg.attitude_linear
return (
PIDGains(kp=pos.kp, ki=pos.ki, kd=pos.kd),
PIDGains(kp=att.kp, ki=att.ki, kd=att.kd),
)
def build_cascaded_controller(
control_cfg: ControlConfig,
dynamics: QuadDynamics,
uav_cfg: UavConfig,
target_position: np.ndarray,
target_velocity: np.ndarray | None = None,
target_yaw: float = 0.0,
) -> CascadedController:
pos_gains, att_gains = gains_from_control(control_cfg)
return CascadedController(
dynamics=dynamics,
uav_cfg=uav_cfg,
mode=control_cfg.controller.mode,
target_position=np.asarray(target_position, dtype=np.float64),
target_velocity=(
np.zeros(3, dtype=np.float64)
if target_velocity is None
else np.asarray(target_velocity, dtype=np.float64)
),
target_yaw=target_yaw,
pos_gains=pos_gains,
att_gains=att_gains,
max_tilt=control_cfg.controller.max_tilt,
)
+98
View File
@@ -0,0 +1,98 @@
"""Quaternion and rotation helpers for control."""
from __future__ import annotations
import numpy as np
from uavcatch.dynamics.state import euler_to_quaternion, normalize_quaternion
def quaternion_conjugate(q: np.ndarray) -> np.ndarray:
q = np.asarray(q, dtype=np.float64)
return np.array([q[0], -q[1], -q[2], -q[3]], dtype=np.float64)
def quaternion_multiply(q1: np.ndarray, q2: np.ndarray) -> np.ndarray:
w1, x1, y1, z1 = q1
w2, x2, y2, z2 = q2
return normalize_quaternion(
np.array(
[
w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2,
w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2,
w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2,
w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2,
],
dtype=np.float64,
)
)
def quaternion_from_yaw(yaw: float) -> np.ndarray:
return euler_to_quaternion(0.0, 0.0, yaw)
def rotation_matrix_to_quaternion(R: np.ndarray) -> np.ndarray:
"""Convert body-to-world rotation matrix to quaternion [w, x, y, z]."""
R = np.asarray(R, dtype=np.float64)
trace = float(np.trace(R))
if trace > 0.0:
s = np.sqrt(trace + 1.0) * 2.0
w = 0.25 * s
x = (R[2, 1] - R[1, 2]) / s
y = (R[0, 2] - R[2, 0]) / s
z = (R[1, 0] - R[0, 1]) / s
elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
s = np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2.0
w = (R[2, 1] - R[1, 2]) / s
x = 0.25 * s
y = (R[0, 1] + R[1, 0]) / s
z = (R[0, 2] + R[2, 0]) / s
elif R[1, 1] > R[2, 2]:
s = np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2.0
w = (R[0, 2] - R[2, 0]) / s
x = (R[0, 1] + R[1, 0]) / s
y = 0.25 * s
z = (R[1, 2] + R[2, 1]) / s
else:
s = np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2.0
w = (R[1, 0] - R[0, 1]) / s
x = (R[0, 2] + R[2, 0]) / s
y = (R[1, 2] + R[2, 1]) / s
z = 0.25 * s
return normalize_quaternion(np.array([w, x, y, z], dtype=np.float64))
def desired_attitude_from_thrust_and_yaw(
thrust_direction_world: np.ndarray,
yaw: float,
) -> np.ndarray:
"""Build desired body attitude quaternion from thrust axis and yaw."""
z_b = np.asarray(thrust_direction_world, dtype=np.float64)
norm = np.linalg.norm(z_b)
if norm < 1e-9:
z_b = np.array([0.0, 0.0, 1.0], dtype=np.float64)
else:
z_b = z_b / norm
x_c = np.array([np.cos(yaw), np.sin(yaw), 0.0], dtype=np.float64)
y_b = np.cross(z_b, x_c)
y_norm = np.linalg.norm(y_b)
if y_norm < 1e-6:
x_c = np.array([1.0, 0.0, 0.0], dtype=np.float64)
y_b = np.cross(z_b, x_c)
y_norm = np.linalg.norm(y_b)
y_b = y_b / y_norm
x_b = np.cross(y_b, z_b)
R = np.column_stack([x_b, y_b, z_b])
return rotation_matrix_to_quaternion(R)
def quaternion_attitude_error(q_des: np.ndarray, q: np.ndarray) -> np.ndarray:
"""Rotation error vector (body frame) from desired/current quaternions."""
q_des = normalize_quaternion(q_des)
q = normalize_quaternion(q)
q_e = quaternion_multiply(quaternion_conjugate(q_des), q)
if q_e[0] < 0.0:
q_e = -q_e
return (2.0 * np.sign(q_e[0]) * q_e[1:4]).astype(np.float64)
+208
View File
@@ -0,0 +1,208 @@
"""Trajectory helpers and backward-compatible controller aliases."""
from __future__ import annotations
import math
from collections.abc import Callable
import numpy as np
from uavcatch.config import UavConfig, ControlConfig
from uavcatch.control.cascade import CascadedController, TrackingController
from uavcatch.control.factory import build_cascaded_controller, gains_from_control
from uavcatch.control.types import PhaseTarget, PIDGains, TargetFn
from uavcatch.dynamics.model import QuadDynamics
from uavcatch.dynamics.state import QuadState
def smoothstep(u: float) -> float:
u = float(np.clip(u, 0.0, 1.0))
return u * u * (3.0 - 2.0 * u)
def smoothstep_deriv(u: float) -> float:
u = float(np.clip(u, 0.0, 1.0))
return 6.0 * u * (1.0 - u)
def phased_takeoff_hover_forward(
ground_z: float,
hover_z: float,
forward_x: float,
takeoff_duration: float,
hover_duration: float,
forward_duration: float,
ground_clearance: float = 0.05,
) -> TargetFn:
"""Build setpoint: ground -> hover -> move along +X."""
t1 = takeoff_duration
t2 = t1 + hover_duration
start_z = ground_z + ground_clearance
def target_at(t: float) -> PhaseTarget:
if t < t1:
u = smoothstep(t / max(t1, 1e-9))
z = start_z + u * (hover_z - start_z)
vz = (hover_z - start_z) * smoothstep_deriv(t / max(t1, 1e-9)) / max(t1, 1e-9)
return PhaseTarget(
position=np.array([0.0, 0.0, z], dtype=np.float64),
velocity=np.array([0.0, 0.0, vz], dtype=np.float64),
)
if t < t2:
return PhaseTarget(
position=np.array([0.0, 0.0, hover_z], dtype=np.float64),
velocity=np.zeros(3, dtype=np.float64),
)
u = (t - t2) / max(forward_duration, 1e-9)
s = smoothstep(u)
ds = smoothstep_deriv(u) / max(forward_duration, 1e-9)
return PhaseTarget(
position=np.array([forward_x * s, 0.0, hover_z], dtype=np.float64),
velocity=np.array([forward_x * ds, 0.0, 0.0], dtype=np.float64),
)
return target_at
def circle_trajectory(
center: np.ndarray,
radius: float,
height: float,
period: float,
clockwise: bool = False,
) -> tuple[TargetFn, TargetFn, TargetFn]:
"""Return position, yaw, and feedforward acceleration callbacks for a circle."""
center = np.asarray(center, dtype=np.float64)
omega = 2.0 * math.pi / max(period, 1e-9)
sign = -1.0 if clockwise else 1.0
def position_at(t: float) -> PhaseTarget:
angle = sign * omega * t
c = math.cos(angle)
s = math.sin(angle)
pos = center + np.array([radius * c, radius * s, height], dtype=np.float64)
vel = np.array(
[-sign * radius * omega * s, sign * radius * omega * c, 0.0],
dtype=np.float64,
)
return PhaseTarget(position=pos, velocity=vel)
def yaw_at(t: float) -> float:
angle = sign * omega * t
tangent = np.array(
[-sign * math.sin(angle), sign * math.cos(angle)],
dtype=np.float64,
)
return float(math.atan2(tangent[1], tangent[0]))
def accel_at(t: float) -> np.ndarray:
angle = sign * omega * t
c = math.cos(angle)
s = math.sin(angle)
centripetal = -(omega ** 2) * radius
return np.array([centripetal * c, centripetal * s, 0.0], dtype=np.float64)
return position_at, yaw_at, accel_at
def circle_trajectory_with_takeoff(
center: np.ndarray,
radius: float,
height: float,
period: float,
takeoff_time: float,
ground_z: float = 0.0,
ground_clearance: float = 0.05,
clockwise: bool = False,
) -> tuple[TargetFn, TargetFn, TargetFn]:
"""Take off from ground, then track a horizontal circle."""
pos_fn, yaw_fn, accel_fn = circle_trajectory(
center=center,
radius=radius,
height=height,
period=period,
clockwise=clockwise,
)
center = np.asarray(center, dtype=np.float64)
start_z = ground_z + ground_clearance
start_pos = np.array([center[0], center[1], start_z], dtype=np.float64)
circle_entry = pos_fn(0.0)
entry_pos = np.asarray(circle_entry.position, dtype=np.float64)
entry_yaw = float(yaw_fn(0.0))
t_takeoff = max(takeoff_time, 1e-9)
def position_at(t: float) -> PhaseTarget:
if t < t_takeoff:
u = t / t_takeoff
s = smoothstep(u)
ds = smoothstep_deriv(u) / t_takeoff
pos = start_pos + s * (entry_pos - start_pos)
vel = ds * (entry_pos - start_pos)
return PhaseTarget(position=pos, velocity=vel)
return pos_fn(t - t_takeoff)
def yaw_at(t: float) -> float:
if t < t_takeoff:
return entry_yaw
return yaw_fn(t - t_takeoff)
def accel_at(t: float) -> np.ndarray:
if t < t_takeoff:
return np.zeros(3, dtype=np.float64)
return accel_fn(t - t_takeoff)
return position_at, yaw_at, accel_at
class HoverPIDController(CascadedController):
"""Backward-compatible alias for cascaded hover control."""
def __init__(
self,
dynamics: QuadDynamics,
uav_cfg: UavConfig,
target_position: np.ndarray,
target_velocity: np.ndarray | None = None,
control_cfg: ControlConfig | None = None,
pos_gains: PIDGains | None = None,
att_gains: PIDGains | None = None,
yaw_gains: PIDGains | None = None,
) -> None:
del yaw_gains
if control_cfg is not None:
pos_g, att_g = gains_from_control(control_cfg)
super().__init__(
dynamics=dynamics,
uav_cfg=uav_cfg,
mode=control_cfg.controller.mode,
target_position=np.asarray(target_position, dtype=np.float64),
target_velocity=(
np.zeros(3, dtype=np.float64)
if target_velocity is None
else np.asarray(target_velocity, dtype=np.float64)
),
pos_gains=pos_g,
att_gains=att_g,
max_tilt=control_cfg.controller.max_tilt,
)
return
super().__init__(
dynamics=dynamics,
uav_cfg=uav_cfg,
mode="linear",
target_position=np.asarray(target_position, dtype=np.float64),
target_velocity=(
np.zeros(3, dtype=np.float64)
if target_velocity is None
else np.asarray(target_velocity, dtype=np.float64)
),
pos_gains=pos_gains or PIDGains(kp=0.8, ki=0.2, kd=0.9),
att_gains=att_gains or PIDGains(kp=8.0, ki=0.5, kd=0.6),
)
class TrackingPIDController(TrackingController):
"""Backward-compatible alias for time-varying linear cascade."""
def __init__(self, pid: HoverPIDController, target_fn: TargetFn) -> None:
super().__init__(controller=pid, target_fn=target_fn)
+100
View File
@@ -0,0 +1,100 @@
"""Outer-loop position controllers (linear and SE(3) geometric)."""
from __future__ import annotations
from dataclasses import dataclass, field
import numpy as np
from uavcatch.control.math3d import desired_attitude_from_thrust_and_yaw
from uavcatch.control.types import PIDGains, PositionCommand
from uavcatch.dynamics.state import QuadState, euler_to_quaternion
@dataclass
class LinearPositionController:
"""Small-angle position controller (legacy / hover-friendly)."""
mass: float
gravity: float
max_thrust: float
gains: PIDGains = field(default_factory=lambda: PIDGains(kp=0.8, ki=0.2, kd=0.9))
max_tilt: float = 0.35
_integral: np.ndarray = field(default_factory=lambda: np.zeros(3, dtype=np.float64))
_integral_limit: float = 3.0
def reset(self) -> None:
self._integral[:] = 0.0
def compute(
self,
state: QuadState,
target_position: np.ndarray,
target_velocity: np.ndarray,
target_yaw: float,
dt: float,
) -> PositionCommand:
pos_err = target_position - state.position
vel_err = target_velocity - state.velocity
self._integral += pos_err * dt
self._integral = np.clip(self._integral, -self._integral_limit, self._integral_limit)
ax = self.gains.kp * pos_err[0] + self.gains.kd * vel_err[0]
ay = self.gains.kp * pos_err[1] + self.gains.kd * vel_err[1]
desired_pitch = float(np.clip(ax / self.gravity, -self.max_tilt, self.max_tilt))
desired_roll = float(np.clip(-ay / self.gravity, -self.max_tilt, self.max_tilt))
q_des = euler_to_quaternion(desired_roll, desired_pitch, target_yaw)
thrust = self.mass * (
self.gravity
+ self.gains.kp * pos_err[2]
+ self.gains.ki * self._integral[2]
+ self.gains.kd * vel_err[2]
)
thrust = float(np.clip(thrust, 0.1 * self.mass * self.gravity, self.max_thrust))
return PositionCommand(thrust=thrust, desired_quaternion=q_des)
@dataclass
class GeometricPositionController:
"""SE(3) geometric position controller (Lee et al. style)."""
mass: float
gravity: float
max_thrust: float
gains: PIDGains = field(default_factory=lambda: PIDGains(kp=2.5, ki=0.4, kd=2.0))
_integral: np.ndarray = field(default_factory=lambda: np.zeros(3, dtype=np.float64))
_integral_limit: float = 2.0
def reset(self) -> None:
self._integral[:] = 0.0
def compute(
self,
state: QuadState,
target_position: np.ndarray,
target_velocity: np.ndarray,
target_yaw: float,
dt: float,
target_acceleration: np.ndarray | None = None,
) -> PositionCommand:
pos_err = target_position - state.position
vel_err = target_velocity - state.velocity
self._integral += pos_err * dt
self._integral = np.clip(self._integral, -self._integral_limit, self._integral_limit)
a_cmd = (
self.gains.kp * pos_err
+ self.gains.kd * vel_err
+ self.gains.ki * self._integral
)
if target_acceleration is not None:
a_cmd = a_cmd + np.asarray(target_acceleration, dtype=np.float64)
a_cmd[2] += self.gravity
thrust = self.mass * float(np.linalg.norm(a_cmd))
thrust = float(np.clip(thrust, 0.1 * self.mass * self.gravity, self.max_thrust))
z_b_des = a_cmd / max(np.linalg.norm(a_cmd), 1e-9)
q_des = desired_attitude_from_thrust_and_yaw(z_b_des, target_yaw)
return PositionCommand(thrust=thrust, desired_quaternion=q_des)
+42
View File
@@ -0,0 +1,42 @@
"""Shared control types."""
from __future__ import annotations
from collections.abc import Callable
from dataclasses import dataclass, field
import numpy as np
@dataclass
class PIDGains:
kp: float
ki: float
kd: float
@dataclass
class PhaseTarget:
position: np.ndarray
velocity: np.ndarray
TargetFn = Callable[[float], np.ndarray | PhaseTarget]
@dataclass
class PositionCommand:
"""Output of the position / outer-loop controller."""
thrust: float
desired_quaternion: np.ndarray
desired_angular_rate: np.ndarray = field(
default_factory=lambda: np.zeros(3, dtype=np.float64)
)
@dataclass
class AttitudeCommand:
"""Output of the attitude / inner-loop controller."""
torque: np.ndarray
+5
View File
@@ -0,0 +1,5 @@
from uavcatch.dynamics.model import QuadDynamics
from uavcatch.dynamics.state import QuadState
from uavcatch.dynamics.wind import WindModel
__all__ = ["QuadDynamics", "QuadState", "WindModel"]
+146
View File
@@ -0,0 +1,146 @@
"""Explicit 6DOF quadcopter dynamics."""
from __future__ import annotations
import numpy as np
from uavcatch.config import EnvironmentConfig, UavConfig
from uavcatch.dynamics.state import QuadState, normalize_quaternion, quaternion_to_rotation_matrix
from uavcatch.dynamics.wind import WindModel
class QuadDynamics:
"""Rigid-body quadcopter model: state X + rotor speeds U -> next state."""
def __init__(self, env_cfg: EnvironmentConfig, uav_cfg: UavConfig) -> None:
self.env_cfg = env_cfg
self.uav_cfg = uav_cfg
self._mass = uav_cfg.body.mass
self._arm = uav_cfg.body.arm_length
self._kf = uav_cfg.rotor.kf
self._km = uav_cfg.rotor.km
self._layout = uav_cfg.rotor.layout
self._gravity = env_cfg.physics.gravity
self._inertia = np.diag(
[uav_cfg.body.Ixx, uav_cfg.body.Iyy, uav_cfg.body.Izz]
).astype(np.float64)
self._inertia_inv = np.linalg.inv(self._inertia)
self._linear_drag = uav_cfg.drag.linear
self._angular_drag = uav_cfg.drag.angular
# Quadratic aerodynamic drag coefficient: 0.5 * rho * Cd * A
self._quad_drag = 0.5 * env_cfg.physics.air_density * uav_cfg.drag.cd * uav_cfg.drag.area
self._wind = WindModel.from_config(env_cfg.wind)
self._integrator = env_cfg.simulation.integrator
def clip_omega(self, omega: np.ndarray) -> np.ndarray:
omega = np.asarray(omega, dtype=np.float64)
return np.clip(omega, self.uav_cfg.rotor.omega_min, self.uav_cfg.rotor.omega_max)
def hover_omega(self) -> float:
"""Per-rotor angular speed for approximate hover."""
thrust_per_rotor = self._mass * self._gravity / 4.0
return float(np.sqrt(max(thrust_per_rotor / self._kf, 0.0)))
def rotor_forces_and_torques(self, omega: np.ndarray) -> tuple[float, np.ndarray]:
"""Map rotor speeds to total thrust and body torques."""
omega = self.clip_omega(omega)
thrusts = self._kf * omega * omega
total_thrust = float(np.sum(thrusts))
w1, w2, w3, w4 = omega
if self._layout == "x":
tau_x = self._kf * self._arm * (w4 * w4 - w2 * w2)
tau_y = self._kf * self._arm * (w3 * w3 - w1 * w1)
else:
tau_x = self._kf * self._arm * (w3 * w3 - w1 * w1)
tau_y = self._kf * self._arm * (w4 * w4 - w2 * w2)
tau_z = self._km * (w1 * w1 - w2 * w2 + w3 * w3 - w4 * w4)
return total_thrust, np.array([tau_x, tau_y, tau_z], dtype=np.float64)
def aerodynamic_force(self, velocity: np.ndarray, wind_velocity: np.ndarray) -> np.ndarray:
"""Drag force (world frame) on the air-relative velocity.
F = -linear * v_rel - 0.5 * rho * Cd * A * |v_rel| * v_rel
"""
v_rel = velocity - wind_velocity
force = np.zeros(3, dtype=np.float64)
if self._linear_drag > 0.0:
force -= self._linear_drag * v_rel
if self._quad_drag > 0.0:
speed = float(np.linalg.norm(v_rel))
if speed > 0.0:
force -= self._quad_drag * speed * v_rel
return force
def derivatives(
self,
state: QuadState,
omega: np.ndarray,
wind_velocity: np.ndarray | None = None,
) -> np.ndarray:
"""Compute time derivative of the 13-D state vector."""
thrust, torque = self.rotor_forces_and_torques(omega)
q = normalize_quaternion(state.quaternion)
R = quaternion_to_rotation_matrix(q)
body_thrust = np.array([0.0, 0.0, thrust], dtype=np.float64)
gravity = np.array([0.0, 0.0, -self._gravity], dtype=np.float64)
if wind_velocity is None:
wind_velocity = np.zeros(3, dtype=np.float64)
drag_force = self.aerodynamic_force(state.velocity, wind_velocity)
accel = (R @ body_thrust) / self._mass + gravity + drag_force / self._mass
p_dot = state.velocity
v_dot = accel
w, x, y, z = q
p, q_rate, r = state.angular_rate
q_dot = 0.5 * np.array(
[
-x * p - y * q_rate - z * r,
w * p + y * r - z * q_rate,
w * q_rate - x * r + z * p,
w * r + x * q_rate - y * p,
],
dtype=np.float64,
)
omega_body = state.angular_rate
gyro = np.cross(omega_body, self._inertia @ omega_body)
omega_dot = self._inertia_inv @ (torque - gyro)
if self._angular_drag > 0.0:
omega_dot -= self._angular_drag * omega_body
return np.concatenate([p_dot, v_dot, q_dot, omega_dot])
def step(self, state: QuadState, omega: np.ndarray, dt: float, t: float = 0.0) -> QuadState:
# Wind is held constant across the step (and RK4 substeps); dt is small.
wind = self._wind.sample(t)
if self._integrator == "euler":
return self._step_euler(state, omega, dt, wind)
return self._step_rk4(state, omega, dt, wind)
def _step_euler(
self, state: QuadState, omega: np.ndarray, dt: float, wind: np.ndarray
) -> QuadState:
x = state.as_vector()
dx = self.derivatives(state, omega, wind)
x_next = x + dt * dx
x_next[6:10] = normalize_quaternion(x_next[6:10])
return QuadState.from_vector(x_next)
def _step_rk4(
self, state: QuadState, omega: np.ndarray, dt: float, wind: np.ndarray
) -> QuadState:
x = state.as_vector()
def f(x_vec: np.ndarray) -> np.ndarray:
return self.derivatives(QuadState.from_vector(x_vec), omega, wind)
k1 = f(x)
k2 = f(x + 0.5 * dt * k1)
k3 = f(x + 0.5 * dt * k2)
k4 = f(x + dt * k3)
x_next = x + (dt / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4)
x_next[6:10] = normalize_quaternion(x_next[6:10])
return QuadState.from_vector(x_next)
+110
View File
@@ -0,0 +1,110 @@
"""Quadcopter state representation."""
from __future__ import annotations
from dataclasses import dataclass
import numpy as np
def euler_to_quaternion(roll: float, pitch: float, yaw: float) -> np.ndarray:
"""Convert ZYX Euler angles to quaternion [w, x, y, z]."""
cr, sr = np.cos(roll * 0.5), np.sin(roll * 0.5)
cp, sp = np.cos(pitch * 0.5), np.sin(pitch * 0.5)
cy, sy = np.cos(yaw * 0.5), np.sin(yaw * 0.5)
w = cr * cp * cy + sr * sp * sy
x = sr * cp * cy - cr * sp * sy
y = cr * sp * cy + sr * cp * sy
z = cr * cp * sy - sr * sp * cy
q = np.array([w, x, y, z], dtype=np.float64)
return q / np.linalg.norm(q)
def quaternion_to_rotation_matrix(q: np.ndarray) -> np.ndarray:
"""Body-to-world rotation matrix from unit quaternion [w, x, y, z]."""
w, x, y, z = q
return np.array(
[
[1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
[2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
[2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
],
dtype=np.float64,
)
def quaternion_to_euler(q: np.ndarray) -> tuple[float, float, float]:
"""Return roll, pitch, yaw (rad) from quaternion [w, x, y, z]."""
w, x, y, z = q
sinr_cosp = 2.0 * (w * x + y * z)
cosr_cosp = 1.0 - 2.0 * (x * x + y * y)
roll = float(np.arctan2(sinr_cosp, cosr_cosp))
sinp = 2.0 * (w * y - z * x)
pitch = float(np.arcsin(np.clip(sinp, -1.0, 1.0)))
siny_cosp = 2.0 * (w * z + x * y)
cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
yaw = float(np.arctan2(siny_cosp, cosy_cosp))
return roll, pitch, yaw
def normalize_quaternion(q: np.ndarray) -> np.ndarray:
norm = np.linalg.norm(q)
if norm < 1e-12:
return np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64)
return q / norm
@dataclass
class QuadState:
"""13-DOF quadcopter state in world frame."""
position: np.ndarray
velocity: np.ndarray
quaternion: np.ndarray
angular_rate: np.ndarray
def as_vector(self) -> np.ndarray:
return np.concatenate(
[
self.position,
self.velocity,
self.quaternion,
self.angular_rate,
]
)
def copy(self) -> QuadState:
return QuadState(
position=self.position.copy(),
velocity=self.velocity.copy(),
quaternion=self.quaternion.copy(),
angular_rate=self.angular_rate.copy(),
)
@classmethod
def from_vector(cls, x: np.ndarray) -> QuadState:
x = np.asarray(x, dtype=np.float64)
return cls(
position=x[0:3].copy(),
velocity=x[3:6].copy(),
quaternion=normalize_quaternion(x[6:10].copy()),
angular_rate=x[10:13].copy(),
)
@classmethod
def from_initial(
cls,
position: list[float] | np.ndarray,
velocity: list[float] | np.ndarray,
attitude_euler: list[float] | np.ndarray,
angular_rate: list[float] | np.ndarray,
) -> QuadState:
euler = np.asarray(attitude_euler, dtype=np.float64)
return cls(
position=np.asarray(position, dtype=np.float64),
velocity=np.asarray(velocity, dtype=np.float64),
quaternion=euler_to_quaternion(float(euler[0]), float(euler[1]), float(euler[2])),
angular_rate=np.asarray(angular_rate, dtype=np.float64),
)
+52
View File
@@ -0,0 +1,52 @@
"""Ambient wind field model."""
from __future__ import annotations
import math
from dataclasses import dataclass
import numpy as np
from uavcatch.config import WindConfig
@dataclass
class WindModel:
"""Deterministic wind: constant base plus a sinusoidal gust.
wind(t) = base + gust_amplitude * sin(2*pi*gust_frequency*t) * direction
where direction is the unit base-wind vector (falls back to +X if base ~0).
"""
base_velocity: np.ndarray
gust_amplitude: float = 0.0
gust_frequency: float = 0.0
@classmethod
def from_config(cls, cfg: WindConfig) -> WindModel:
return cls(
base_velocity=np.asarray(cfg.velocity, dtype=np.float64),
gust_amplitude=float(cfg.gust_amplitude),
gust_frequency=float(cfg.gust_frequency),
)
def __post_init__(self) -> None:
self.base_velocity = np.asarray(self.base_velocity, dtype=np.float64)
norm = float(np.linalg.norm(self.base_velocity))
if norm > 1e-9:
self._gust_dir = self.base_velocity / norm
else:
self._gust_dir = np.array([1.0, 0.0, 0.0], dtype=np.float64)
@property
def is_active(self) -> bool:
return (
float(np.linalg.norm(self.base_velocity)) > 0.0
or (self.gust_amplitude != 0.0 and self.gust_frequency != 0.0)
)
def sample(self, t: float) -> np.ndarray:
if self.gust_amplitude == 0.0 or self.gust_frequency == 0.0:
return self.base_velocity
gust = self.gust_amplitude * math.sin(2.0 * math.pi * self.gust_frequency * t)
return self.base_velocity + gust * self._gust_dir
+18
View File
@@ -0,0 +1,18 @@
"""Control evaluation utilities."""
from __future__ import annotations
from uavcatch.eval.maneuvers import ManeuverSpec, default_maneuver_suite
from uavcatch.eval.metrics import ManeuverMetrics, evaluate_maneuver
from uavcatch.eval.plots import plot_maneuver_report, plot_summary_comparison
from uavcatch.eval.recorder import SimRecorder
__all__ = [
"ManeuverSpec",
"ManeuverMetrics",
"SimRecorder",
"default_maneuver_suite",
"evaluate_maneuver",
"plot_maneuver_report",
"plot_summary_comparison",
]
Binary file not shown.
+175
View File
@@ -0,0 +1,175 @@
"""Maneuver trajectories for control evaluation."""
from __future__ import annotations
from dataclasses import dataclass
import numpy as np
from uavcatch.config import EvalConfig
from uavcatch.control.pid import (
circle_trajectory,
circle_trajectory_with_takeoff,
phased_takeoff_hover_forward,
)
from uavcatch.control.types import PhaseTarget, TargetFn
@dataclass(frozen=True)
class ManeuverSpec:
name: str
description: str
duration: float
target_fn: TargetFn
yaw_fn: TargetFn | None = None
accel_fn: TargetFn | None = None
initial_position: tuple[float, float, float] | None = None
step_axis: int | None = None
step_time: float | None = None
step_from: float | None = None
step_to: float | None = None
def _hold(position: np.ndarray) -> TargetFn:
pos = np.asarray(position, dtype=np.float64)
def target_at(t: float) -> PhaseTarget:
del t
return PhaseTarget(position=pos.copy(), velocity=np.zeros(3, dtype=np.float64))
return target_at
def step_position(
initial: np.ndarray,
final: np.ndarray,
step_time: float,
duration: float,
axis: int,
) -> ManeuverSpec:
"""Step change on one position axis after an initial hold."""
initial = np.asarray(initial, dtype=np.float64)
final = np.asarray(final, dtype=np.float64)
axis_names = ["x", "y", "z"]
def target_at(t: float) -> PhaseTarget:
pos = initial.copy() if t < step_time else final.copy()
vel = np.zeros(3, dtype=np.float64)
return PhaseTarget(position=pos, velocity=vel)
return ManeuverSpec(
name=f"step_{axis_names[axis]}",
description=f"Step {axis_names[axis]}: {initial[axis]:.2f} -> {final[axis]:.2f} m at t={step_time:.1f}s",
duration=duration,
target_fn=target_at,
step_axis=axis,
step_time=step_time,
step_from=float(initial[axis]),
step_to=float(final[axis]),
)
def hover_hold(height: float = 2.0, duration: float = 8.0) -> ManeuverSpec:
return ManeuverSpec(
name="hover",
description=f"Hover at z={height:.1f} m",
duration=duration,
target_fn=_hold(np.array([0.0, 0.0, height])),
)
def takeoff_forward(
ground_z: float = 0.0,
hover_z: float = 2.0,
forward_x: float = 4.0,
takeoff_time: float = 3.0,
hover_time: float = 2.0,
forward_time: float = 6.0,
) -> ManeuverSpec:
duration = takeoff_time + hover_time + forward_time
return ManeuverSpec(
name="takeoff_forward",
description="Takeoff -> hover -> forward +X",
duration=duration,
target_fn=phased_takeoff_hover_forward(
ground_z, hover_z, forward_x, takeoff_time, hover_time, forward_time
),
)
def circle_track(
radius: float = 2.5,
height: float = 2.0,
period: float = 10.0,
duration: float = 20.0,
takeoff_time: float = 2.0,
ground_z: float = 0.0,
ground_clearance: float = 0.05,
) -> ManeuverSpec:
pos_fn, yaw_fn, accel_fn = circle_trajectory_with_takeoff(
center=np.zeros(3),
radius=radius,
height=height,
period=period,
takeoff_time=takeoff_time,
ground_z=ground_z,
ground_clearance=ground_clearance,
)
return ManeuverSpec(
name="circle",
description=(
f"Takeoff {takeoff_time:.1f}s from ground, then circle r={radius:.1f} m, T={period:.1f} s"
),
duration=duration,
target_fn=pos_fn,
yaw_fn=yaw_fn,
accel_fn=accel_fn,
initial_position=(0.0, 0.0, ground_z + ground_clearance),
)
def default_maneuver_suite(eval_cfg: EvalConfig) -> list[ManeuverSpec]:
home = np.array([0.0, 0.0, eval_cfg.initial_height])
step = eval_cfg.step
takeoff = eval_cfg.takeoff_forward
circle = eval_cfg.circle
return [
hover_hold(height=eval_cfg.initial_height, duration=eval_cfg.hover.duration),
step_position(
home,
np.array([0.0, 0.0, step.step_z]),
step_time=step.step_time,
duration=step.duration,
axis=2,
),
step_position(
home,
np.array([step.step_x, 0.0, eval_cfg.initial_height]),
step_time=step.step_time,
duration=step.duration,
axis=0,
),
step_position(
home,
np.array([0.0, step.step_y, eval_cfg.initial_height]),
step_time=step.step_time,
duration=step.duration,
axis=1,
),
takeoff_forward(
ground_z=takeoff.ground_z,
hover_z=takeoff.hover_z,
forward_x=takeoff.forward_x,
takeoff_time=takeoff.takeoff_time,
hover_time=takeoff.hover_time,
forward_time=takeoff.forward_time,
),
circle_track(
radius=circle.radius,
height=circle.height,
period=circle.period,
duration=circle.duration,
takeoff_time=circle.takeoff_time,
ground_z=circle.ground_z,
),
]
+208
View File
@@ -0,0 +1,208 @@
"""Control performance metrics."""
from __future__ import annotations
from dataclasses import dataclass, field
import numpy as np
from uavcatch.eval.maneuvers import ManeuverSpec
@dataclass
class StepMetrics:
axis: str
rise_time: float | None
settling_time: float | None
overshoot_pct: float
steady_state_error: float
peak_value: float
final_value: float
@dataclass
class TrackingMetrics:
rmse: np.ndarray
mae: np.ndarray
max_error: np.ndarray
rmse_3d: float
max_error_3d: float
@dataclass
class ControlEffortMetrics:
omega_mean: np.ndarray
omega_std: np.ndarray
omega_max: np.ndarray
saturation_ratio: float
@dataclass
class ManeuverMetrics:
name: str
description: str
mode: str
tracking: TrackingMetrics
attitude_max_deg: np.ndarray
control: ControlEffortMetrics
step: StepMetrics | None = None
summary: dict[str, float] = field(default_factory=dict)
_AXIS_NAMES = ("x", "y", "z")
def _step_response_metrics(
time: np.ndarray,
signal: np.ndarray,
step_time: float,
y0: float,
yf: float,
settle_band: float = 0.02,
) -> StepMetrics:
axis_idx = 0
mask = time >= step_time
t = time[mask] - step_time
y = signal[mask]
if len(t) < 2:
return StepMetrics(
axis=_AXIS_NAMES[axis_idx],
rise_time=None,
settling_time=None,
overshoot_pct=0.0,
steady_state_error=0.0,
peak_value=float(y[-1]) if len(y) else yf,
final_value=yf,
)
delta = yf - y0
if abs(delta) < 1e-9:
return StepMetrics(
axis="",
rise_time=0.0,
settling_time=0.0,
overshoot_pct=0.0,
steady_state_error=float(np.mean(y[-max(10, len(y) // 10):] - yf)),
peak_value=float(y[-1]),
final_value=yf,
)
y10 = y0 + 0.1 * delta
y90 = y0 + 0.9 * delta
band = settle_band * abs(delta)
if delta > 0:
rise_idx_10 = np.argmax(y >= y10)
rise_idx_90 = np.argmax(y >= y90)
overshoot = max(0.0, float(np.max(y) - yf))
else:
rise_idx_10 = np.argmax(y <= y10)
rise_idx_90 = np.argmax(y <= y90)
overshoot = max(0.0, float(yf - np.min(y)))
rise_time = float(t[rise_idx_90] - t[rise_idx_10]) if rise_idx_90 > rise_idx_10 else None
settling_time = None
for i in range(len(t)):
if np.all(np.abs(y[i:] - yf) <= band):
settling_time = float(t[i])
break
tail = y[-max(20, len(y) // 10):]
ss_err = float(np.mean(tail - yf))
overshoot_pct = 100.0 * overshoot / abs(delta)
return StepMetrics(
axis="",
rise_time=rise_time,
settling_time=settling_time,
overshoot_pct=overshoot_pct,
steady_state_error=ss_err,
peak_value=float(np.max(y) if delta > 0 else np.min(y)),
final_value=yf,
)
def compute_tracking_metrics(error: np.ndarray) -> TrackingMetrics:
if error.size == 0:
z = np.zeros(3)
return TrackingMetrics(rmse=z, mae=z, max_error=z, rmse_3d=0.0, max_error_3d=0.0)
rmse = np.sqrt(np.mean(error * error, axis=0))
mae = np.mean(np.abs(error), axis=0)
max_error = np.max(np.abs(error), axis=0)
err_norm = np.linalg.norm(error, axis=1)
return TrackingMetrics(
rmse=rmse,
mae=mae,
max_error=max_error,
rmse_3d=float(np.sqrt(np.mean(err_norm * err_norm))),
max_error_3d=float(np.max(err_norm)),
)
def compute_control_metrics(omega: np.ndarray, omega_max: float) -> ControlEffortMetrics:
if omega.size == 0:
z4 = np.zeros(4)
return ControlEffortMetrics(omega_mean=z4, omega_std=z4, omega_max=z4, saturation_ratio=0.0)
sat = float(np.mean(np.any(omega >= 0.99 * omega_max, axis=1)))
return ControlEffortMetrics(
omega_mean=np.mean(omega, axis=0),
omega_std=np.std(omega, axis=0),
omega_max=np.max(omega, axis=0),
saturation_ratio=sat,
)
def evaluate_maneuver(
spec: ManeuverSpec,
data: dict[str, np.ndarray],
omega_max: float,
mode: str,
) -> ManeuverMetrics:
time = data["time"]
pos = data["position"]
err = data["tracking_error"]
euler = data["euler"]
omega = data["omega"]
tracking = compute_tracking_metrics(err)
control = compute_control_metrics(omega, omega_max)
att_max = np.degrees(np.max(np.abs(euler), axis=0))
step_metrics = None
if spec.step_axis is not None and spec.step_time is not None:
step_metrics = _step_response_metrics(
time,
pos[:, spec.step_axis],
spec.step_time,
spec.step_from or 0.0,
spec.step_to or 0.0,
)
step_metrics.axis = _AXIS_NAMES[spec.step_axis]
summary = {
"rmse_3d": tracking.rmse_3d,
"max_err_3d": tracking.max_error_3d,
"saturation_ratio": control.saturation_ratio,
"max_roll_deg": float(att_max[0]),
"max_pitch_deg": float(att_max[1]),
"max_yaw_deg": float(att_max[2]),
}
if step_metrics is not None:
if step_metrics.rise_time is not None:
summary["rise_time"] = step_metrics.rise_time
if step_metrics.settling_time is not None:
summary["settling_time"] = step_metrics.settling_time
summary["overshoot_pct"] = step_metrics.overshoot_pct
summary["steady_state_error"] = abs(step_metrics.steady_state_error)
return ManeuverMetrics(
name=spec.name,
description=spec.description,
mode=mode,
tracking=tracking,
attitude_max_deg=att_max,
control=control,
step=step_metrics,
summary=summary,
)
+126
View File
@@ -0,0 +1,126 @@
"""Time-domain plots for control evaluation."""
from __future__ import annotations
from pathlib import Path
import matplotlib.pyplot as plt
import numpy as np
from uavcatch.eval.metrics import ManeuverMetrics
def plot_maneuver_report(
data: dict[str, np.ndarray],
metrics: ManeuverMetrics,
save_path: Path | None = None,
show: bool = False,
) -> plt.Figure:
time = data["time"]
pos = data["position"]
target = data["target_position"]
err = data["tracking_error"]
euler = np.degrees(data["euler"])
omega = data["omega"]
fig = plt.figure(figsize=(12, 10))
fig.suptitle(f"{metrics.name} [{metrics.mode}] - {metrics.description}", fontsize=11)
gs = fig.add_gridspec(4, 3, hspace=0.35, wspace=0.28)
labels = ("x", "y", "z")
colors = ("#2563eb", "#16a34a", "#dc2626")
ax_pos = fig.add_subplot(gs[0, :])
for i, (lab, c) in enumerate(zip(labels, colors)):
ax_pos.plot(time, target[:, i], "--", color=c, alpha=0.55, linewidth=1.2, label=f"{lab} ref")
ax_pos.plot(time, pos[:, i], "-", color=c, linewidth=1.6, label=f"{lab} act")
ax_pos.set_ylabel("Position (m)")
ax_pos.legend(loc="upper right", ncol=3, fontsize=8)
ax_pos.grid(alpha=0.3)
ax_pos.set_xlabel("Time (s)")
for i, (lab, c) in enumerate(zip(labels, colors)):
ax = fig.add_subplot(gs[1, i])
ax.plot(time, err[:, i], color=c, linewidth=1.4)
ax.axhline(0.0, color="k", linewidth=0.6, alpha=0.4)
ax.set_title(f"Error {lab} RMSE={metrics.tracking.rmse[i]:.3f} m")
ax.set_ylabel("Error (m)")
ax.set_xlabel("Time (s)")
ax.grid(alpha=0.3)
ax_att = fig.add_subplot(gs[2, 0])
att_labels = ("roll", "pitch", "yaw")
for i, (lab, c) in enumerate(zip(att_labels, colors)):
ax_att.plot(time, euler[:, i], color=c, linewidth=1.2, label=lab)
ax_att.set_ylabel("Attitude (deg)")
ax_att.set_xlabel("Time (s)")
ax_att.legend(fontsize=8)
ax_att.grid(alpha=0.3)
ax_om = fig.add_subplot(gs[2, 1:])
for i in range(omega.shape[1]):
ax_om.plot(time, omega[:, i], linewidth=1.0, alpha=0.85, label=f"m{i + 1}")
ax_om.set_ylabel("Motor ω (rad/s)")
ax_om.set_xlabel("Time (s)")
ax_om.legend(fontsize=7, ncol=2)
ax_om.grid(alpha=0.3)
ax_txt = fig.add_subplot(gs[3, :])
ax_txt.axis("off")
lines = [
f"3D RMSE = {metrics.tracking.rmse_3d:.4f} m | Max error = {metrics.tracking.max_error_3d:.4f} m",
f"RMSE (x,y,z) = ({metrics.tracking.rmse[0]:.4f}, {metrics.tracking.rmse[1]:.4f}, {metrics.tracking.rmse[2]:.4f}) m",
f"Max |roll,pitch,yaw| = ({metrics.attitude_max_deg[0]:.1f}, {metrics.attitude_max_deg[1]:.1f}, {metrics.attitude_max_deg[2]:.1f}) deg",
f"Motor saturation ratio = {metrics.control.saturation_ratio * 100:.1f}%",
]
if metrics.step is not None:
s = metrics.step
rise = f"{s.rise_time:.3f}s" if s.rise_time is not None else "N/A"
settle = f"{s.settling_time:.3f}s" if s.settling_time is not None else "N/A"
lines.append(
f"Step {s.axis}: rise={rise}, settle={settle}, overshoot={s.overshoot_pct:.1f}%, ss_err={s.steady_state_error:.4f} m"
)
ax_txt.text(0.02, 0.6, "\n".join(lines), fontsize=10, family="monospace", va="top")
if save_path is not None:
save_path.parent.mkdir(parents=True, exist_ok=True)
fig.savefig(save_path, dpi=140, bbox_inches="tight")
if show:
plt.show()
return fig
def plot_summary_comparison(
all_metrics: list[ManeuverMetrics],
save_path: Path | None = None,
show: bool = False,
) -> plt.Figure:
names = [m.name for m in all_metrics]
rmse = [m.tracking.rmse_3d for m in all_metrics]
max_err = [m.tracking.max_error_3d for m in all_metrics]
sat = [m.control.saturation_ratio * 100.0 for m in all_metrics]
x = np.arange(len(names))
fig, axes = plt.subplots(1, 3, figsize=(12, 4))
fig.suptitle("Control Performance Summary", fontsize=12)
axes[0].bar(x, rmse, color="#2563eb")
axes[0].set_title("3D RMSE (m)")
axes[1].bar(x, max_err, color="#dc2626")
axes[1].set_title("Max 3D Error (m)")
axes[2].bar(x, sat, color="#ca8a04")
axes[2].set_title("Saturation (%)")
for ax in axes:
ax.set_xticks(x)
ax.set_xticklabels(names, rotation=35, ha="right", fontsize=8)
ax.grid(axis="y", alpha=0.3)
fig.tight_layout()
if save_path is not None:
save_path.parent.mkdir(parents=True, exist_ok=True)
fig.savefig(save_path, dpi=140, bbox_inches="tight")
if show:
plt.show()
return fig
+72
View File
@@ -0,0 +1,72 @@
"""Simulation data recorder."""
from __future__ import annotations
from dataclasses import dataclass, field
import numpy as np
from uavcatch.control.types import PhaseTarget, TargetFn
from uavcatch.dynamics.state import QuadState, quaternion_to_euler
@dataclass
class SimRecorder:
"""Collect time-series during SimEngine.run."""
time: list[float] = field(default_factory=list)
position: list[np.ndarray] = field(default_factory=list)
velocity: list[np.ndarray] = field(default_factory=list)
target_position: list[np.ndarray] = field(default_factory=list)
target_velocity: list[np.ndarray] = field(default_factory=list)
euler: list[np.ndarray] = field(default_factory=list)
omega: list[np.ndarray] = field(default_factory=list)
tracking_error: list[np.ndarray] = field(default_factory=list)
def reset(self) -> None:
self.time.clear()
self.position.clear()
self.velocity.clear()
self.target_position.clear()
self.target_velocity.clear()
self.euler.clear()
self.omega.clear()
self.tracking_error.clear()
def record(
self,
t: float,
state: QuadState,
target_pos: np.ndarray,
target_vel: np.ndarray,
omega: np.ndarray,
) -> None:
self.time.append(float(t))
self.position.append(state.position.copy())
self.velocity.append(state.velocity.copy())
self.target_position.append(np.asarray(target_pos, dtype=np.float64).copy())
self.target_velocity.append(np.asarray(target_vel, dtype=np.float64).copy())
roll, pitch, yaw = quaternion_to_euler(state.quaternion)
self.euler.append(np.array([roll, pitch, yaw], dtype=np.float64))
self.omega.append(np.asarray(omega, dtype=np.float64).copy())
self.tracking_error.append(target_pos - state.position)
@staticmethod
def resolve_target(target_fn: TargetFn, t: float) -> tuple[np.ndarray, np.ndarray]:
target = target_fn(t)
if isinstance(target, PhaseTarget):
return target.position.copy(), target.velocity.copy()
pos = np.asarray(target, dtype=np.float64)
return pos, np.zeros(3, dtype=np.float64)
def as_arrays(self) -> dict[str, np.ndarray]:
return {
"time": np.asarray(self.time, dtype=np.float64),
"position": np.vstack(self.position) if self.position else np.empty((0, 3)),
"velocity": np.vstack(self.velocity) if self.velocity else np.empty((0, 3)),
"target_position": np.vstack(self.target_position) if self.target_position else np.empty((0, 3)),
"target_velocity": np.vstack(self.target_velocity) if self.target_velocity else np.empty((0, 3)),
"euler": np.vstack(self.euler) if self.euler else np.empty((0, 3)),
"omega": np.vstack(self.omega) if self.omega else np.empty((0, 4)),
"tracking_error": np.vstack(self.tracking_error) if self.tracking_error else np.empty((0, 3)),
}
+4
View File
@@ -0,0 +1,4 @@
from uavcatch.sim.engine import SimEngine
from uavcatch.sim.mujoco_viz import MuJoCoViewer
__all__ = ["SimEngine", "MuJoCoViewer"]
Binary file not shown.
+90
View File
@@ -0,0 +1,90 @@
"""Fixed-step quadcopter simulation engine."""
from __future__ import annotations
import logging
from collections.abc import Callable
from dataclasses import dataclass, field
import numpy as np
from uavcatch.config import EnvironmentConfig, UavConfig
from uavcatch.dynamics.model import QuadDynamics
from uavcatch.dynamics.state import QuadState
logger = logging.getLogger(__name__)
ControllerFn = Callable[[QuadState, float], np.ndarray]
@dataclass
class SimEngine:
env_cfg: EnvironmentConfig
uav_cfg: UavConfig
dynamics: QuadDynamics = field(init=False)
state: QuadState = field(init=False)
time: float = 0.0
_out_of_bounds_warned: bool = field(default=False, init=False)
def __post_init__(self) -> None:
self.dynamics = QuadDynamics(self.env_cfg, self.uav_cfg)
self.state = self._initial_state()
def _initial_state(self) -> QuadState:
init = self.uav_cfg.initial
return QuadState.from_initial(
position=init.position,
velocity=init.velocity,
attitude_euler=init.attitude_euler,
angular_rate=init.angular_rate,
)
def reset(self) -> QuadState:
self.time = 0.0
self._out_of_bounds_warned = False
self.state = self._initial_state()
return self.state.copy()
def step(self, omega: np.ndarray) -> QuadState:
dt = self.env_cfg.simulation.dt
self.state = self.dynamics.step(self.state, omega, dt, self.time)
self.time += dt
self._check_bounds()
return self.state.copy()
def run(
self,
controller: ControllerFn,
duration: float | None = None,
on_step: Callable[[QuadState, float, np.ndarray], None] | None = None,
) -> list[QuadState]:
if duration is None:
duration = self.env_cfg.simulation.duration
dt = self.env_cfg.simulation.dt
steps = int(np.ceil(duration / dt))
history: list[QuadState] = []
for _ in range(steps):
omega = controller(self.state, self.time)
omega = self.dynamics.clip_omega(np.asarray(omega, dtype=np.float64))
self.step(omega)
history.append(self.state.copy())
if on_step is not None:
on_step(self.state, self.time, omega)
return history
def _check_bounds(self) -> None:
scene = self.env_cfg.scene
half_x = scene.size_x * 0.5
half_y = scene.size_y * 0.5
p = self.state.position
out = (
abs(p[0]) > half_x
or abs(p[1]) > half_y
or p[2] < scene.ground_z
or p[2] > scene.size_z
)
if out and not self._out_of_bounds_warned:
logger.warning("UAV left scene bounds at t=%.3f, position=%s", self.time, p)
self._out_of_bounds_warned = True
+162
View File
@@ -0,0 +1,162 @@
"""Procedural MuJoCo scene and quadcopter model generation."""
from __future__ import annotations
import math
from typing import TYPE_CHECKING
if TYPE_CHECKING:
from uavcatch.config import EnvironmentConfig, UavConfig
def _motor_xy(layout: str, arm_length: float) -> list[tuple[float, float]]:
"""Return motor hub XY offsets matching QuadDynamics indexing."""
a = arm_length
if layout == "plus":
return [(0.0, a), (-a, 0.0), (0.0, -a), (a, 0.0)]
return [(a, a), (-a, a), (-a, -a), (a, -a)]
def build_quad_body_xml(uav_cfg: UavConfig) -> str:
"""Build detailed quadcopter MJCF body subtree (visual only, negligible mass)."""
arm = uav_cfg.body.arm_length
layout = uav_cfg.rotor.layout
motors = _motor_xy(layout, arm)
prop_radius = max(0.06, arm * 0.38)
hub_radius = max(0.012, arm * 0.06)
hub_half_h = 0.012
arm_radius = max(0.005, arm * 0.024)
fuselage_r = max(0.035, arm * 0.16)
deck_z = 0.018
hub_z = deck_z + hub_half_h
leg_drop = 0.05
parts: list[str] = [
'<body name="drone" pos="0 0 1">',
' <freejoint name="drone_free"/>',
' <inertial pos="0 0 0" mass="0.001" diaginertia="1e-6 1e-6 1e-6"/>',
f' <geom name="fuselage_lower" type="ellipsoid" size="{fuselage_r:.5g} {fuselage_r:.5g} {deck_z:.5g}" '
'pos="0 0 0" rgba="0.18 0.22 0.28 1" contype="0" conaffinity="0"/>',
f' <geom name="fuselage_deck" type="cylinder" size="{fuselage_r * 0.82:.5g} {0.006:.5g}" '
f'pos="0 0 {deck_z:.5g}" rgba="0.25 0.30 0.36 1" contype="0" conaffinity="0"/>',
' <geom name="nose_mark" type="box" size="0.012 0.008 0.004" '
f'pos="{fuselage_r * 0.75:.5g} 0 {deck_z + 0.004:.5g}" rgba="0.95 0.35 0.15 1" contype="0" conaffinity="0"/>',
' <geom name="tail_mark" type="box" size="0.008 0.008 0.004" '
f'pos="{-fuselage_r * 0.65:.5g} 0 {deck_z + 0.004:.5g}" rgba="0.15 0.75 0.95 1" contype="0" conaffinity="0"/>',
]
for idx, (mx, my) in enumerate(motors, start=1):
dist = math.hypot(mx, my)
if dist < 1e-9:
continue
arm_start_z = deck_z * 0.5
parts.append(
f' <geom name="arm{idx}" type="capsule" '
f'fromto="0 0 {arm_start_z:.5g} {mx:.5g} {my:.5g} {deck_z:.5g}" '
f'size="{arm_radius:.5g}" rgba="0.42 0.44 0.48 1" contype="0" conaffinity="0"/>'
)
parts.append(
f' <geom name="arm_tip{idx}" type="sphere" size="{arm_radius * 1.6:.5g}" '
f'pos="{mx:.5g} {my:.5g} {deck_z:.5g}" rgba="0.30 0.32 0.36 1" contype="0" conaffinity="0"/>'
)
parts.append(f' <body name="rotor{idx}" pos="{mx:.5g} {my:.5g} {hub_z:.5g}">')
parts.append(
f' <joint name="rotor{idx}_hinge" type="hinge" axis="0 0 1" damping="0" '
'limited="false" armature="0"/>'
)
parts.append(
f' <geom name="motor_hub{idx}" type="cylinder" size="{hub_radius:.5g} {hub_half_h:.5g}" '
'rgba="0.12 0.12 0.14 1" contype="0" conaffinity="0"/>'
)
blade_len = prop_radius
blade_w = max(0.012, prop_radius * 0.11)
blade_t = 0.0025
for sign, suffix in ((1.0, "a"), (-1.0, "b")):
parts.append(
f' <geom name="blade{idx}_{suffix}" type="box" '
f'size="{blade_len:.5g} {blade_w:.5g} {blade_t:.5g}" '
f'pos="{sign * blade_len:.5g} 0 0" rgba="0.08 0.08 0.10 0.92" contype="0" conaffinity="0"/>'
)
parts.append(
f' <geom name="blade{idx}_cross_a" type="box" '
f'size="{blade_w:.5g} {blade_len:.5g} {blade_t:.5g}" '
f'pos="0 {blade_len:.5g} 0" rgba="0.10 0.10 0.12 0.88" contype="0" conaffinity="0"/>'
)
parts.append(
f' <geom name="blade{idx}_cross_b" type="box" '
f'size="{blade_w:.5g} {blade_len:.5g} {blade_t:.5g}" '
f'pos="0 {-blade_len:.5g} 0" rgba="0.10 0.10 0.12 0.88" contype="0" conaffinity="0"/>'
)
parts.append(" </body>")
for idx, (mx, my) in enumerate(motors, start=1):
dist = math.hypot(mx, my)
if dist < 1e-9:
continue
sx = mx * 0.55
sy = my * 0.55
ex = mx * 0.92
ey = my * 0.92
parts.append(
f' <geom name="leg{idx}" type="capsule" '
f'fromto="{sx:.5g} {sy:.5g} 0 {ex:.5g} {ey:.5g} {-leg_drop:.5g}" '
f'size="{arm_radius * 0.9:.5g}" rgba="0.20 0.20 0.22 1" contype="0" conaffinity="0"/>'
)
parts.append(
f' <geom name="foot{idx}" type="capsule" '
f'fromto="{ex - 0.03 * mx / dist:.5g} {ey - 0.03 * my / dist:.5g} {-leg_drop:.5g} '
f'{ex + 0.03 * mx / dist:.5g} {ey + 0.03 * my / dist:.5g} {-leg_drop:.5g}" '
f'size="{arm_radius:.5g}" rgba="0.16 0.16 0.18 1" contype="0" conaffinity="0"/>'
)
parts.append("</body>")
return "\n".join(parts)
def build_scene_mjcf(env_cfg: EnvironmentConfig, uav_cfg: UavConfig) -> str:
"""Build full MuJoCo scene XML string."""
scene = env_cfg.scene
half_x = scene.size_x * 0.5
half_y = scene.size_y * 0.5
ground = max(scene.ground_z, 0.0)
quad_body = build_quad_body_xml(uav_cfg)
return f"""<mujoco model="uavcatch_scene">
<compiler angle="radian" autolimits="true" coordinate="local"/>
<option timestep="{env_cfg.simulation.dt}" gravity="0 0 {-env_cfg.physics.gravity}"/>
<visual>
<headlight diffuse="0.85 0.85 0.85" specular="0.2 0.2 0.2"/>
<rgba haze="0.12 0.20 0.30 1"/>
<global azimuth="135" elevation="-22"/>
<quality shadowsize="4096"/>
</visual>
<asset>
<texture type="skybox" builtin="gradient" rgb1="0.45 0.63 0.92" rgb2="0.08 0.12 0.20" width="800" height="800"/>
<texture name="grid" type="2d" builtin="checker" rgb1="0.22 0.26 0.30" rgb2="0.34 0.38 0.42" width="512" height="512"/>
<material name="ground" texture="grid" texrepeat="10 10" reflectance="0.12" specular="0.2" shininess="0.2"/>
<material name="boundary" rgba="0.92 0.55 0.12 0.85" specular="0.3"/>
</asset>
<worldbody>
<light pos="0 0 12" dir="0 0 -1" diffuse="0.95 0.95 0.95" specular="0.15 0.15 0.15"/>
<light pos="6 6 8" dir="-1 -1 -1" diffuse="0.35 0.35 0.40" specular="0.05 0.05 0.05"/>
<geom name="ground" type="plane" size="{max(half_x, half_y):.5g} {max(half_x, half_y):.5g} 0.1" pos="0 0 {ground:.5g}" material="ground"/>
<geom name="corner1" type="sphere" size="0.06" pos="{half_x:.5g} {half_y:.5g} {ground:.5g}" material="boundary" contype="0" conaffinity="0"/>
<geom name="corner2" type="sphere" size="0.06" pos="{-half_x:.5g} {half_y:.5g} {ground:.5g}" material="boundary" contype="0" conaffinity="0"/>
<geom name="corner3" type="sphere" size="0.06" pos="{-half_x:.5g} {-half_y:.5g} {ground:.5g}" material="boundary" contype="0" conaffinity="0"/>
<geom name="corner4" type="sphere" size="0.06" pos="{half_x:.5g} {-half_y:.5g} {ground:.5g}" material="boundary" contype="0" conaffinity="0"/>
<geom name="height_marker" type="sphere" size="0.06" pos="0 0 {scene.size_z:.5g}" material="boundary" contype="0" conaffinity="0"/>
<geom name="edge_x1" type="box" size="{half_x:.5g} 0.015 0.015" pos="0 {half_y:.5g} {ground + 0.02:.5g}" material="boundary" contype="0" conaffinity="0"/>
<geom name="edge_x2" type="box" size="{half_x:.5g} 0.015 0.015" pos="0 {-half_y:.5g} {ground + 0.02:.5g}" material="boundary" contype="0" conaffinity="0"/>
<geom name="edge_y1" type="box" size="0.015 {half_y:.5g} 0.015" pos="{half_x:.5g} 0 {ground + 0.02:.5g}" material="boundary" contype="0" conaffinity="0"/>
<geom name="edge_y2" type="box" size="0.015 {half_y:.5g} 0.015" pos="{-half_x:.5g} 0 {ground + 0.02:.5g}" material="boundary" contype="0" conaffinity="0"/>
{quad_body}
</worldbody>
</mujoco>
"""
+142
View File
@@ -0,0 +1,142 @@
"""MuJoCo passive viewer for quadcopter visualization."""
from __future__ import annotations
import time
from pathlib import Path
import mujoco
import mujoco.viewer
import numpy as np
from uavcatch.config import EnvironmentConfig, UavConfig
from uavcatch.dynamics.state import QuadState
from uavcatch.sim.mujoco_model import build_scene_mjcf
_ASSETS_DIR = Path(__file__).resolve().parents[3] / "assets" / "mujoco"
class MuJoCoViewer:
"""Sync explicit dynamics state to MuJoCo scene (render only, no physics)."""
def __init__(
self,
env_cfg: EnvironmentConfig,
uav_cfg: UavConfig | None = None,
scene_path: Path | None = None,
) -> None:
self.env_cfg = env_cfg
self.uav_cfg = uav_cfg
if scene_path is not None:
if not scene_path.is_file():
raise FileNotFoundError(f"MuJoCo scene not found: {scene_path}")
self.model = mujoco.MjModel.from_xml_path(str(scene_path))
else:
if uav_cfg is None:
raise ValueError("uav_cfg is required when scene_path is not provided")
mjcf = build_scene_mjcf(env_cfg, uav_cfg)
self.model = mujoco.MjModel.from_xml_string(mjcf)
self.data = mujoco.MjData(self.model)
self._viewer = None
self._rotor_joint_ids = [
mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, f"rotor{i}_hinge")
for i in range(1, 5)
]
self._rotor_angles = np.zeros(4, dtype=np.float64)
# Real-time rendering bookkeeping
self._sim_clock = 0.0
self._next_frame_sim = 0.0
self._wall_anchor: float | None = None
def __enter__(self) -> MuJoCoViewer:
self._viewer = mujoco.viewer.launch_passive(self.model, self.data)
self._setup_camera()
self._sim_clock = 0.0
self._next_frame_sim = 0.0
self._wall_anchor = None
return self
def __exit__(self, exc_type, exc, tb) -> None:
if self._viewer is not None:
self._viewer.close()
self._viewer = None
def _setup_camera(self) -> None:
if self._viewer is None:
return
dist = self.env_cfg.viewer.camera_distance
self._viewer.cam.distance = dist
self._viewer.cam.elevation = -22.0
self._viewer.cam.azimuth = 140.0
init_z = 1.0
if self.uav_cfg is not None:
init_z = float(self.uav_cfg.initial.position[2])
self._viewer.cam.lookat[:] = [0.0, 0.0, init_z]
def _write_state(self, state: QuadState) -> None:
"""Push pose and accumulated rotor angles into MuJoCo qpos."""
self.data.qpos[0:3] = state.position
self.data.qpos[3:7] = state.quaternion
for i in range(4):
joint_adr = self.model.jnt_qposadr[self._rotor_joint_ids[i]]
self.data.qpos[joint_adr] = self._rotor_angles[i]
mujoco.mj_forward(self.model, self.data)
def sync(self, state: QuadState, omega: np.ndarray | None = None, dt: float = 0.002) -> None:
"""Immediately render one frame (no frame skipping / throttling)."""
if omega is not None:
self._rotor_angles += np.asarray(omega, dtype=np.float64) * dt
self._write_state(state)
if self._viewer is not None:
self._viewer.sync()
def render_realtime(
self,
state: QuadState,
omega: np.ndarray | None,
sim_dt: float,
speed: float = 1.0,
rotor_visual_scale: float = 1.0,
) -> bool:
"""Advance one physics step; render + throttle only on frame boundaries.
Decouples physics rate from display rate: the rotor angle integrates every
physics step (so spin speed is physically correct), but we only draw at
`viewer.fps` and sleep to keep wall-clock time aligned with sim time scaled
by `speed`. Returns True when a frame was actually drawn.
`rotor_visual_scale` only affects the visual spin (useful to tame the
stroboscopic / wagon-wheel aliasing of a very fast rotor); set < 1 to slow
the displayed blades without changing the physics.
"""
if omega is not None:
self._rotor_angles += np.asarray(omega, dtype=np.float64) * sim_dt * rotor_visual_scale
self._sim_clock += sim_dt
frame_interval = 1.0 / max(self.env_cfg.viewer.fps, 1)
if self._sim_clock + 1e-12 < self._next_frame_sim:
return False
self._next_frame_sim += frame_interval
if self._next_frame_sim < self._sim_clock:
# Fell behind (e.g. heavy step); resync schedule to avoid a render burst.
self._next_frame_sim = self._sim_clock + frame_interval
self._write_state(state)
if self._viewer is not None:
self._viewer.sync()
now = time.perf_counter()
if self._wall_anchor is None:
self._wall_anchor = now
target = self._wall_anchor + self._sim_clock / max(speed, 1e-9)
delay = target - time.perf_counter()
if delay > 0.0:
time.sleep(delay)
return True
def is_running(self) -> bool:
return self._viewer is not None and self._viewer.is_running()
def sleep_for_fps(self) -> None:
fps = max(self.env_cfg.viewer.fps, 1)
time.sleep(1.0 / fps)
+105
View File
@@ -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
+183
View File
@@ -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)
+72
View File
@@ -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
+28
View File
@@ -0,0 +1,28 @@
[body]
mass = 1.3
arm_length = 0.23
Ixx = 0.0075
Iyy = 0.0075
Izz = 0.013
[rotor]
# T_i = kf * omega_i^2; 悬停标定: kf = m*g / (4 * omega_hover^2), omega_hover ≈ 550 rad/s
kf = 1.054e-5
km = 2.64e-7
omega_min = 0.0
omega_max = 838.0
layout = "x"
[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]
[drag]
# 平动阻力(世界系): F = -linear * v_rel - 0.5 * rho * cd * area * |v_rel| * v_rel
# v_rel = 机体速度 - 风速; rho = environment.toml 的 air_density
linear = 0.0 # 线性阻力系数 N/(m/s),低速近似,0 则只用二次项
angular = 0.05 # 角速度阻尼 N·m/(rad/s)
cd = 0.8 # 气动阻力系数
area = 0.1 # 等效迎风面积 m^2