Flight

simweave.flight simulates multirotor flight dynamics: a quadrotor model, a cascaded position/attitude controller, reference trajectories and missions (take off, figure-8, land), wind and fault injection. It needs only numpy. See FLIGHT_DESIGN.md in the repository for the full plan.

Fly a mission

from simweave.flight import Mission, Quadrotor, ConstantWind, fly

mission = Mission.figure8(height=3.0, a=4.0, b=2.0, period=20.0)
flight = fly(Quadrotor(wind=ConstantWind((4.0, 0.0, 0.0))), mission)

print(flight.metrics())       # rms_error, max_error, energy_wh, touchdown_speed, ...
print(flight.phase_rms())     # tracking error per phase
data = flight.to_dict()       # time, x, y, z, roll, pitch, yaw, ... (Z up, radians)
viewer = flight.to_viewer()   # same, mapped for a Y-up 3D viewer

Mission.figure8 chains take off, hover, a quintic blend into the figure-8, the figure-8 laps, a blend out, hover and a soft landing. Position, velocity and acceleration are continuous through every join. Build your own from Hold, Quintic (minimum-jerk move between two RefPoint states) and Figure8, or pass any callable ref(t) -> RefPoint.

fly uses a CascadedController: position P, velocity PI, thrust-vector and attitude (SO(3) error), body-rate PI, then the mixer. It models the vehicle with vehicle.params, so changing vehicle.mass afterwards (a payload) or a motor_eff_* value tests it against model mismatch; the integral terms absorb it. Tune with ControllerGains. The motors switch off once the reference and the vehicle are on the ground.

Run demos/27_quadcopter_figure8.py for a complete example.

Plots

from simweave.viz import plot_trajectory_3d, plot_flight_summary

plot_trajectory_3d(flight).show()    # flown path coloured by speed, plus the reference
plot_flight_summary(flight).show()   # error, tilt, rotor thrusts and power, phases shaded

plot_landing_dispersion(points) scatters (x, y) landing points with circles containing 50% and 95% of them. Plotly is imported lazily, so the rest of simweave.flight works without the viz extra.

Monte Carlo and faults

fly is a plain function of a vehicle and a mission, so it drops straight into run_monte_carlo. Demo 28 randomises steady wind, seeded turbulence and an unknown payload and summarises tracking error, touchdown speed and landing dispersion. Define the scenario at module level so the process pool can pickle it.

fly also accepts a FaultInjector wrapping the quadrotor. Demo 29 degrades one rotor during the figure-8: the integral terms push that rotor harder to compensate until it reaches its thrust limit, which for the default vehicle is a loss of about 58% (1 - hover_thrust / max_thrust). Past that, the vehicle can no longer hold altitude and attitude together.

Conventions

• World frame is Z up; body frame is x forward, y left, z up.
• Euler angles are ZYX. Positive roll banks right, positive pitch is nose down, positive yaw is counter-clockwise seen from above.
• Rotors are numbered 1 front-right, 2 front-left, 3 rear-left, 4 rear-right.

Quick example

import numpy as np
from simweave.continuous import simulate
from simweave.flight import Quadrotor, Mixer

quad = Quadrotor(hover=True, position=(0, 0, 5))
mixer = Mixer(quad.params)


class Hover:
    def update(self, t, state, dt):
        return mixer.allocate(quad.params.weight, (0.0, 0.0, 0.0))


result = simulate(quad, (0, 5), dt=0.005, controller=Hover(), control_dt=0.01)
print(result.state[-1, :3])   # still at (0, 0, 5)

The quadrotor has 16 states (x y z vx vy vz roll pitch yaw p q r f1 f2 f3 f4, where f1..f4 are the actual rotor thrusts, which follow the commanded thrusts with time constant motor_tau). Its input is the four commanded rotor thrusts. Mixer.allocate(collective, torques) converts a collective thrust and body torques into rotor thrusts within limits, giving attitude torques priority over collective thrust when the request does not fit.

Disturbances and faults

• wind= takes ConstantWind, GustWind (smooth "1 - cos" gust) or TurbulentWind (seeded Gauss-Markov turbulence). Drag acts on the velocity relative to the air.
• motor_eff_1 .. motor_eff_4 scale each rotor's thrust, so a FaultInjector with a ParameterFault can degrade a single rotor.
• Quadrotor.power(thrusts) gives rotor power by momentum theory, for energy estimates.

Ground contact

The floor at z = 0 stops downward motion. While the rotors cannot lift the vehicle it stays upright and still, so a landed vehicle does not tumble.

API

Flight dynamics: quadrotor physics, control allocation and wind models.

Importing this package needs only numpy. Conventions (Z-up world, body x forward / y left / z up, ZYX Euler angles) are described in :mod:simweave.flight.frames.

CascadedController

CascadedController(reference: Callable[[float], RefPoint], params: QuadrotorParams | None = None, gains: ControllerGains | None = None, disarm_height: float = 0.03)

Tracks a reference ref(t) -> RefPoint (a trajectory or a mission).

Parameters:

