drones-sim 0.2.0__py3-none-any.whl

drones_sim/__init__.py

@@ -0,0 +1,15 @@
+"""Composable quadcopter modeling, control, estimation, and visualization."""
+
+from .simulation import ClosedLoopSimulator, SimulationConfig, SimulationResult
+from .state import ControlOutput, TrajectorySetpoint, VehicleState
+from .visualization import visualize
+
+__all__ = [
+    "ClosedLoopSimulator",
+    "ControlOutput",
+    "SimulationConfig",
+    "SimulationResult",
+    "TrajectorySetpoint",
+    "VehicleState",
+    "visualize",
+]

drones_sim/control/__init__.py

@@ -0,0 +1,15 @@
+from .allocation import AllocationResult, ControlAllocator
+from .cascaded import QuadcopterController
+from .geometric import GeometricController, GeometricControllerConfig
+from .lqr import LQRController
+from .pid import PIDController
+
+__all__ = [
+    "AllocationResult",
+    "ControlAllocator",
+    "GeometricController",
+    "GeometricControllerConfig",
+    "LQRController",
+    "PIDController",
+    "QuadcopterController",
+]

drones_sim/control/allocation.py

@@ -0,0 +1,76 @@
+"""Bounded control allocation from body wrench to rotor speeds."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+from numpy.typing import NDArray
+from scipy.optimize import lsq_linear
+
+
+@dataclass
+class AllocationResult:
+    motor_speeds: NDArray
+    achieved_wrench: NDArray
+    saturated: bool
+    residual_norm: float
+
+
+class ControlAllocator:
+    """Weighted, bounded least-squares rotor allocator.
+
+    Solving in squared-speed space respects the one-sided nature of propeller
+    thrust. Unlike inverse-then-clip allocation, it redistributes an infeasible
+    request over the remaining authority instead of silently changing all four
+    wrench axes.
+    """
+
+    def __init__(
+        self,
+        allocation_matrix: NDArray,
+        min_speed: float = 0.0,
+        max_speed: float = 4000.0,
+        weights: NDArray | None = None,
+    ) -> None:
+        self.matrix = np.asarray(allocation_matrix, dtype=float).copy()
+        if self.matrix.shape != (4, 4):
+            raise ValueError("allocation_matrix must have shape (4, 4)")
+        self.min_speed = float(min_speed)
+        self.max_speed = float(max_speed)
+        if not 0.0 <= self.min_speed < self.max_speed:
+            raise ValueError("motor speed limits are invalid")
+        self.weights = np.asarray(
+            np.ones(4) if weights is None else weights, dtype=float
+        )
+        if self.weights.shape != (4,) or np.any(self.weights <= 0.0):
+            raise ValueError("weights must be a positive shape-(4,) vector")
+
+    def allocate(self, wrench: NDArray) -> AllocationResult:
+        desired = np.asarray(wrench, dtype=float)
+        if desired.shape != (4,) or not np.all(np.isfinite(desired)):
+            raise ValueError("wrench must be a finite shape-(4,) vector")
+        weighted_matrix = self.weights[:, None] * self.matrix
+        weighted_wrench = self.weights * desired
+        solution = lsq_linear(
+            weighted_matrix,
+            weighted_wrench,
+            bounds=(self.min_speed**2, self.max_speed**2),
+            method="bvls",
+            tol=1e-10,
+        )
+        squared_speeds = np.maximum(solution.x, 0.0)
+        motor_speeds = np.sqrt(squared_speeds)
+        achieved = self.matrix @ squared_speeds
+        tolerance = 1e-7 * max(1.0, self.max_speed**2)
+        saturated = bool(
+            np.any(squared_speeds <= self.min_speed**2 + tolerance)
+            or np.any(squared_speeds >= self.max_speed**2 - tolerance)
+            or np.linalg.norm(achieved - desired) > 1e-5
+        )
+        return AllocationResult(
+            motor_speeds=motor_speeds,
+            achieved_wrench=achieved,
+            saturated=saturated,
+            residual_norm=float(np.linalg.norm(achieved - desired)),
+        )

drones_sim/control/cascaded.py

@@ -0,0 +1,204 @@
+"""Cascaded PID controller for quadcopter position→velocity→attitude control.
+
+Consolidated from quadcopter_simulation.py.
+"""
+
+from __future__ import annotations
+
+import numpy as np
+from numpy.typing import NDArray
+
+from ..dynamics.quadcopter import QuadcopterDynamics
+from .allocation import ControlAllocator
+from .pid import PIDController
+
+
+class QuadcopterController:
+    """Three-loop cascaded controller with physically consistent outputs."""
+
+    def __init__(self, quad: QuadcopterDynamics):
+        self.quad = quad
+        self.convergence_threshold = 0.05
+        self.max_lateral_speed = 3.0          # increased from 1.5 for trajectory tracking
+        self.max_vertical_speed = 1.0
+        self.max_lateral_accel = 5.0          # increased from 3.0
+        self.max_vertical_accel = 4.0
+        self.max_tilt = np.deg2rad(25.0)      # increased from 17° — was saturating 74% of the time
+        self.max_torque = np.array([0.25, 0.25, 0.08])
+        self.rate_damping = np.array([0.03, 0.03, 0.015])
+        self.min_thrust = 0.2 * self.quad.mass * self.quad.g
+        self.max_thrust = 2.0 * self.quad.mass * self.quad.g
+        self.max_motor_speed = 4000.0
+
+        # -- Position (outer) --
+        # Wider output limits match new max_lateral_speed; higher kp+kd for
+        # faster acquisition of a moving reference.
+        self.x_ctrl = PIDController(1.8, 0.1, 0.5, (-3.0, 3.0), (-1.0, 1.0))
+        self.y_ctrl = PIDController(1.8, 0.1, 0.5, (-3.0, 3.0), (-1.0, 1.0))
+        self.z_ctrl = PIDController(1.6, 0.25, 0.45, (-1.0, 1.0), (-0.8, 0.8))
+
+        # -- Velocity (middle) --
+        # Higher kp and wider limits feed more accel demand to the attitude loop.
+        self.vx_ctrl = PIDController(3.0, 0.4, 0.3, (-5.0, 5.0), (-1.0, 1.0))
+        self.vy_ctrl = PIDController(3.0, 0.4, 0.3, (-5.0, 5.0), (-1.0, 1.0))
+        self.vz_ctrl = PIDController(5.0, 1.5, 0.3, (-4.0, 4.0), (-1.0, 1.0))
+
+        # -- Attitude (inner, torque outputs in N·m) --
+        # kp doubled + kd added so the drone reaches the commanded tilt quickly;
+        # without this, the attitude loop is the bottleneck at high tilt angles.
+        self.roll_ctrl = PIDController(
+            0.25,
+            0.03,
+            0.08,
+            (-0.25, 0.25),
+            (-0.15, 0.15),
+        )
+        self.pitch_ctrl = PIDController(
+            0.25,
+            0.03,
+            0.08,
+            (-0.25, 0.25),
+            (-0.15, 0.15),
+        )
+        self.yaw_ctrl = PIDController(
+            0.05,
+            0.01,
+            0.0,
+            (-0.08, 0.08),
+            (-0.08, 0.08),
+        )
+
+        self._prev_target_vel = np.zeros(3)
+
+    def reset(self) -> None:
+        for ctrl in [
+            self.x_ctrl, self.y_ctrl, self.z_ctrl,
+            self.vx_ctrl, self.vy_ctrl, self.vz_ctrl,
+            self.roll_ctrl, self.pitch_ctrl, self.yaw_ctrl,
+        ]:
+            ctrl.reset()
+        self._prev_target_vel = np.zeros(3)
+
+    def compute(
+        self,
+        target_pos: NDArray,
+        target_yaw: float,
+        dt: float,
+        prev_target_pos: NDArray | None = None,
+    ) -> NDArray:
+        """Compute 4 motor speeds for position + yaw tracking.
+
+        Returns motor speeds as a (4,) array.
+        """
+        pos = self.quad.get_position()
+        vel = self.quad.get_velocity()
+        att = self.quad.get_attitude()
+        omega = self.quad.get_angular_velocity()
+
+        target_vel_ff = np.zeros(3)
+        if prev_target_pos is not None:
+            target_vel_ff = (target_pos - prev_target_pos) / max(dt, 1e-6)
+        target_vel_ff = np.clip(
+            target_vel_ff,
+            [
+                -self.max_lateral_speed,
+                -self.max_lateral_speed,
+                -self.max_vertical_speed,
+            ],
+            [
+                self.max_lateral_speed,
+                self.max_lateral_speed,
+                self.max_vertical_speed,
+            ],
+        )
+
+        # Position -> desired velocity (m/s)
+        target_vel = np.array([
+            self.x_ctrl.update(target_pos[0], pos[0], dt),
+            self.y_ctrl.update(target_pos[1], pos[1], dt),
+            self.z_ctrl.update(target_pos[2], pos[2], dt),
+        ])
+        # High FF coefficient (0.9) directly injects trajectory velocity so
+        # the drone doesn't have to build up a large position error first.
+        target_vel += np.array([0.9, 0.9, 0.15]) * target_vel_ff
+        target_vel = np.clip(
+            target_vel,
+            [
+                -self.max_lateral_speed,
+                -self.max_lateral_speed,
+                -self.max_vertical_speed,
+            ],
+            [
+                self.max_lateral_speed,
+                self.max_lateral_speed,
+                self.max_vertical_speed,
+            ],
+        )
+
+        # Velocity -> desired acceleration (m/s^2)
+        desired_accel = np.array([
+            self.vx_ctrl.update(target_vel[0], vel[0], dt),
+            self.vy_ctrl.update(target_vel[1], vel[1], dt),
+            self.vz_ctrl.update(target_vel[2], vel[2], dt),
+        ])
+        target_acc_ff = (target_vel - self._prev_target_vel) / max(dt, 1e-6)
+        self._prev_target_vel = target_vel.copy()
+        desired_accel += np.array([0.3, 0.3, 0.05]) * target_acc_ff
+        desired_accel[:2] = np.clip(
+            desired_accel[:2],
+            -self.max_lateral_accel,
+            self.max_lateral_accel,
+        )
+        desired_accel[2] = np.clip(
+            desired_accel[2],
+            -self.max_vertical_accel,
+            self.max_vertical_accel,
+        )
+
+        # Small-angle mapping with package rotation convention:
+        # positive pitch -> +x acceleration, positive roll -> -y acceleration.
+        des_pitch = np.clip(
+            desired_accel[0] / self.quad.g,
+            -self.max_tilt,
+            self.max_tilt,
+        )
+        des_roll = np.clip(
+            -desired_accel[1] / self.quad.g,
+            -self.max_tilt,
+            self.max_tilt,
+        )
+
+        thrust = self.quad.mass * (self.quad.g + desired_accel[2])
+        thrust /= max(np.cos(att[0]) * np.cos(att[1]), 0.7)
+        thrust = float(np.clip(thrust, self.min_thrust, self.max_thrust))
+
+        # Attitude -> torques (N·m) with angular-rate damping.
+        roll_tau = self.roll_ctrl.update(
+            des_roll,
+            att[0],
+            dt,
+        ) - self.rate_damping[0] * omega[0]
+        pitch_tau = self.pitch_ctrl.update(
+            des_pitch,
+            att[1],
+            dt,
+        ) - self.rate_damping[1] * omega[1]
+        yaw_tau = self.yaw_ctrl.update(
+            target_yaw,
+            att[2],
+            dt,
+        ) - self.rate_damping[2] * omega[2]
+        torques = np.clip(
+            np.array([roll_tau, pitch_tau, yaw_tau]),
+            -self.max_torque,
+            self.max_torque,
+        )
+
+        desired_wrench = np.array([thrust, torques[0], torques[1], torques[2]])
+
+        allocator = ControlAllocator(
+            self.quad.allocation_matrix(),
+            min_speed=self.quad.min_motor_speed,
+            max_speed=min(self.max_motor_speed, self.quad.max_motor_speed),
+        )
+        return allocator.allocate(desired_wrench).motor_speeds

drones_sim/control/geometric.py

@@ -0,0 +1,170 @@
+"""Coordinate-free SE(3) position and attitude controller."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+
+import numpy as np
+from numpy.typing import NDArray
+
+from ..state import ControlOutput, TrajectorySetpoint, VehicleState
+from .allocation import ControlAllocator
+
+
+@dataclass
+class GeometricControllerConfig:
+    position_gain: NDArray = field(
+        default_factory=lambda: np.array([3.0, 3.0, 6.0])
+    )
+    velocity_gain: NDArray = field(
+        default_factory=lambda: np.array([3.0, 3.0, 4.0])
+    )
+    integral_gain: NDArray = field(
+        default_factory=lambda: np.array([0.02, 0.02, 0.10])
+    )
+    attitude_gain: NDArray = field(
+        default_factory=lambda: np.array([0.22, 0.22, 0.08])
+    )
+    rate_gain: NDArray = field(
+        default_factory=lambda: np.array([0.06, 0.06, 0.025])
+    )
+    integral_limit: NDArray = field(
+        default_factory=lambda: np.array([2.0, 2.0, 1.0])
+    )
+    max_tilt: float = np.deg2rad(45.0)
+    min_thrust_ratio: float = 0.05
+    max_thrust_ratio: float = 2.5
+
+
+def _vee(skew: NDArray) -> NDArray:
+    return np.array([skew[2, 1], skew[0, 2], skew[1, 0]])
+
+
+class GeometricController:
+    """Nonlinear trajectory controller on SE(3).
+
+    Position feedback produces a desired world-frame force. Its direction and
+    the requested yaw define the full desired attitude, avoiding Euler-angle
+    singularities and the small-angle approximation used by the legacy PID.
+    """
+
+    def __init__(self, quad, config: GeometricControllerConfig | None = None):
+        self.quad = quad
+        self.config = config or GeometricControllerConfig()
+        self._integral_error = np.zeros(3)
+        self._previous_target: NDArray | None = None
+
+    def reset(self) -> None:
+        self._integral_error.fill(0.0)
+        self._previous_target = None
+
+    def compute_output(
+        self,
+        setpoint: TrajectorySetpoint,
+        dt: float,
+        state: VehicleState | None = None,
+    ) -> ControlOutput:
+        if dt <= 0.0:
+            raise ValueError("dt must be positive")
+        current = self.quad.get_state() if state is None else state
+        cfg = self.config
+        mass = self.quad.mass
+        gravity = self.quad.g
+
+        position_error = setpoint.position - current.position
+        velocity_error = setpoint.velocity - current.velocity
+        self._integral_error += position_error * dt
+        self._integral_error = np.clip(
+            self._integral_error, -cfg.integral_limit, cfg.integral_limit
+        )
+
+        desired_accel = (
+            setpoint.acceleration
+            + cfg.position_gain * position_error
+            + cfg.velocity_gain * velocity_error
+            + cfg.integral_gain * self._integral_error
+        )
+        desired_force = mass * (desired_accel + np.array([0.0, 0.0, gravity]))
+
+        # Enforce a tilt cone by limiting the horizontal/vertical force ratio.
+        vertical = max(float(desired_force[2]), 1e-6)
+        horizontal = desired_force[:2]
+        horizontal_norm = float(np.linalg.norm(horizontal))
+        max_horizontal = vertical * np.tan(cfg.max_tilt)
+        if horizontal_norm > max_horizontal:
+            desired_force[:2] *= max_horizontal / horizontal_norm
+
+        force_norm = float(np.linalg.norm(desired_force))
+        if force_norm < 1e-9:
+            desired_b3 = np.array([0.0, 0.0, 1.0])
+        else:
+            desired_b3 = desired_force / force_norm
+        heading = np.array([np.cos(setpoint.yaw), np.sin(setpoint.yaw), 0.0])
+        desired_b2 = np.cross(desired_b3, heading)
+        if np.linalg.norm(desired_b2) < 1e-8:
+            desired_b2 = np.array([-np.sin(setpoint.yaw), np.cos(setpoint.yaw), 0.0])
+        desired_b2 /= np.linalg.norm(desired_b2)
+        desired_b1 = np.cross(desired_b2, desired_b3)
+        desired_rotation = np.column_stack([desired_b1, desired_b2, desired_b3])
+
+        rotation = current.rotation_matrix
+        attitude_error = 0.5 * _vee(
+            desired_rotation.T @ rotation - rotation.T @ desired_rotation
+        )
+        desired_rates = desired_rotation.T @ np.array(
+            [0.0, 0.0, setpoint.yaw_rate]
+        )
+        rate_error = current.body_rates - rotation.T @ desired_rotation @ desired_rates
+
+        thrust = float(np.dot(desired_force, rotation[:, 2]))
+        hover = mass * gravity
+        thrust = float(
+            np.clip(
+                thrust,
+                cfg.min_thrust_ratio * hover,
+                cfg.max_thrust_ratio * hover,
+            )
+        )
+        inertia = self.quad.I
+        torque = (
+            -cfg.attitude_gain * attitude_error
+            - cfg.rate_gain * rate_error
+            + np.cross(current.body_rates, inertia @ current.body_rates)
+        )
+        requested_wrench = np.concatenate([[thrust], torque])
+        allocator = ControlAllocator(
+            self.quad.allocation_matrix(),
+            min_speed=self.quad.min_motor_speed,
+            max_speed=self.quad.max_motor_speed,
+            weights=np.array([1.0, 5.0, 5.0, 2.0]),
+        )
+        allocation = allocator.allocate(requested_wrench)
+        if allocation.saturated:
+            # Back-calculation prevents integral accumulation when thrust is limited.
+            self._integral_error *= 0.995
+        return ControlOutput(
+            thrust=thrust,
+            torque=torque,
+            motor_speeds=allocation.motor_speeds,
+            achieved_wrench=allocation.achieved_wrench,
+            saturated=allocation.saturated,
+        )
+
+    def compute(
+        self,
+        target_pos: NDArray,
+        target_yaw: float,
+        dt: float,
+        prev_target_pos: NDArray | None = None,
+    ) -> NDArray:
+        """Compatibility interface shared with the legacy controllers."""
+        velocity = np.zeros(3)
+        if prev_target_pos is not None:
+            velocity = (np.asarray(target_pos) - prev_target_pos) / dt
+        output = self.compute_output(
+            TrajectorySetpoint(
+                position=np.asarray(target_pos), velocity=velocity, yaw=target_yaw
+            ),
+            dt,
+        )
+        return output.motor_speeds

