aerial-kit 0.1.0__py3-none-any.whl

aerial_kit/README.md

@@ -0,0 +1,58 @@
+# aerial_kit
+
+A shared control-stack package for aerial robots: airframe capabilities, actuator
+allocation/trim, 6-DOF and point-mass dynamics models, controllers (PID/LQR/MPC, and an
+L1 + TECS fixed-wing autopilot), and lateral/longitudinal guidance laws -- all pure
+numpy/scipy, no ROS and no matplotlib dependency.
+
+It grew out of [UAV-Controller](https://github.com/RawFish69/UAV-Controller), a
+quadcopter-to-general-aerial-robotics control stack, where it's the shared layer imported
+by both a standalone Python simulator and ROS 2 flight nodes so they don't duplicate
+control code.
+
+## What's in it
+
+- `aerial_kit.types` -- `SimState`, `ControlTarget`, `Capabilities`, `Wrench`,
+  `CommandKind`, `Waypoint`
+- `aerial_kit.interfaces` -- `Controller`, `DynamicsBackend`, `Planner` ABCs
+- `aerial_kit.registry` -- a pluggable component registry
+  (`register_airframe`/`register_controller`/etc., `create_*` factories)
+- `aerial_kit.airframes` -- `Airframe` ABC, `MultirotorAirframe` (mixer-driven quad/hex/
+  octo), `TwinWingAirframe` (elevon + differential-thrust allocation, trim)
+- `aerial_kit.dynamics` -- 6-DOF multirotor dynamics, point-mass dynamics, and a
+  hand-rolled 6-DOF fixed-wing model with a flat-plate-blended lift curve, drag polar,
+  and moment derivatives
+- `aerial_kit.controllers` -- PID/LQR/MPC position controllers, and
+  `FixedWingL1TECSController` (L1 lateral guidance + TECS-lite longitudinal control +
+  coordinated-turn attitude PID)
+- `aerial_kit.guidance` -- `l1_bank_command`, `tecs_command` as standalone functions
+
+## Quick start
+
+```python
+from aerial_kit.registry import register_builtin_components, create_airframe, create_controller
+
+register_builtin_components()
+airframe = create_airframe("quad")
+controller = create_controller("pid")
+print(airframe.capabilities)
+```
+
+`register_builtin_components()` here registers only what lives inside `aerial_kit`
+itself (airframes, controllers) -- it has no ROS or matplotlib dependency and does not
+know about any host application's own dynamics backends or planners. A host application
+(like `sim_py` in the parent repo) registers its own backends/planners into the same
+registry alongside this.
+
+## Status
+
+Early, actively developed alongside the parent repo's twin-motor-wing simulation work.
+The multirotor path (quad/hex/octo airframes, PID/LQR/MPC) is stable and used by a flying
+quadcopter's ground-station simulator. The fixed-wing path (`TwinWingAirframe`,
+`FixedWingL1TECSController`, guidance) is simulation-only so far -- not flown, and its aero
+model uses plausible placeholder coefficients rather than a fitted model of any specific
+real airframe.
+
+## License
+
+MIT

aerial_kit/__init__.py

@@ -0,0 +1,5 @@
+"""aerial_kit: shared control-stack package for aerial robots.
+
+No ROS and no matplotlib dependency, so it can be imported by both the
+standalone ``sim_py`` simulator and the ROS 2 nodes in ``ros2_ws/src/``.
+"""

aerial_kit/airframes/__init__.py

@@ -0,0 +1,9 @@
+"""Airframe profiles: capabilities, allocation, and trim per vehicle family."""
+
+from __future__ import annotations
+
+from .base import Airframe
+from .fixed_wing import TwinWingAirframe
+from .multirotor import MultirotorAirframe
+
+__all__ = ["Airframe", "MultirotorAirframe", "TwinWingAirframe"]

aerial_kit/airframes/base.py