Name	Type	Description	Default
reference	Callable[[float], RefPoint]	Callable returning the desired :class:RefPoint at time t.	required
params	QuadrotorParams | None	The controller's model of the vehicle.	None
gains	ControllerGains | None	:class:ControllerGains; defaults suit the default vehicle.	None
disarm_height	float	When both the reference and the vehicle are at or below this height the motors are switched off (as a real flight controller does after landing) and the integrators are reset.	0.03

Source code in src/simweave/flight/control.py

def __init__(
    self,
    reference: Callable[[float], RefPoint],
    params: QuadrotorParams | None = None,
    gains: ControllerGains | None = None,
    disarm_height: float = 0.03,
) -> None:
    self.reference = reference
    self.params = params if params is not None else QuadrotorParams()
    self.gains = gains if gains is not None else ControllerGains()
    self.mixer = Mixer(self.params)
    self.disarm_height = float(disarm_height)
    self.armed = False
    g = self.gains
    a_xy, a_up, a_dn = g.max_accel_xy, g.max_accel_up, g.max_accel_down
    self._vel_pid = [
        PIDController(g.kp_vel_xy, g.ki_vel_xy, output_limits=(-a_xy, a_xy)),
        PIDController(g.kp_vel_xy, g.ki_vel_xy, output_limits=(-a_xy, a_xy)),
        PIDController(g.kp_vel_z, g.ki_vel_z, output_limits=(-a_dn, a_up)),
    ]
    lim = g.max_angular_accel
    self._rate_pid = [
        PIDController(g.kp_rate_rp, g.ki_rate, output_limits=(-lim, lim)),
        PIDController(g.kp_rate_rp, g.ki_rate, output_limits=(-lim, lim)),
        PIDController(g.kp_rate_yaw, g.ki_rate, output_limits=(-lim, lim)),
    ]
    self._inertia = np.array(self.params.inertia)

ControllerGains dataclass

ControllerGains(kp_pos_xy: float = 1.5, kp_pos_z: float = 2.0, kp_vel_xy: float = 3.5, ki_vel_xy: float = 0.8, kp_vel_z: float = 5.0, ki_vel_z: float = 1.5, kp_att: float = 8.0, kp_yaw: float = 3.0, kp_rate_rp: float = 20.0, kp_rate_yaw: float = 10.0, ki_rate: float = 2.0, max_tilt: float = radians(35.0), max_speed_xy: float = 8.0, max_speed_up: float = 3.0, max_speed_down: float = 2.0, max_accel_xy: float = 6.0, max_accel_up: float = 6.0, max_accel_down: float = 5.0, max_angular_accel: float = 300.0)

Gains and limits for :class:CascadedController.

Rate-loop gains act on angular acceleration (rad/s^2 per rad/s), so they do not need rescaling when the inertia changes.

Mixer

Mixer(params: QuadrotorParams | None = None)

Maps collective thrust and body torques to four rotor thrusts.

wrench = A @ thrusts with wrench = [T, tau_x, tau_y, tau_z].

Source code in src/simweave/flight/mixer.py

def __init__(self, params: QuadrotorParams | None = None) -> None:
    self.params = params if params is not None else QuadrotorParams()
    d = self.params.arm_length / math.sqrt(2.0)
    k = self.params.torque_ratio
    self.allocation = np.array(
        [
            [1.0, 1.0, 1.0, 1.0],
            [-d, d, d, -d],
            [-d, -d, d, d],
            [-k, k, -k, k],
        ]
    )
    self._inverse = np.linalg.inv(self.allocation)

wrench

wrench(thrusts) -> ndarray

[T, tau_x, tau_y, tau_z] produced by four rotor thrusts.

Source code in src/simweave/flight/mixer.py

def wrench(self, thrusts) -> np.ndarray:
    """``[T, tau_x, tau_y, tau_z]`` produced by four rotor thrusts."""
    return self.allocation @ np.asarray(thrusts, dtype=float)

thrusts

thrusts(collective: float, torques) -> ndarray

Unconstrained rotor thrusts for a requested wrench.

Source code in src/simweave/flight/mixer.py

def thrusts(self, collective: float, torques) -> np.ndarray:
    """Unconstrained rotor thrusts for a requested wrench."""
    w = np.array([collective, *np.asarray(torques, dtype=float)])
    return self._inverse @ w

allocate

allocate(collective: float, torques) -> ndarray

Rotor thrusts within [0, max_thrust] for a requested wrench.

When the request does not fit, attitude torques take priority over collective thrust: the collective is moved first (keeping the torques exact), and only if the torques alone cannot be produced are they scaled down uniformly.

Source code in src/simweave/flight/mixer.py

def allocate(self, collective: float, torques) -> np.ndarray:
    """Rotor thrusts within ``[0, max_thrust]`` for a requested wrench.

    When the request does not fit, attitude torques take priority over
    collective thrust: the collective is moved first (keeping the torques
    exact), and only if the torques alone cannot be produced are they
    scaled down uniformly.
    """
    f_max = self.params.max_thrust
    base = self._inverse[:, 0] * collective  # collective only: T/4 each
    spread = self._inverse[:, 1:] @ np.asarray(torques, dtype=float)
    # Shift every rotor by the same amount c (collective change) so that
    # base_i + c + s * spread_i stays inside [0, f_max].
    base_level = float(base[0])
    lo = -float(spread.min())
    hi = f_max - float(spread.max())
    scale = 1.0
    if lo <= hi:
        level = min(max(base_level, lo), hi)
    else:
        span = float(spread.max() - spread.min())
        scale = f_max / span
        level = -scale * float(spread.min())
    return np.clip(level + scale * spread, 0.0, f_max)

