drones-sim 0.2.0__py3-none-any.whl

drones_sim/control/pid.py

@@ -0,0 +1,50 @@
+"""Generic PID controller with anti-windup and output saturation.
+
+Consolidated from quadcopter_simulation.py.
+"""
+
+from __future__ import annotations
+
+import numpy as np
+
+
+class PIDController:
+    """Scalar PID with integral anti-windup and output clamping."""
+
+    def __init__(
+        self,
+        kp: float,
+        ki: float,
+        kd: float,
+        output_limits: tuple[float, float] | None = None,
+        windup_limits: tuple[float, float] | None = None,
+    ):
+        self.kp = kp
+        self.ki = ki
+        self.kd = kd
+        self.output_limits = output_limits
+        self.windup_limits = windup_limits
+
+        self._prev_error = 0.0
+        self._integral = 0.0
+
+    def reset(self) -> None:
+        self._prev_error = 0.0
+        self._integral = 0.0
+
+    def update(self, setpoint: float, measurement: float, dt: float) -> float:
+        error = setpoint - measurement
+        dt = max(dt, 1e-6)
+
+        self._integral += error * dt
+        if self.windup_limits is not None:
+            self._integral = np.clip(self._integral, *self.windup_limits)
+
+        derivative = (error - self._prev_error) / dt
+        self._prev_error = error
+
+        output = self.kp * error + self.ki * self._integral + self.kd * derivative
+        if self.output_limits is not None:
+            output = np.clip(output, *self.output_limits)
+
+        return float(output)

drones_sim/dynamics/__init__.py

@@ -0,0 +1,13 @@
+from .config import QuadcopterConfig
+from .disturbances import (  # noqa: F401
+    ConstantWind,
+    Disturbance,
+    DrydenGust,
+    GroundEffect,
+    MotorFailure,
+    PayloadDrop,
+    StepWind,
+)
+from .quadcopter import QuadcopterDynamics
+
+__all__ = ["QuadcopterConfig", "QuadcopterDynamics"]

drones_sim/dynamics/config.py

