Files
uav-catch/examples/eval_control.py
T
2026-07-03 04:19:44 +08:00

321 lines
5.0 KiB
Python

#!/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())