120 lines
3.9 KiB
Python
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
|