@@ -0,0 +1,25 @@
+"""Abstract airframe interface: capabilities, allocation, trim."""
+
+from __future__ import annotations
+
+from abc import ABC, abstractmethod
+
+import numpy as np
+
+from ..types import Capabilities, SimState, Wrench
+
+
+class Airframe(ABC):
+    """A vehicle profile: what it can do, and how a wrench becomes actuator commands."""
+
+    name: str
+    capabilities: Capabilities
+
+    @abstractmethod
+    def allocate(self, wrench: Wrench, state: SimState) -> np.ndarray:
+        """Body thrust+moment -> per-actuator commands."""
+
+    @abstractmethod
+    def trim(self, state: SimState) -> np.ndarray:
+        """Actuator vector holding steady flight (hover for multirotor, level
+        cruise for a wing)."""

aerial_kit/airframes/fixed_wing.py

@@ -0,0 +1,104 @@
+"""Twin-motor flying wing: two motors, two elevons, no rudder.
+
+See plans/04-twin-wing-control/PLAN.md for the design. Yaw is differential
+thrust only -- authority is proportional to throttle and collapses at idle
+or in a glide, so a controller must coordinate turns with bank rather than
+relying on yaw. Aero coefficients for a specific airframe are unknown at
+this stage (no FixedWingBackend/aero model exists yet), so allocate() below
+is a deliberately simple, documented placeholder: moment_body is
+interpreted directly in actuator-native units (elevon radians for
+pitch/roll) rather than through a dynamic-pressure-scaled aero model. It
+will be superseded once a real aero model can convert aerodynamic moments
+into these commands.
+"""
+
+from __future__ import annotations
+
+import numpy as np
+
+from ..types import Capabilities, CommandKind, SimState, Wrench
+from .base import Airframe
+
+
+class TwinWingAirframe(Airframe):
+    """Flying wing: throttle_L/R + elevon_L/R actuators, AIRSPEED_NAV command kind."""
+
+    def __init__(
+        self,
+        mass_kg: float = 1.5,
+        cruise_airspeed_mps: float = 15.0,
+        min_airspeed_mps: float = 8.0,
+        max_airspeed_mps: float = 25.0,
+        max_climb_rate_mps: float = 3.0,
+        max_bank_deg: float = 45.0,
+        max_thrust_n: float = 10.0,
+        max_elevon_rad: float = np.radians(25.0),
+        motor_separation_m: float = 0.6,
+        trim_throttle_n: float = 3.0,
+        trim_elevon_rad: float = np.radians(2.0),
+        gravity_mps2: float = 9.81,
+    ) -> None:
+        if mass_kg <= 0.0:
+            raise ValueError(f"mass_kg must be positive, got {mass_kg}")
+        if motor_separation_m <= 0.0:
+            raise ValueError(f"motor_separation_m must be positive, got {motor_separation_m}")
+        if max_thrust_n <= 0.0:
+            raise ValueError(f"max_thrust_n must be positive, got {max_thrust_n}")
+        if not (0.0 < max_bank_deg < 90.0):
+            raise ValueError(f"max_bank_deg must be in (0, 90), got {max_bank_deg}")
+
+        self.name = "twin_wing"
+        self.mass_kg = float(mass_kg)
+        self.motor_separation_m = float(motor_separation_m)
+        self.max_thrust_n = float(max_thrust_n)
+        self.max_elevon_rad = float(max_elevon_rad)
+        self.trim_throttle_n = float(trim_throttle_n)
+        self.trim_elevon_rad = float(trim_elevon_rad)
+        self.gravity_mps2 = float(gravity_mps2)
+
+        phi_max_rad = np.radians(max_bank_deg)
+        min_turn_radius_m = (cruise_airspeed_mps**2) / (gravity_mps2 * np.tan(phi_max_rad))
+
+        self.capabilities = Capabilities(
+            can_hover=False,
+            min_airspeed_mps=float(min_airspeed_mps),
+            max_airspeed_mps=float(max_airspeed_mps),
+            max_climb_rate_mps=float(max_climb_rate_mps),
+            max_bank_deg=float(max_bank_deg),
+            min_turn_radius_m=float(min_turn_radius_m),
+            n_actuators=4,
+            command_kind=CommandKind.AIRSPEED_NAV,
+        )
+
+    def allocate(self, wrench: Wrench, state: SimState) -> np.ndarray:
+        """[throttle_L, throttle_R, elevon_L, elevon_R], saturating by priority.
+
+        Interprets ``wrench.force_body[0]`` as collective thrust demand (T_c) and
+        ``wrench.moment_body`` as [roll, pitch, yaw] demand. Pitch is preserved
+        first (it is what keeps the wing out of a stall), roll second, and yaw
+        (via differential thrust) last -- matching the plan's saturation-by-
+        priority rule rather than naive clipping.
+        """
+        t_c = float(np.asarray(wrench.force_body, dtype=float)[0])
+        roll_m, pitch_m, yaw_m = np.asarray(wrench.moment_body, dtype=float)
+
+        delta_pitch = float(np.clip(pitch_m, -self.max_elevon_rad, self.max_elevon_rad))
+        roll_headroom = self.max_elevon_rad - abs(delta_pitch)
+        delta_roll = float(np.clip(roll_m, -roll_headroom, roll_headroom))
+        elevon_l = delta_pitch + delta_roll
+        elevon_r = delta_pitch - delta_roll
+
+        t_c = float(np.clip(t_c, 0.0, self.max_thrust_n))
+        q_c_demand = yaw_m / self.motor_separation_m
+        yaw_headroom = min(self.max_thrust_n - t_c, t_c)
+        q_c = float(np.clip(q_c_demand, -yaw_headroom, yaw_headroom))
+        throttle_l = float(np.clip(t_c + q_c, 0.0, self.max_thrust_n))
+        throttle_r = float(np.clip(t_c - q_c, 0.0, self.max_thrust_n))
+
+        return np.array([throttle_l, throttle_r, elevon_l, elevon_r], dtype=float)
+
+    def trim(self, state: SimState) -> np.ndarray:
+        return np.array(
+            [self.trim_throttle_n, self.trim_throttle_n, self.trim_elevon_rad, self.trim_elevon_rad],
+            dtype=float,
+        )

