321 lines
5.0 KiB
Python
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())
|
|
|