drones_sim/control/lqr.py

@@ -0,0 +1,174 @@
+"""LQR controller for quadcopter stabilization.
+
+Linearizes the plant around hover and solves the continuous-time Algebraic
+Riccati Equation to obtain the full-state feedback gain K.
+
+Control law:  u_delta = -K @ x_error
+Wrench:       [T, tau_phi, tau_theta, tau_psi] = hover_wrench + u_delta
+Motors:       allocation_matrix inverse maps wrench -> motor speeds
+"""
+
+from __future__ import annotations
+
+import numpy as np
+from numpy.typing import NDArray
+from scipy.linalg import solve_continuous_are
+
+from ..dynamics.quadcopter import QuadcopterDynamics
+from .allocation import ControlAllocator
+
+
+class LQRController:
+    """Full-state feedback LQR for quadcopter position and attitude control.
+
+    The linearization is computed once at construction around the hover trim
+    point (phi=theta=0, T=mg).  The resulting gain K is constant (offline
+    optimal).
+
+    Interface matches QuadcopterController.compute() so both can be used
+    interchangeably in simulation loops:
+
+        motors = lqr.compute(target_pos, target_yaw, dt, prev_target_pos)
+
+    Parameters
+    ----------
+    quad:
+        Live QuadcopterDynamics instance (reads state each step).
+    Q:
+        12x12 state-error penalty matrix.  Defaults to a diagonal matrix
+        that penalizes position and attitude tracking most.
+    R:
+        4x4 control-effort penalty matrix for [T, tau_phi, tau_theta, tau_psi].
+    """
+
+    def __init__(
+        self,
+        quad: QuadcopterDynamics,
+        Q: NDArray | None = None,
+        R: NDArray | None = None,
+    ) -> None:
+        self.quad = quad
+        self.K = self._compute_gain(Q, R)
+
+    # ------------------------------------------------------------------
+    # Hover linearization
+    # ------------------------------------------------------------------
+
+    def _linearize_hover(self) -> tuple[NDArray, NDArray]:
+        """Return (A, B) continuous-time hover linearization matrices."""
+        m = self.quad.mass
+        g = self.quad.g
+        k_d = self.quad.k_d
+        Ix, Iy, Iz = np.diag(self.quad.I)
+
+        # State: [x, y, z, vx, vy, vz, phi, theta, psi, p, q, r]
+        A = np.zeros((12, 12))
+        # Position kinematics: dp/dt = v
+        A[0, 3] = 1.0
+        A[1, 4] = 1.0
+        A[2, 5] = 1.0
+        # Velocity: aero drag + gravity coupling through small-angle attitude
+        A[3, 3] = -k_d / m
+        A[3, 7] = g          # d(vx)/d(theta)  (pitch -> forward accel)
+        A[4, 4] = -k_d / m
+        A[4, 6] = -g         # d(vy)/d(phi)    (roll  -> lateral accel)
+        A[5, 5] = -k_d / m
+        # Euler angle kinematics (linearized, small angles)
+        A[6, 9]  = 1.0       # dphi/dt   = p
+        A[7, 10] = 1.0       # dtheta/dt = q
+        A[8, 11] = 1.0       # dpsi/dt   = r
+
+        # Input: [T, tau_phi, tau_theta, tau_psi]
+        B = np.zeros((12, 4))
+        B[5, 0]  = 1.0 / m   # d(vz)/dT
+        B[9, 1]  = 1.0 / Ix  # d(p)/d(tau_phi)
+        B[10, 2] = 1.0 / Iy  # d(q)/d(tau_theta)
+        B[11, 3] = 1.0 / Iz  # d(r)/d(tau_psi)
+
+        return A, B
+
+    def _compute_gain(self, Q: NDArray | None, R: NDArray | None) -> NDArray:
+        """Solve ARE and return gain K = R^{-1} B^T P."""
+        A, B = self._linearize_hover()
+
+        if Q is None:
+            q_diag = np.array([
+                10.0, 10.0, 12.0,  # position  x, y, z
+                2.0,  2.0,  4.0,   # velocity  vx, vy, vz
+                5.0,  5.0,  3.0,   # attitude  phi, theta, psi
+                0.5,  0.5,  0.3,   # body rate p, q, r
+            ])
+            Q = np.diag(q_diag)
+
+        if R is None:
+            R = np.diag([0.01, 50.0, 50.0, 20.0])  # [T, tau_phi, tau_theta, tau_psi]
+
+        P = solve_continuous_are(A, B, Q, R)
+        return np.linalg.inv(R) @ B.T @ P
+
+    # ------------------------------------------------------------------
+    # Runtime interface
+    # ------------------------------------------------------------------
+
+    def reset(self) -> None:
+        """No integrators; nothing to reset."""
+
+    def compute(
+        self,
+        target_pos: NDArray,
+        target_yaw: float,
+        dt: float,
+        prev_target_pos: NDArray | None = None,
+    ) -> NDArray:
+        """Compute motor speeds for a given position and yaw target.
+
+        Parameters
+        ----------
+        target_pos:
+            Desired 3-D position [x, y, z] in world frame.
+        target_yaw:
+            Desired yaw angle (rad).
+        dt:
+            Time step (unused by LQR; kept for interface compatibility).
+        prev_target_pos:
+            Previous target (unused; kept for interface compatibility).
+
+        Returns
+        -------
+        motor_speeds: NDArray shape (4,)
+            Commanded rotor speeds in rad/s.
+        """
+        m  = self.quad.mass
+        g  = self.quad.g
+
+        pos   = self.quad.get_position()
+        vel   = self.quad.get_velocity()
+        att   = self.quad.get_attitude()    # [phi, theta, psi]
+        omega = self.quad.get_angular_velocity()  # [p, q, r]
+
+        # Error w.r.t. target (target velocity / rate = 0)
+        x_err = np.concatenate([
+            pos - target_pos,
+            vel,
+            att - np.array([0.0, 0.0, target_yaw]),
+            omega,
+        ])
+        # Wrap yaw error to [-pi, pi]
+        x_err[8] = (x_err[8] + np.pi) % (2.0 * np.pi) - np.pi
+
+        # LQR feedback: delta wrench
+        u_delta = -self.K @ x_err  # [delta_T, tau_phi, tau_theta, tau_psi]
+
+        # Absolute thrust with hover compensation
+        T = float(np.clip(m * g + u_delta[0], 0.2 * m * g, 2.0 * m * g))
+        tau_max = np.array([0.25, 0.25, 0.08])
+        torques = np.clip(u_delta[1:], -tau_max, tau_max)
+
+        wrench = np.array([T, torques[0], torques[1], torques[2]])
+
+        allocator = ControlAllocator(
+            self.quad.allocation_matrix(),
+            min_speed=self.quad.min_motor_speed,
+            max_speed=self.quad.max_motor_speed,
+        )
+        return allocator.allocate(wrench).motor_speeds