QuadrotorParams dataclass

QuadrotorParams(mass: float = 1.2, inertia: tuple[float, float, float] = (0.012, 0.012, 0.022), arm_length: float = 0.17, max_thrust: float = 7.0, torque_ratio: float = 0.016, motor_tau: float = 0.03, drag_lin: float = 0.1, drag_quad: float = 0.0, gravity: float = 9.81, air_density: float = 1.225, rotor_radius: float = 0.12, figure_of_merit: float = 0.65)

Rigid-body and propulsion parameters (SI units; unit objects accepted).

The defaults describe a roughly 1.2 kg sport quadrotor with a thrust-to-weight ratio of about 2.4. They are plausible, not a model of any particular airframe.

Attributes:

Name	Type	Description
mass	float	Total mass, kg.
inertia	tuple[float, float, float]	Principal moments (Ixx, Iyy, Izz), kg m^2 (body axes, diagonal).
arm_length	float	Distance from the centre of mass to each rotor, m. Rotors sit at 45 degrees to the body axes ("X" layout).
max_thrust	float	Maximum thrust of one rotor, N.
torque_ratio	float	Rotor reaction torque per unit thrust (yaw), m.
motor_tau	float	First-order rotor response time constant, s. Must be positive.
drag_lin, drag_quad		Linear (N s/m) and quadratic (N s^2/m^2) drag coefficients on the velocity relative to the air.
gravity	float	Gravitational acceleration, m/s^2.
air_density	float	Air density, kg/m^3 (used by the energy model).
rotor_radius	float	Propeller radius, m (used by the energy model).
figure_of_merit	float	Hover efficiency of the rotors (ideal power / actual power).

hover_thrust property

hover_thrust: float

Thrust per rotor needed to hover, N.

Quadrotor

Quadrotor(params: QuadrotorParams | None = None, position=(0.0, 0.0, 0.0), yaw: float = 0.0, hover: bool = False, wind=None, motor_eff=(1.0, 1.0, 1.0, 1.0))

Bases: DynamicSystem

Quadrotor in "X" layout. See the module docstring for the state layout.

Parameters:

Name	Type	Description	Default
params	QuadrotorParams | None	Physical parameters; defaults to :class:QuadrotorParams.	None
position		Initial position (m) and heading (rad).	(0.0, 0.0, 0.0)
yaw		Initial position (m) and heading (rad).	(0.0, 0.0, 0.0)
hover	bool	If true, start with all rotors already producing hover thrust; otherwise the rotors start at zero thrust (vehicle on the ground).	False
wind		Optional :class:~simweave.flight.wind.Wind callable.	None
motor_eff		Per-rotor thrust effectiveness (1.0 = healthy). These are exposed as the scalar attributes motor_eff_1 .. motor_eff_4 so a :class:~simweave.faults.FaultInjector can degrade one rotor.	(1.0, 1.0, 1.0, 1.0)

Notes

The commanded rotor thrusts are supplied through simulate(..., controller=...) or inputs=; with no input the rotors are commanded to zero thrust.

Source code in src/simweave/flight/quadrotor.py

def __init__(
    self,
    params: QuadrotorParams | None = None,
    position=(0.0, 0.0, 0.0),
    yaw: float = 0.0,
    hover: bool = False,
    wind=None,
    motor_eff=(1.0, 1.0, 1.0, 1.0),
) -> None:
    self.params = params if params is not None else QuadrotorParams()
    p = self.params
    # Plain attributes (not read through ``params``) so fault injection
    # and parameter sweeps can perturb them on the instance.
    self.mass = p.mass
    self.Ixx, self.Iyy, self.Izz = p.inertia
    self.arm_length = p.arm_length
    self.max_thrust = p.max_thrust
    self.torque_ratio = p.torque_ratio
    self.motor_tau = p.motor_tau
    self.drag_lin = p.drag_lin
    self.drag_quad = p.drag_quad
    self.gravity = p.gravity
    self.wind = wind
    eff = tuple(float(e) for e in motor_eff)
    if len(eff) != 4:
        raise ValueError("motor_eff needs four values.")
    self.motor_eff_1, self.motor_eff_2, self.motor_eff_3, self.motor_eff_4 = eff

    pos = np.asarray(position, dtype=float)
    if pos.shape != (3,):
        raise ValueError("position must have three components.")
    self._x0 = self.hover_state(pos, yaw) if hover else self._ground_state(pos, yaw)

hover_thrust property

hover_thrust: float

Thrust per rotor that balances the current weight, N.

hover_state

hover_state(position=(0.0, 0.0, 0.0), yaw: float = 0.0) -> ndarray