aerial_kit/airframes/multirotor.py

@@ -0,0 +1,81 @@
+"""Multirotor airframe: quad/hex/octo differ only in mixer geometry."""
+
+from __future__ import annotations
+
+import numpy as np
+
+from ..types import Capabilities, CommandKind, SimState, Wrench
+from .base import Airframe
+
+
+class MultirotorAirframe(Airframe):
+    """An omnidirectional multirotor: quad, hex, or octo, X or + layout.
+
+    The allocation matrix maps [total_thrust, Mx, My, Mz] to per-rotor thrust
+    via its pseudo-inverse. Rotors are placed on a regular polygon and spin
+    direction alternates so that all-equal thrust yields zero net yaw torque
+    at trim.
+    """
+
+    def __init__(
+        self,
+        arms: int = 4,
+        layout: str = "x",
+        arm_length_m: float = 0.2,
+        mass_kg: float = 1.0,
+        yaw_torque_coeff: float = 0.02,
+        gravity_mps2: float = 9.81,
+        max_airspeed_mps: float = 20.0,
+        max_climb_rate_mps: float = 5.0,
+        max_bank_deg: float = 45.0,
+    ) -> None:
+        if arms < 3:
+            raise ValueError(f"arms must be >= 3, got {arms}")
+        if layout not in {"x", "+"}:
+            raise ValueError(f"layout must be 'x' or '+', got {layout!r}")
+
+        self.name = f"multirotor_{arms}_{layout}"
+        self.arms = arms
+        self.layout = layout
+        self.arm_length_m = float(arm_length_m)
+        self.mass_kg = float(mass_kg)
+        self.yaw_torque_coeff = float(yaw_torque_coeff)
+        self.gravity_mps2 = float(gravity_mps2)
+
+        self.capabilities = Capabilities(
+            can_hover=True,
+            min_airspeed_mps=None,
+            max_airspeed_mps=float(max_airspeed_mps),
+            max_climb_rate_mps=float(max_climb_rate_mps),
+            max_bank_deg=float(max_bank_deg),
+            min_turn_radius_m=None,
+            n_actuators=arms,
+            command_kind=CommandKind.ACCEL,
+        )
+
+        self._mixer = self._build_mixer()
+        self._mixer_pinv = np.linalg.pinv(self._mixer)
+
+    def _build_mixer(self) -> np.ndarray:
+        offset = np.pi / self.arms if self.layout == "x" else 0.0
+        mixer = np.zeros((4, self.arms), dtype=float)
+        for i in range(self.arms):
+            theta = 2.0 * np.pi * i / self.arms + offset
+            x_i = self.arm_length_m * np.cos(theta)
+            y_i = self.arm_length_m * np.sin(theta)
+            spin_dir = 1.0 if i % 2 == 0 else -1.0
+            mixer[0, i] = 1.0
+            mixer[1, i] = y_i
+            mixer[2, i] = -x_i
+            mixer[3, i] = spin_dir * self.yaw_torque_coeff
+        return mixer
+
+    def allocate(self, wrench: Wrench, state: SimState) -> np.ndarray:
+        thrust_total = float(np.asarray(wrench.force_body, dtype=float)[2])
+        mx, my, mz = np.asarray(wrench.moment_body, dtype=float)
+        demand = np.array([thrust_total, mx, my, mz], dtype=float)
+        return self._mixer_pinv @ demand
+
+    def trim(self, state: SimState) -> np.ndarray:
+        demand = np.array([self.mass_kg * self.gravity_mps2, 0.0, 0.0, 0.0], dtype=float)
+        return self._mixer_pinv @ demand

