init
This commit is contained in:
commit
e901fa9579
72 files changed
+3715
No files matched your search
@@ -0,0 +1,2 @@
|
|||||||
|
# Static snapshot for reference only.
|
||||||
|
# Runtime visualization uses procedural MJCF from uavcatch.sim.mujoco_model.
|
||||||
@@ -0,0 +1,2 @@
|
|||||||
|
# Static snapshot for reference only.
|
||||||
|
# Runtime visualization uses procedural MJCF from uavcatch.sim.mujoco_model.
|
||||||
@@ -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
|
||||||
@@ -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)
|
||||||
@@ -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())
|
||||||
|
|
||||||
@@ -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␍
|
||||||
@@ -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
|
||||||
@@ -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 |
@@ -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"]
|
||||||
@@ -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
|
||||||
@@ -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"
|
||||||
@@ -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 @@
|
|||||||
|
|
||||||
@@ -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
|
||||||
@@ -0,0 +1 @@
|
|||||||
|
uavcatch
|
||||||
@@ -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.
@@ -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))
|
||||||
@@ -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.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -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)
|
||||||
@@ -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))
|
||||||
@@ -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)
|
||||||
@@ -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,
|
||||||
|
)
|
||||||
@@ -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)
|
||||||
@@ -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)
|
||||||
@@ -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)
|
||||||
@@ -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
|
||||||
@@ -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"]
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -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)
|
||||||
@@ -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),
|
||||||
|
)
|
||||||
@@ -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
|
||||||
@@ -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.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -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,
|
||||||
|
),
|
||||||
|
]
|
||||||
@@ -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,
|
||||||
|
)
|
||||||
@@ -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
|
||||||
@@ -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)),
|
||||||
|
}
|
||||||
@@ -0,0 +1,4 @@
|
|||||||
|
from uavcatch.sim.engine import SimEngine
|
||||||
|
from uavcatch.sim.mujoco_viz import MuJoCoViewer
|
||||||
|
|
||||||
|
__all__ = ["SimEngine", "MuJoCoViewer"]
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -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
|
||||||
@@ -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>
|
||||||
|
"""
|
||||||
@@ -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)
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -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
|
||||||
@@ -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)
|
||||||
@@ -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
|
||||||
@@ -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
|
||||||
Reference in new issue
Block a user