State at rest with every rotor already producing hover thrust.

Source code in src/simweave/flight/quadrotor.py

def hover_state(self, position=(0.0, 0.0, 0.0), yaw: float = 0.0) -> np.ndarray:
    """State at rest with every rotor already producing hover thrust."""
    x0 = self._ground_state(np.asarray(position, dtype=float), yaw)
    x0[12:16] = self.mass * self.gravity / 4.0
    return x0

post_step

post_step(state: ndarray) -> ndarray

Ground contact: the floor at z = 0 stops downward motion, and a vehicle that cannot lift off stays upright and still.

Source code in src/simweave/flight/quadrotor.py

def post_step(self, state: np.ndarray) -> np.ndarray:
    """Ground contact: the floor at ``z = 0`` stops downward motion, and a
    vehicle that cannot lift off stays upright and still."""
    if state[2] > 1e-9:
        return state
    s = state.copy()
    s[2] = 0.0
    if s[5] < 0.0:
        s[5] = 0.0
    thrust = (
        s[12] * self.motor_eff_1 + s[13] * self.motor_eff_2
        + s[14] * self.motor_eff_3 + s[15] * self.motor_eff_4
    )
    if thrust < self.mass * self.gravity:
        s[3] = s[4] = 0.0
        s[6] = s[7] = 0.0
        s[9:12] = 0.0
    return s

power

power(thrusts) -> ndarray

Electrical rotor power (W) for rotor thrusts, by momentum theory: P = f^1.5 / (sqrt(2 rho A) * figure_of_merit) summed over rotors.

Accepts (..., 4) and returns the total over the last axis.

Source code in src/simweave/flight/quadrotor.py

def power(self, thrusts) -> np.ndarray:
    """Electrical rotor power (W) for rotor thrusts, by momentum theory:
    ``P = f^1.5 / (sqrt(2 rho A) * figure_of_merit)`` summed over rotors.

    Accepts ``(..., 4)`` and returns the total over the last axis.
    """
    p = self.params
    f = np.maximum(np.asarray(thrusts, dtype=float), 0.0)
    per_rotor = f**1.5 / (math.sqrt(2.0 * p.air_density * p.disk_area) * p.figure_of_merit)
    return per_rotor.sum(axis=-1)

FlightResult dataclass

FlightResult(result: SimulationResult, reference: Callable[[float], RefPoint], vehicle: Quadrotor)

A simulated flight plus the reference it was flying.

reference is the callable the controller tracked (a :class:~simweave.flight.trajectory.Mission, usually).

euler property

euler: ndarray

(roll, pitch, yaw) in radians, one row per sample.

thrusts property

thrusts: ndarray

Actual rotor thrusts (N), one column per rotor.

commands property

commands: ndarray

Commanded rotor thrusts (N) held during each step.

to_dict

to_dict() -> dict[str, ndarray]

Columns time, x, y, z, roll, pitch, yaw first (angles in radians, Z up), then velocity, body rates and rotor thrusts.

Source code in src/simweave/flight/results.py

def to_dict(self) -> dict[str, np.ndarray]:
    """Columns ``time, x, y, z, roll, pitch, yaw`` first (angles in
    radians, Z up), then velocity, body rates and rotor thrusts."""
    s = self.result.state
    data = {"time": self.time}
    for i, name in enumerate(_COLUMNS[1:]):
        data[name] = s[:, i]
    return data

to_frame

to_frame()

Same columns as :meth:to_dict as a pandas DataFrame.

Source code in src/simweave/flight/results.py

def to_frame(self):
    """Same columns as :meth:`to_dict` as a pandas DataFrame."""
    import pandas as pd  # optional dependency

    return pd.DataFrame(self.to_dict())

to_viewer

to_viewer() -> dict[str, ndarray]

Columns for the Y-up 3D viewer (model faces +Z, Euler order YXZ).

Viewer (X, Y, Z) = (y, z, x) of this package's Z-up frame, which keeps the frame right-handed. The three angles keep their values: roll about the forward axis, pitch (positive nose down) about the left axis, yaw about the up axis. Angles are radians.

Source code in src/simweave/flight/results.py

def to_viewer(self) -> dict[str, np.ndarray]:
    """Columns for the Y-up 3D viewer (model faces +Z, Euler order YXZ).

    Viewer ``(X, Y, Z) = (y, z, x)`` of this package's Z-up frame, which
    keeps the frame right-handed. The three angles keep their values:
    roll about the forward axis, pitch (positive nose down) about the
    left axis, yaw about the up axis. Angles are radians.
    """
    s = self.result.state
    return {
        "time": self.time,
        "x": s[:, 1],
        "y": s[:, 2],
        "z": s[:, 0],
        "roll": s[:, 6],
        "pitch": s[:, 7],
        "yaw": s[:, 8],
    }

tracking_error

tracking_error() -> ndarray

Distance (m) between the vehicle and the reference at each sample.

Source code in src/simweave/flight/results.py

def tracking_error(self) -> np.ndarray:
    """Distance (m) between the vehicle and the reference at each sample."""
    return np.linalg.norm(self.position - self.reference_position, axis=1)