aerial_kit/controllers/__init__.py

@@ -0,0 +1,18 @@
+"""Controller implementations (numpy/scipy-only, no ROS/matplotlib)."""
+
+from .basic import LQRController, MPCController, PIDController
+from .fixed_wing import AttitudeGains, FixedWingL1TECSController, body_axis_pitch_bank
+from .position import lqr_gain_double_integrator, lqr_position_control, mpc_position_control, pid_position_control
+
+__all__ = [
+    "PIDController",
+    "LQRController",
+    "MPCController",
+    "FixedWingL1TECSController",
+    "AttitudeGains",
+    "body_axis_pitch_bank",
+    "pid_position_control",
+    "lqr_gain_double_integrator",
+    "lqr_position_control",
+    "mpc_position_control",
+]

aerial_kit/controllers/basic.py

@@ -0,0 +1,79 @@
+"""Built-in controller implementations for the simulation framework."""
+
+from __future__ import annotations
+
+from typing import Any, Mapping
+
+import numpy as np
+
+from ..interfaces import Controller
+from ..types import ControlTarget, SimState, Waypoint
+from .position import lqr_position_control, mpc_position_control, pid_position_control
+
+
+class PIDController(Controller):
+    """PD position controller returning acceleration commands."""
+
+    def compute(
+        self,
+        state: SimState,
+        target_waypoint: Waypoint,
+        cfg: Mapping[str, Any],
+    ) -> ControlTarget:
+        ctrl_cfg = dict(cfg.get("controller", {}) or {})
+        pid_cfg = dict(ctrl_cfg.get("pid", {}) or {})
+        kp = float(pid_cfg.get("kp", 0.8))
+        kd = float(pid_cfg.get("kd", 1.2))
+
+        acc_cmd = pid_position_control(
+            state.position,
+            state.velocity,
+            target_waypoint.position,
+            kp=kp,
+            kd=kd,
+        )
+        return ControlTarget(accel_cmd=np.asarray(acc_cmd, dtype=float), metadata={"controller": "pid"})
+
+
+class LQRController(Controller):
+    """LQR position controller returning acceleration commands."""
+
+    def compute(
+        self,
+        state: SimState,
+        target_waypoint: Waypoint,
+        cfg: Mapping[str, Any],
+    ) -> ControlTarget:
+        ctrl_cfg = dict(cfg.get("controller", {}) or {})
+        lqr_cfg = dict(ctrl_cfg.get("lqr", {}) or {})
+        acc_cmd = lqr_position_control(
+            state.position,
+            state.velocity,
+            target_waypoint.position,
+            q_pos=float(lqr_cfg.get("q_pos", 10.0)),
+            q_vel=float(lqr_cfg.get("q_vel", 2.0)),
+            r_acc=float(lqr_cfg.get("r_acc", 1.0)),
+        )
+        return ControlTarget(accel_cmd=np.asarray(acc_cmd, dtype=float), metadata={"controller": "lqr"})
+
+
+class MPCController(Controller):
+    """MPC-like position controller returning acceleration commands."""
+
+    def compute(
+        self,
+        state: SimState,
+        target_waypoint: Waypoint,
+        cfg: Mapping[str, Any],
+    ) -> ControlTarget:
+        ctrl_cfg = dict(cfg.get("controller", {}) or {})
+        mpc_cfg = dict(ctrl_cfg.get("mpc", {}) or {})
+        acc_cmd = mpc_position_control(
+            state.position,
+            state.velocity,
+            target_waypoint.position,
+            q_pos=float(mpc_cfg.get("q_pos", 8.0)),
+            q_vel=float(mpc_cfg.get("q_vel", 2.0)),
+            r_acc=float(mpc_cfg.get("r_acc", 10.0)),
+        )
+        return ControlTarget(accel_cmd=np.asarray(acc_cmd, dtype=float), metadata={"controller": "mpc"})