@@ -0,0 +1,117 @@
+"""Physical configuration for the quadcopter plant."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+
+import numpy as np
+from numpy.typing import NDArray
+
+
+@dataclass
+class QuadcopterConfig:
+    """Validated physical and actuator parameters.
+
+    The default rotor positions form a ``+`` layout compatible with the
+    historical model: front, right, rear, left. Positive rotor directions
+    produce positive yaw moments.
+    """
+
+    mass: float = 1.0
+    inertia: NDArray = field(
+        default_factory=lambda: np.diag([0.01, 0.01, 0.018])
+    )
+    arm_length: float = 0.2
+    thrust_coefficient: float = 1.0e-6
+    moment_coefficient: float = 1.0e-7
+    linear_drag: NDArray = field(default_factory=lambda: np.full(3, 0.1))
+    quadratic_drag: NDArray = field(default_factory=lambda: np.zeros(3))
+    gravity: float = 9.81
+    motor_time_constant: float = 0.04
+    min_motor_speed: float = 0.0
+    max_motor_speed: float = 4000.0
+    rotor_directions: NDArray = field(
+        default_factory=lambda: np.array([1.0, -1.0, 1.0, -1.0])
+    )
+
+    def __post_init__(self) -> None:
+        self.mass = float(self.mass)
+        self.arm_length = float(self.arm_length)
+        self.thrust_coefficient = float(self.thrust_coefficient)
+        self.moment_coefficient = float(self.moment_coefficient)
+        self.gravity = float(self.gravity)
+        self.motor_time_constant = float(self.motor_time_constant)
+        self.min_motor_speed = float(self.min_motor_speed)
+        self.max_motor_speed = float(self.max_motor_speed)
+        self.inertia = np.asarray(self.inertia, dtype=float).copy()
+        if self.inertia.shape == (3,):
+            self.inertia = np.diag(self.inertia)
+        self.linear_drag = self._axis_value(self.linear_drag, "linear_drag")
+        self.quadratic_drag = self._axis_value(
+            self.quadratic_drag, "quadratic_drag"
+        )
+        self.rotor_directions = np.asarray(
+            self.rotor_directions, dtype=float
+        ).copy()
+        self.validate()
+
+    @staticmethod
+    def _axis_value(value: NDArray | float, name: str) -> NDArray:
+        array = np.asarray(value, dtype=float)
+        if array.ndim == 0:
+            array = np.full(3, float(array))
+        if array.shape != (3,):
+            raise ValueError(f"{name} must be scalar or shape (3,), got {array.shape}")
+        return array.copy()
+
+    def validate(self) -> None:
+        if self.mass <= 0.0:
+            raise ValueError("mass must be positive")
+        if self.arm_length <= 0.0:
+            raise ValueError("arm_length must be positive")
+        if self.thrust_coefficient <= 0.0 or self.moment_coefficient <= 0.0:
+            raise ValueError("rotor coefficients must be positive")
+        if self.inertia.shape != (3, 3):
+            raise ValueError("inertia must have shape (3, 3)")
+        if not np.allclose(self.inertia, self.inertia.T):
+            raise ValueError("inertia must be symmetric")
+        if np.min(np.linalg.eigvalsh(self.inertia)) <= 0.0:
+            raise ValueError("inertia must be positive definite")
+        if np.any(self.linear_drag < 0.0) or np.any(self.quadratic_drag < 0.0):
+            raise ValueError("drag coefficients must be non-negative")
+        if self.gravity <= 0.0:
+            raise ValueError("gravity must be positive")
+        if self.motor_time_constant < 0.0:
+            raise ValueError("motor_time_constant cannot be negative")
+        if not 0.0 <= self.min_motor_speed < self.max_motor_speed:
+            raise ValueError("motor speed limits are invalid")
+        if self.rotor_directions.shape != (4,):
+            raise ValueError("rotor_directions must have shape (4,)")
+
+    @property
+    def rotor_positions(self) -> NDArray:
+        length = self.arm_length
+        return np.array(
+            [[length, 0.0, 0.0], [0.0, length, 0.0],
+             [-length, 0.0, 0.0], [0.0, -length, 0.0]]
+        )
+
+    def allocation_matrix(self, efficiencies: NDArray | None = None) -> NDArray:
+        """Map squared rotor speeds to ``[thrust, tau_x, tau_y, tau_z]``."""
+        efficiency = (
+            np.ones(4)
+            if efficiencies is None
+            else np.asarray(efficiencies, dtype=float)
+        )
+        if efficiency.shape != (4,):
+            raise ValueError("efficiencies must have shape (4,)")
+        force_gain = self.thrust_coefficient * efficiency
+        positions = self.rotor_positions
+        return np.vstack(
+            [
+                force_gain,
+                positions[:, 1] * force_gain,
+                -positions[:, 0] * force_gain,
+                self.rotor_directions * self.moment_coefficient * efficiency,
+            ]
+        )

drones_sim/dynamics/disturbances.py

@@ -0,0 +1,262 @@
+"""Disturbance models for quadcopter dynamics.
+
+Each disturbance can inject an external force (world frame), an external torque
+(body frame), or modify the physical parameters of the quadcopter (mass, inertia,
+rotor coefficients) at a given simulation time.
+
+All disturbances derive from the abstract ``Disturbance`` base and are wired into
+``QuadcopterDynamics._derivatives`` via the ``disturbances=`` list parameter in the
+constructor (see ``quadcopter.py``).
+
+Reference
+---------
+- Dryden gust model: MIL-F-8785C / U.S. Military Specification on Flying Qualities
+  of Piloted Airplanes (1980).
+- Ground effect: Cheeseman & Bennett (1955), "The Effect of the Ground on a
+  Helicopter Rotor in Forward Flight", ARC R&M 3021.
+"""
+
+from __future__ import annotations
+
+from abc import ABC
+
+import numpy as np
+from numpy.typing import NDArray
+
+
+class Disturbance(ABC):
+    """Abstract base for any external disturbance acting on the quadcopter.
+
+    Subclasses override one or more of the three hooks.  The default
+    implementations are no-ops.
+    """
+
+    def reset(self) -> None:
+        """Re-seed any internal state (called at the start of a new episode)."""
+
+    def advance(self, t: float, dt: float, state: NDArray) -> None:
+        """Advance stochastic state once per integration step.
+
+        This is separate from force evaluation because RK4 evaluates the force
+        model four times per integration step.
+        """
+
+    def external_force(self, t: float, dt: float, state: NDArray) -> NDArray:
+        """External force in the world frame [N].
+
+        Called inside ``_derivatives``; result is added directly to the total
+        force vector before dividing by mass.
+        """
+        return np.zeros(3)
+
+    def external_torque(self, t: float, dt: float, state: NDArray) -> NDArray:
+        """External torque in the body frame [N·m]."""
+        return np.zeros(3)
+
+    def modify_dynamics(self, quad: object, t: float) -> None:
+        """Mutate quadcopter physical attributes in-place (mass, inertia, etc.).
+
+        Called once per ``update()`` call, before ``_derivatives``.
+        The ``quad`` argument is the ``QuadcopterDynamics`` instance.
+        """
+
+
+# ---------------------------------------------------------------------------
+# Wind models
+# ---------------------------------------------------------------------------
+
+class ConstantWind(Disturbance):
+    """Steady world-frame wind force.
+
+    The wind pushes the drone with a force proportional to the wind velocity:
+
+        F = k_d * v_wind
+
+    where *k_d* is matched to the quadcopter's drag coefficient so the
+    simulation remains dimensionally consistent.
+    """
+
+    def __init__(self, velocity: NDArray, k_d: float = 0.1) -> None:
+        self.velocity = np.asarray(velocity, dtype=float)
+        self.k_d = k_d
+
+    def external_force(self, t: float, dt: float, state: NDArray) -> NDArray:
+        # Retained for direct/legacy callers. The plant calls ``advance`` once
+        # and obtains the resulting wind through ``wind_velocity``.
+        self.advance(t, dt, state)
+        return np.zeros(3)
+
+    def wind_velocity(self, t: float = 0.0) -> NDArray:
+        return self.velocity.copy()
+
+
+class StepWind(Disturbance):
+    """Wind that switches on at time *t_on* with a given world-frame velocity."""
+
+    def __init__(self, velocity: NDArray, t_on: float = 0.0) -> None:
+        self.velocity = np.asarray(velocity, dtype=float)
+        self.t_on = t_on
+
+    def external_force(self, t: float, dt: float, state: NDArray) -> NDArray:
+        return np.zeros(3)
+
+    def wind_velocity(self, t: float = 0.0) -> NDArray:
+        return self.velocity.copy() if t >= self.t_on else np.zeros(3)
+
+
+class DrydenGust(Disturbance):
+    """Dryden turbulence model — continuous random gust in the world frame.
+
+    The Dryden spectrum is approximated in the time domain as a first-order
+    Gauss-Markov process with correlation length *L* (m) and intensity *sigma*
+    (m/s).  The filter is:
+
+        d(wind)/dt = -(V / L) * wind + sigma * sqrt(2*V / (pi*L)) * eta(t)
+
+    where *V* is the reference speed (default: hover-induced downwash proxy) and
+    *eta* ~ N(0,1/dt) per step.
+
+    Reference
+    ---------
+    MIL-F-8785C, §3.7.2 "Discrete Gust and Continuous Turbulence Models"
+    """
+
+    def __init__(
+        self,
+        intensity: float = 2.0,       # sigma [m/s]
+        length_scale: float = 50.0,   # L [m]
+        reference_speed: float = 5.0, # V [m/s]
+        seed: int | None = None,
+    ) -> None:
+        self.intensity = intensity
+        self.length_scale = length_scale
+        self.reference_speed = reference_speed
+
+        self._seed = seed
+        self._rng = np.random.default_rng(seed)
+        self._wind = np.zeros(3)
+        self._alpha_cache: float | None = None  # exp(-V/L * dt), cached per dt
+
+    def reset(self) -> None:
+        self._rng = np.random.default_rng(self._seed)
+        self._wind = np.zeros(3)
+        self._alpha_cache = None
+
+    def advance(self, t: float, dt: float, state: NDArray) -> None:
+        if dt <= 0.0:
+            return
+        # Build the Dryden filter coefficients for this dt (cached).
+        if self._alpha_cache is None or self._alpha_cache != dt:
+            V, L = self.reference_speed, self.length_scale
+            self._alpha_cache = float(dt)
+            self._alpha = float(np.exp(-V / L * dt))
+            self._beta = self.intensity * float(np.sqrt(2.0 * V / (np.pi * L)))
+
+        # Discrete Gauss-Markov step
+        drive = self._rng.normal(0.0, 1.0, 3) / np.sqrt(dt + 1e-12)
+        self._wind = self._alpha * self._wind + self._beta * drive
+
+    def external_force(self, t: float, dt: float, state: NDArray) -> NDArray:
+        self.advance(t, dt, state)
+        return np.zeros(3)
+
+    def wind_velocity(self, t: float = 0.0) -> NDArray:
+        return self._wind.copy()
+
+
+# ---------------------------------------------------------------------------
+# Mechanical / failure disturbances
+# ---------------------------------------------------------------------------
+
+class MotorFailure(Disturbance):
+    """Degrade one or more rotors' thrust coefficient *k_f* at time *t_fail*.
+
+    The affected motor(s) produce ``efficiency * k_f`` after failure, simulating
+    a partial loss-of-thrust event (propeller damage, ESC brownout).
+    """
+
+    def __init__(
+        self,
+        motor_indices: list[int],
+        efficiency: float = 0.3,
+        t_fail: float = 1.0,
+    ) -> None:
+        self.motor_indices = motor_indices
+        self.efficiency = efficiency
+        self.t_fail = t_fail
+        self._failed = False
+
+    def reset(self) -> None:
+        self._failed = False
+
+    def modify_dynamics(self, quad: object, t: float) -> None:
+        if not self._failed and t >= self.t_fail:
+            self._failed = True
+
+    def rotor_efficiencies(self, t: float) -> NDArray:
+        efficiency = np.ones(4)
+        if t >= self.t_fail:
+            efficiency[self.motor_indices] = self.efficiency
+        return efficiency
+
+
+class PayloadDrop(Disturbance):
+    """Instantaneous mass change at time *t_drop* (simulates payload release).
+
+    At ``t >= t_drop`` the drone's mass is set to the post-drop value.
+    On ``reset()`` the mass is restored to the original constructor value.
+    """
+
+    def __init__(self, post_drop_mass: float, t_drop: float = 2.0) -> None:
+        self.post_drop_mass = post_drop_mass
+        self.t_drop = t_drop
+        self._active = False
+        self._original_mass: float | None = None
+
+    def reset(self) -> None:
+        self._active = False
+
+    def modify_dynamics(self, quad: object, t: float) -> None:
+        # Capture the drone's mass on the very first call (before any mutation).
+        if self._original_mass is None:
+            self._original_mass = float(quad.mass)
+        if not self._active and t >= self.t_drop:
+            self._active = True
+        if self._active:
+            quad.mass = self.post_drop_mass
+        else:
+            quad.mass = self._original_mass
+
+
+# ---------------------------------------------------------------------------
+# Environmental disturbances
+# ---------------------------------------------------------------------------
+
+class GroundEffect(Disturbance):
+    """Thrust augmentation near the ground.
+
+    The thrust multiplier follows an exponential decay with altitude:
+
+        T/T∞ = 1 + 0.3 * exp(-h / r)
+
+    where *h* is altitude above ground [m] and *r* is the rotor radius [m].
+    This gives:
+      - h = 0:     T/T∞ ≈ 1.3  (maximum augmentation)
+      - h = r:     T/T∞ ≈ 1.11
+      - h = 2r:    T/T∞ ≈ 1.04
+      - h → ∞:     T/T∞ → 1.0
+
+    The multiplier is applied inside ``_derivatives`` by calling
+    ``ground_effect.thrust_multiplier(altitude)`` and scaling the total thrust.
+
+    Attributes
+    ----------
+    radius: Rotor radius [m] (default 0.13 m for a 10" propeller).
+    """
+
+    def __init__(self, radius: float = 0.13) -> None:
+        self.radius = radius
+
+    def thrust_multiplier(self, alt: float) -> float:
+        alt_safe = max(alt, 0.0)
+        return float(1.0 + 0.3 * np.exp(-alt_safe / self.radius))