tilt

tilt() -> ndarray

Angle (rad) between the body z axis and vertical at each sample.

Source code in src/simweave/flight/results.py

def tilt(self) -> np.ndarray:
    """Angle (rad) between the body z axis and vertical at each sample."""
    roll, pitch = self.euler[:, 0], self.euler[:, 1]
    return np.arccos(np.clip(np.cos(roll) * np.cos(pitch), -1.0, 1.0))

power

power() -> ndarray

Electrical rotor power (W) at each sample.

Source code in src/simweave/flight/results.py

def power(self) -> np.ndarray:
    """Electrical rotor power (W) at each sample."""
    return self.vehicle.power(self.thrusts)

energy_wh

energy_wh() -> float

Rotor energy used over the flight, Wh.

Source code in src/simweave/flight/results.py

def energy_wh(self) -> float:
    """Rotor energy used over the flight, Wh."""
    trapezoid = getattr(np, "trapezoid", None) or np.trapz
    return float(trapezoid(self.power(), self.time) / 3600.0)

saturation_fraction

saturation_fraction() -> float

Fraction of airborne control steps with any rotor command at a limit.

Source code in src/simweave/flight/results.py

def saturation_fraction(self) -> float:
    """Fraction of airborne control steps with any rotor command at a limit."""
    cmd = self.commands
    armed = np.any(cmd > 0.0, axis=1)
    if not armed.any():
        return 0.0
    fmax = self.vehicle.max_thrust
    hit = np.any((cmd[armed] >= fmax - 1e-9) | (cmd[armed] <= 1e-9), axis=1)
    return float(hit.mean())

touchdown

touchdown() -> dict[str, float] | None

Time, vertical speed, horizontal position and miss distance at touchdown.

Touchdown is the first sample at or below 2 cm after the vehicle has been above 0.5 m; None if it never lands.

Source code in src/simweave/flight/results.py

def touchdown(self) -> dict[str, float] | None:
    """Time, vertical speed, horizontal position and miss distance at touchdown.

    Touchdown is the first sample at or below 2 cm after the vehicle has
    been above 0.5 m; ``None`` if it never lands.
    """
    z = self.position[:, 2]
    up = np.flatnonzero(z > 0.5)
    if up.size == 0:
        return None
    down = np.flatnonzero(z[up[0]:] <= 0.02)
    if down.size == 0:
        return None
    i = up[0] + down[0]
    ref_xy = np.asarray(self.reference(self.time[-1]).pos)[0:2]
    return {
        "time": float(self.time[i]),
        "vertical_speed": float(self.velocity[i, 2]),
        "horizontal_error": float(np.linalg.norm(self.position[i, 0:2] - ref_xy)),
        "x": float(self.position[i, 0]),
        "y": float(self.position[i, 1]),
    }

metrics

metrics() -> dict[str, float]

Headline numbers for a flight.

Source code in src/simweave/flight/results.py

def metrics(self) -> dict[str, float]:
    """Headline numbers for a flight."""
    err = self.tracking_error()
    out = {
        "rms_error": float(np.sqrt(np.mean(err**2))),
        "max_error": float(err.max()),
        "max_tilt_deg": float(np.degrees(self.tilt().max())),
        "saturation_fraction": self.saturation_fraction(),
        "energy_wh": self.energy_wh(),
        "duration": float(self.time[-1] - self.time[0]),
    }
    td = self.touchdown()
    if td is not None:
        out["touchdown_speed"] = td["vertical_speed"]
        out["landing_error"] = td["horizontal_error"]
    return out

phase_rms

phase_rms() -> dict[str, float]

RMS tracking error per mission phase (needs a :class:Mission).

Source code in src/simweave/flight/results.py

def phase_rms(self) -> dict[str, float]:
    """RMS tracking error per mission phase (needs a :class:`Mission`)."""
    if not isinstance(self.reference, Mission):
        raise TypeError("phase_rms needs the reference to be a Mission.")
    err = self.tracking_error()
    out: dict[str, list[float]] = {}
    names = np.array([self.reference.phase(t) for t in self.time])
    for name in dict.fromkeys(names):
        out[name] = err[names == name]
    return {str(k): float(np.sqrt(np.mean(v**2))) for k, v in out.items()}

Figure8

Figure8(center=(0.0, 0.0, 3.0), a: float = 4.0, b: float = 2.0, period: float = 20.0, laps: float = 1.0, z_amp: float = 0.0, yaw_mode: str = 'fixed', yaw: float = 0.0, name: str = 'figure8')

A figure-of-eight (lemniscate) flown at constant altitude.

x = a sin(w t), y = b sin(2 w t) about center with w = 2 pi / period; the path starts at the crossing point moving diagonally. An optional vertical oscillation z_amp sin(w t) makes one lobe climb and the other dip.

yaw_mode is "fixed" (hold yaw) or "heading" (point along the direction of travel).

Source code in src/simweave/flight/trajectory.py

