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

120 lines
3.9 KiB
Python

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