aerial_kit/controllers/fixed_wing.py

@@ -0,0 +1,147 @@
+"""AIRSPEED_NAV controller for the twin wing: L1 lateral + TECS-lite + attitude PID.
+
+Produces a body ``Wrench`` (in ``ControlTarget.metadata["wrench"]``, since the
+shared ``ControlTarget.accel_cmd`` field is an ACCEL-kind concept this
+controller doesn't use) for ``TwinWingAirframe.allocate()`` to turn into
+actuator commands. Three loops, outer to inner, per plan 04:
+
+1. L1 picks a bank command from the vehicle's position/velocity toward the
+   target waypoint (see ``aerial_kit.guidance.l1`` for why this is the
+   point-target variant, not full path-segment L1).
+2. TECS-lite picks throttle + pitch command from airspeed/altitude error (see
+   ``aerial_kit.guidance.tecs``).
+3. An attitude PID converts bank/pitch *commands* into roll/pitch *moment*
+   demands using the vehicle's actual attitude, closing the loop.
+
+Attitude feedback avoids extracting classical Euler angles from
+``attitude_quat`` (its body-FRD/world-ENU convention is not the one those
+formulas assume -- see ``aerial_kit/dynamics/fixed_wing.py``'s docstring). Instead
+it uses body axes projected onto the world frame directly: pitch is the
+elevation of the body-forward axis above the horizon, bank is (approximately,
+for small pitch) the depression of the body-right axis below the horizon.
+This sidesteps Euler-order/sign ambiguity entirely and is exact at zero pitch,
+which covers the cruise-flight regime this whole hand-rolled model targets.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Any, Mapping
+
+import numpy as np
+
+from ..dynamics.fixed_wing import quat_to_rotmat
+from ..guidance.l1 import l1_bank_command
+from ..guidance.tecs import TecsGains, tecs_command
+from ..interfaces import Controller
+from ..types import CommandKind, ControlTarget, SimState, Waypoint, Wrench
+
+
+@dataclass
+class AttitudeGains:
+    kp_roll: float = 0.6
+    kd_roll: float = 0.15
+    kp_pitch: float = 0.8
+    kd_pitch: float = 0.2
+    kp_yaw_rate: float = 2.0
+
+
+def body_axis_pitch_bank(attitude_quat: np.ndarray) -> tuple[float, float]:
+    """(pitch_rad, bank_rad) from body axes projected onto the world frame."""
+    rot = quat_to_rotmat(attitude_quat)
+    body_x_world = rot @ np.array([1.0, 0.0, 0.0])
+    body_y_world = rot @ np.array([0.0, 1.0, 0.0])
+    pitch = float(np.arcsin(np.clip(body_x_world[2], -1.0, 1.0)))
+    bank = float(-np.arcsin(np.clip(body_y_world[2], -1.0, 1.0)))
+    return pitch, bank
+
+
+class FixedWingL1TECSController(Controller):
+    """L1 + TECS-lite + attitude PID, emitting a Wrench for TwinWingAirframe."""
+
+    command_kind = CommandKind.AIRSPEED_NAV
+
+    def compute(
+        self,
+        state: SimState,
+        target_waypoint: Waypoint,
+        cfg: Mapping[str, Any],
+    ) -> ControlTarget:
+        ctrl_cfg = dict(cfg.get("controller", {}) or {})
+        fw_cfg = dict(ctrl_cfg.get("l1_tecs", {}) or {})
+
+        cruise_airspeed_mps = float(fw_cfg.get("cruise_airspeed_mps", 15.0))
+        l1_distance_m = float(fw_cfg.get("l1_distance_m", 40.0))
+        max_bank_rad = float(fw_cfg.get("max_bank_rad", np.radians(45.0)))
+        gravity_mps2 = float(fw_cfg.get("gravity_mps2", 9.81))
+
+        tecs_gains = TecsGains(
+            kp_throttle=float(fw_cfg.get("tecs_kp_throttle", 0.15)),
+            kp_pitch=float(fw_cfg.get("tecs_kp_pitch", 0.08)),
+            trim_throttle_n=float(fw_cfg.get("trim_throttle_n", 3.0)),
+            max_thrust_n=float(fw_cfg.get("max_thrust_n", 10.0)),
+            max_pitch_rad=float(fw_cfg.get("max_pitch_rad", 0.35)),
+        )
+        att_gains = AttitudeGains(
+            kp_roll=float(fw_cfg.get("kp_roll", 0.6)),
+            kd_roll=float(fw_cfg.get("kd_roll", 0.15)),
+            kp_pitch=float(fw_cfg.get("kp_pitch", 0.8)),
+            kd_pitch=float(fw_cfg.get("kd_pitch", 0.2)),
+            kp_yaw_rate=float(fw_cfg.get("kp_yaw_rate", 2.0)),
+        )
+
+        position = np.asarray(state.position, dtype=float)
+        velocity = np.asarray(state.velocity, dtype=float)
+        airspeed = float(np.linalg.norm(velocity[:2]))
+
+        bank_cmd = l1_bank_command(
+            position_xy=position[:2],
+            velocity_xy=velocity[:2],
+            target_xy=np.asarray(target_waypoint.position, dtype=float)[:2],
+            airspeed_mps=max(airspeed, 1.0),
+            l1_distance_m=l1_distance_m,
+            max_bank_rad=max_bank_rad,
+            gravity_mps2=gravity_mps2,
+        )
+        throttle_cmd_n, pitch_cmd = tecs_command(
+            airspeed_mps=airspeed,
+            airspeed_cmd_mps=cruise_airspeed_mps,
+            altitude_m=float(position[2]),
+            altitude_cmd_m=float(target_waypoint.position[2]),
+            gains=tecs_gains,
+            gravity_mps2=gravity_mps2,
+        )
+
+        if state.attitude_quat is None or state.body_rates is None:
+            pitch, bank = 0.0, 0.0
+            p_rate = q_rate = r_rate = 0.0
+        else:
+            pitch, bank = body_axis_pitch_bank(np.asarray(state.attitude_quat, dtype=float))
+            p_rate, q_rate, r_rate = np.asarray(state.body_rates, dtype=float)
+
+        roll_m = att_gains.kp_roll * (bank_cmd - bank) - att_gains.kd_roll * p_rate
+        pitch_m = att_gains.kp_pitch * (pitch_cmd - pitch) - att_gains.kd_pitch * q_rate
+
+        # Coordinated turn: command the yaw rate a level turn at this bank and
+        # airspeed actually needs (r_cmd = g*tan(bank)/V), rather than only
+        # damping yaw rate toward zero -- banking with no yaw-rate command
+        # produces sideslip, not a clean turn, since this model has no rudder
+        # and only weak weathervaning aero to close that gap on its own.
+        turn_airspeed = max(airspeed, 1.0)
+        r_cmd = gravity_mps2 * np.tan(bank) / turn_airspeed
+        yaw_m = att_gains.kp_yaw_rate * (r_cmd - r_rate)
+
+        wrench = Wrench(
+            force_body=np.array([throttle_cmd_n, 0.0, 0.0], dtype=float),
+            moment_body=np.array([roll_m, pitch_m, yaw_m], dtype=float),
+        )
+        return ControlTarget(
+            accel_cmd=np.zeros(3, dtype=float),
+            metadata={
+                "controller": "l1_tecs",
+                "wrench": wrench,
+                "bank_cmd": bank_cmd,
+                "pitch_cmd": pitch_cmd,
+                "throttle_cmd_n": throttle_cmd_n,
+            },
+        )

aerial_kit/controllers/position.py

@@ -0,0 +1,79 @@
+"""Position-level controllers for the standalone simulator.
+
+These operate on a simple point-mass model:
+  x_dot = v, v_dot = a_cmd
+"""
+
+from __future__ import annotations
+
+import numpy as np
+from scipy.linalg import solve_continuous_are
+
+
+def pid_position_control(
+    pos: np.ndarray,
+    vel: np.ndarray,
+    target: np.ndarray,
+    kp: float,
+    kd: float,
+) -> np.ndarray:
+    """Compute acceleration command using simple PD on position."""
+    pos = np.asarray(pos, dtype=float)
+    vel = np.asarray(vel, dtype=float)
+    target = np.asarray(target, dtype=float)
+
+    pos_error = target - pos
+    vel_error = -vel
+    acc_cmd = kp * pos_error + kd * vel_error
+    return acc_cmd
+
+
+def lqr_gain_double_integrator(q_pos: float, q_vel: float, r_acc: float) -> np.ndarray:
+    """Compute continuous-time LQR gain for a 1D double integrator."""
+    A = np.array([[0.0, 1.0], [0.0, 0.0]])
+    B = np.array([[0.0], [1.0]])
+    Q = np.diag([q_pos, q_vel])
+    R = np.array([[r_acc]])
+
+    P = solve_continuous_are(A, B, Q, R)
+    K = np.linalg.inv(R) @ B.T @ P
+    return K
+
+
+def lqr_position_control(
+    pos: np.ndarray,
+    vel: np.ndarray,
+    target: np.ndarray,
+    q_pos: float = 10.0,
+    q_vel: float = 2.0,
+    r_acc: float = 1.0,
+) -> np.ndarray:
+    """LQR state-feedback for each axis of a double integrator."""
+    pos = np.asarray(pos, dtype=float)
+    vel = np.asarray(vel, dtype=float)
+    target = np.asarray(target, dtype=float)
+
+    x_err = np.stack([pos - target, vel], axis=-1)
+    K = lqr_gain_double_integrator(q_pos, q_vel, r_acc)
+    acc_cmd = -np.einsum("ij,bj->bi", K, x_err).squeeze(-1)
+    return acc_cmd
+
+
+def mpc_position_control(
+    pos: np.ndarray,
+    vel: np.ndarray,
+    target: np.ndarray,
+    q_pos: float,
+    q_vel: float,
+    r_acc: float,
+) -> np.ndarray:
+    """Very lightweight MPC-like controller."""
+    return lqr_position_control(pos, vel, target, q_pos=q_pos, q_vel=q_vel, r_acc=r_acc)
+
+
+__all__ = [
+    "pid_position_control",
+    "lqr_gain_double_integrator",
+    "lqr_position_control",
+    "mpc_position_control",
+]

aerial_kit/dynamics/__init__.py

@@ -0,0 +1,17 @@
+"""6-DOF and point-mass dynamics models (numpy-only, no ROS/matplotlib)."""
+
+from .fixed_wing import FixedWingDynamics, FixedWingParams, level_attitude_quat, lift_coefficient, quat_to_rotmat
+from .multirotor import DynamicsParams, UAVDynamics
+from .pointmass import PointMassDynamics, PointMassParams
+
+__all__ = [
+    "FixedWingDynamics",
+    "FixedWingParams",
+    "level_attitude_quat",
+    "lift_coefficient",
+    "quat_to_rotmat",
+    "DynamicsParams",
+    "UAVDynamics",
+    "PointMassDynamics",
+    "PointMassParams",
+]