def __init__(
    self,
    center=(0.0, 0.0, 3.0),
    a: float = 4.0,
    b: float = 2.0,
    period: float = 20.0,
    laps: float = 1.0,
    z_amp: float = 0.0,
    yaw_mode: str = "fixed",
    yaw: float = 0.0,
    name: str = "figure8",
) -> None:
    if period <= 0 or laps <= 0:
        raise ValueError("period and laps must be positive.")
    if yaw_mode not in ("fixed", "heading"):
        raise ValueError("yaw_mode must be 'fixed' or 'heading'.")
    self.center = _vec3(center, "center")
    self.a, self.b, self.z_amp = float(a), float(b), float(z_amp)
    self.period = float(period)
    self.laps = float(laps)
    self.yaw_mode = yaw_mode
    self.yaw = float(yaw)
    self.duration = self.period * self.laps
    self.name = name
    self._w = 2.0 * math.pi / self.period

Hold

Hold(position, yaw: float = 0.0, duration: float = 1.0, name: str = 'hover')

Stay at a point (zero velocity and acceleration) for duration s.

Source code in src/simweave/flight/trajectory.py

def __init__(self, position, yaw: float = 0.0, duration: float = 1.0, name: str = "hover") -> None:
    if duration <= 0:
        raise ValueError("duration must be positive.")
    self.position = _vec3(position, "position")
    self.yaw = float(yaw)
    self.duration = float(duration)
    self.name = name

Mission

Mission(segments: Sequence)

A chain of segments flown back to back.

mission(t) returns the :class:RefPoint at time t (held at the first/last point outside the mission). phase(t) names the active segment. Use :meth:figure8 for the standard take off, fly a figure-8, land sequence.

Source code in src/simweave/flight/trajectory.py

def __init__(self, segments: Sequence) -> None:
    if not segments:
        raise ValueError("A mission needs at least one segment.")
    self.segments = list(segments)
    durations = np.array([s.duration for s in self.segments], dtype=float)
    self._starts = np.concatenate([[0.0], np.cumsum(durations)])
    self.duration = float(self._starts[-1])

phases property

phases: list[tuple[str, float, float]]

(name, start, end) for each segment.

figure8 classmethod

figure8(height: float = 3.0, a: float = 4.0, b: float = 2.0, period: float = 20.0, laps: float = 1.0, center=(0.0, 0.0), start=(0.0, 0.0), yaw_mode: str = 'fixed', z_amp: float = 0.0, climb_time: float = 4.0, settle_time: float = 2.0, blend_time: float = 3.0, hover_time: float = 2.0, land_time: float = 6.0, land_speed: float = 0.3, ground_time: float = 2.0) -> 'Mission'

Take off at start, fly laps figure-8s about center, land.

The transitions into and out of the figure-8 are quintic blends, so position, velocity and acceleration are continuous throughout. land_speed is the descent rate at touchdown.

Source code in src/simweave/flight/trajectory.py

@classmethod
def figure8(
    cls,
    height: float = 3.0,
    a: float = 4.0,
    b: float = 2.0,
    period: float = 20.0,
    laps: float = 1.0,
    center=(0.0, 0.0),
    start=(0.0, 0.0),
    yaw_mode: str = "fixed",
    z_amp: float = 0.0,
    climb_time: float = 4.0,
    settle_time: float = 2.0,
    blend_time: float = 3.0,
    hover_time: float = 2.0,
    land_time: float = 6.0,
    land_speed: float = 0.3,
    ground_time: float = 2.0,
) -> "Mission":
    """Take off at ``start``, fly ``laps`` figure-8s about ``center``, land.

    The transitions into and out of the figure-8 are quintic blends, so
    position, velocity and acceleration are continuous throughout.
    ``land_speed`` is the descent rate at touchdown.
    """
    if height <= 0:
        raise ValueError("height must be positive.")
    s_xy = np.asarray(start, dtype=float)
    c_xy = np.asarray(center, dtype=float)
    ground = np.array([s_xy[0], s_xy[1], 0.0])
    hover_a = np.array([s_xy[0], s_xy[1], height])
    hover_b = np.array([c_xy[0], c_xy[1], height])
    zero = np.zeros(3)

    eight = Figure8(
        center=hover_b, a=a, b=b, period=period, laps=laps, z_amp=z_amp,
        yaw_mode=yaw_mode, name="figure8",
    )
    entry = eight(0.0)
    exit_ = eight(eight.duration)
    touchdown = RefPoint(
        np.array([c_xy[0], c_xy[1], 0.0]),
        np.array([0.0, 0.0, -abs(land_speed)]),
        zero,
        0.0,
    )
    hover_pt = RefPoint(hover_b, zero, zero, entry.yaw)
    segments = [
        Quintic(RefPoint(ground, zero, zero, 0.0), RefPoint(hover_a, zero, zero, 0.0),
                climb_time, name="takeoff"),
        Hold(hover_a, 0.0, settle_time, name="hover"),
        Quintic(RefPoint(hover_a, zero, zero, 0.0), entry, blend_time, name="transition"),
        eight,
        Quintic(exit_, hover_pt, blend_time, name="transition"),
        Hold(hover_b, entry.yaw, hover_time, name="hover"),
        Quintic(hover_pt, RefPoint(touchdown.pos, touchdown.vel, zero, entry.yaw),
                land_time, name="land"),
        Hold(touchdown.pos, entry.yaw, ground_time, name="landed"),
    ]
    return cls(segments)

Quintic

Quintic(start: RefPoint, end: RefPoint, duration: float, name: str = 'transition')

Minimum-jerk move between two :class:RefPoint states.

Each axis follows the quintic polynomial that meets the position, velocity and acceleration of start and end after duration seconds. Heading changes along a smooth quintic in the shortest direction.

Source code in src/simweave/flight/trajectory.py

def __init__(self, start: RefPoint, end: RefPoint, duration: float, name: str = "transition") -> None:
    if duration <= 0:
        raise ValueError("duration must be positive.")
    T = float(duration)
    p0, v0, a0 = (np.asarray(x, dtype=float) for x in (start.pos, start.vel, start.acc))
    p1, v1, a1 = (np.asarray(x, dtype=float) for x in (end.pos, end.vel, end.acc))
    dp = p1 - p0
    self._c = np.stack(
        [
            p0,
            v0,
            a0 / 2.0,
            (20 * dp - (8 * v1 + 12 * v0) * T - (3 * a0 - a1) * T**2) / (2 * T**3),
            (-30 * dp + (14 * v1 + 16 * v0) * T + (3 * a0 - 2 * a1) * T**2) / (2 * T**4),
            (12 * dp - 6 * (v1 + v0) * T - (a0 - a1) * T**2) / (2 * T**5),
        ]
    )
    self.duration = T
    self.name = name
    self._yaw0 = float(start.yaw)
    self._dyaw = float(wrap_angle(end.yaw - start.yaw))

RefPoint

Bases: NamedTuple

Desired state at one instant (yaw_rate is optional feed-forward).

ConstantWind

ConstantWind(velocity=(0.0, 0.0, 0.0))

Steady wind velocity = (wx, wy, wz).

Source code in src/simweave/flight/wind.py

def __init__(self, velocity=(0.0, 0.0, 0.0)) -> None:
    self.velocity = _vec3(velocity, "velocity")

GustWind

GustWind(amplitude, start: float, duration: float, base=(0.0, 0.0, 0.0))

A steady wind plus one smooth "1 - cos" gust.

The gust adds amplitude * 0.5 * (1 - cos(2 pi (t - start) / duration)) for start <= t <= start + duration.

Source code in src/simweave/flight/wind.py

def __init__(self, amplitude, start: float, duration: float, base=(0.0, 0.0, 0.0)) -> None:
    if duration <= 0:
        raise ValueError("duration must be positive.")
    self.amplitude = _vec3(amplitude, "amplitude")
    self.start = float(start)
    self.duration = float(duration)
    self.base = _vec3(base, "base")

TurbulentWind

TurbulentWind(sigma, correlation_time: float, duration: float, mean=(0.0, 0.0, 0.0), dt: float = 0.01, rng: Generator | None = None)

Steady wind plus first-order Gauss-Markov turbulence.

Each axis is an Ornstein-Uhlenbeck process with standard deviation sigma (scalar or per-axis) and correlation time correlation_time. The realisation is sampled once on a grid of spacing dt covering duration seconds and linearly interpolated, so repeated evaluations at the same time agree. After duration the last value is held.

Source code in src/simweave/flight/wind.py

def __init__(
    self,
    sigma,
    correlation_time: float,
    duration: float,
    mean=(0.0, 0.0, 0.0),
    dt: float = 0.01,
    rng: np.random.Generator | None = None,
) -> None:
    if correlation_time <= 0 or duration <= 0 or dt <= 0:
        raise ValueError("correlation_time, duration and dt must be positive.")
    sigma = np.broadcast_to(np.asarray(sigma, dtype=float), (3,))
    if np.any(sigma < 0):
        raise ValueError("sigma must be non-negative.")
    rng = rng if rng is not None else np.random.default_rng()
    self.mean = _vec3(mean, "mean")
    self.dt = float(dt)
    n = int(math.ceil(duration / dt)) + 1
    a = math.exp(-dt / correlation_time)
    noise = rng.standard_normal((n, 3))
    gust = np.zeros((n, 3))
    gust[0] = sigma * noise[0]
    b = math.sqrt(1.0 - a * a)
    for k in range(1, n):
        gust[k] = a * gust[k - 1] + b * sigma * noise[k]
    self._gust = gust

Wind

Bases: Protocol

Velocity of the air (m/s, world frame) at time t and position.

euler_from_rotation

euler_from_rotation(R: ndarray) -> tuple[float, float, float]

(roll, pitch, yaw) from a body-to-world rotation matrix.

Pitch is returned in [-pi/2, pi/2]. At the gimbal-lock singularity (pitch = +/-90 degrees) yaw is set to zero and roll absorbs the rotation.

Source code in src/simweave/flight/frames.py

def euler_from_rotation(R: np.ndarray) -> tuple[float, float, float]:
    """``(roll, pitch, yaw)`` from a body-to-world rotation matrix.

    Pitch is returned in ``[-pi/2, pi/2]``. At the gimbal-lock singularity
    (pitch = +/-90 degrees) yaw is set to zero and roll absorbs the rotation.
    """
    sp = -float(R[2, 0])
    pitch = math.asin(max(-1.0, min(1.0, sp)))
    if abs(sp) > 1.0 - 1e-12:
        return math.atan2(-R[1, 2], R[1, 1]), pitch, 0.0
    roll = math.atan2(R[2, 1], R[2, 2])
    yaw = math.atan2(R[1, 0], R[0, 0])
    return roll, pitch, yaw

euler_to_quaternion

euler_to_quaternion(roll: float, pitch: float, yaw: float) -> ndarray

Unit quaternion [w, x, y, z] for ZYX Euler angles.

Source code in src/simweave/flight/frames.py

def euler_to_quaternion(roll: float, pitch: float, yaw: float) -> np.ndarray:
    """Unit quaternion ``[w, x, y, z]`` for ZYX Euler angles."""
    cr, sr = math.cos(roll / 2), math.sin(roll / 2)
    cp, sp = math.cos(pitch / 2), math.sin(pitch / 2)
    cy, sy = math.cos(yaw / 2), math.sin(yaw / 2)
    return np.array(
        [
            cr * cp * cy + sr * sp * sy,
            sr * cp * cy - cr * sp * sy,
            cr * sp * cy + sr * cp * sy,
            cr * cp * sy - sr * sp * cy,
        ]
    )

quaternion_to_euler

quaternion_to_euler(q) -> tuple[float, float, float]

ZYX Euler angles (roll, pitch, yaw) from a quaternion [w, x, y, z].

Source code in src/simweave/flight/frames.py

def quaternion_to_euler(q) -> tuple[float, float, float]:
    """ZYX Euler angles ``(roll, pitch, yaw)`` from a quaternion ``[w, x, y, z]``."""
    w, x, y, z = (float(v) for v in np.asarray(q, dtype=float) / np.linalg.norm(q))
    R = 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)],
        ]
    )
    return euler_from_rotation(R)

rotation_matrix

rotation_matrix(roll: float, pitch: float, yaw: float) -> ndarray

Body-to-world rotation matrix for ZYX Euler angles (radians).

Source code in src/simweave/flight/frames.py

def rotation_matrix(roll: float, pitch: float, yaw: float) -> np.ndarray:
    """Body-to-world rotation matrix for ZYX Euler angles (radians)."""
    cr, sr = math.cos(roll), math.sin(roll)
    cp, sp = math.cos(pitch), math.sin(pitch)
    cy, sy = math.cos(yaw), math.sin(yaw)
    return np.array(
        [
            [cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
            [sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr],
            [-sp, cp * sr, cp * cr],
        ]
    )

wrap_angle

wrap_angle(angle)

Wrap an angle (or array of angles) in radians to [-pi, pi).

Source code in src/simweave/flight/frames.py

def wrap_angle(angle):
    """Wrap an angle (or array of angles) in radians to ``[-pi, pi)``."""
    return (np.asarray(angle) + math.pi) % (2.0 * math.pi) - math.pi

fly

fly(vehicle: Quadrotor, mission: Callable[[float], RefPoint], *, gains: ControllerGains | None = None, dt: float = 0.004, control_dt: float = 0.008, t_end: float | None = None, method: str = 'rk4') -> FlightResult

Fly vehicle along mission with a :class:CascadedController.

The controller models the vehicle with vehicle.params, so changing vehicle.mass (a payload) or motor effectiveness afterwards tests the controller against model mismatch. vehicle may also be a wrapper with a .system attribute, such as a :class:~simweave.faults.FaultInjector. t_end defaults to the mission duration.

Source code in src/simweave/flight/results.py

def fly(
    vehicle: Quadrotor,
    mission: Callable[[float], RefPoint],
    *,
    gains: ControllerGains | None = None,
    dt: float = 0.004,
    control_dt: float = 0.008,
    t_end: float | None = None,
    method: str = "rk4",
) -> FlightResult:
    """Fly ``vehicle`` along ``mission`` with a :class:`CascadedController`.

    The controller models the vehicle with ``vehicle.params``, so changing
    ``vehicle.mass`` (a payload) or motor effectiveness afterwards tests the
    controller against model mismatch. ``vehicle`` may also be a wrapper with a
    ``.system`` attribute, such as a :class:`~simweave.faults.FaultInjector`.
    ``t_end`` defaults to the mission duration.
    """
    if t_end is None:
        t_end = getattr(mission, "duration", None)
        if t_end is None:
            raise ValueError("Pass t_end for a reference without a duration.")
    quad = vehicle
    while not isinstance(quad, Quadrotor):  # e.g. a FaultInjector wrapping the quad
        quad = getattr(quad, "system", None)
        if quad is None:
            raise TypeError("fly needs a Quadrotor or a wrapper exposing .system.")
    controller = CascadedController(mission, quad.params, gains)
    result = simulate(
        vehicle, (0.0, float(t_end)), dt=dt, method=method,
        controller=controller, control_dt=control_dt,
    )
    return FlightResult(result=result, reference=mission, vehicle=quad)