first_principles

Full rigid-body physics model for a quadrotor.

This package implements Newton-Euler dynamics based on physical constants — mass, inertia, motor thrust and torque curves, arm length, and drag coefficients. The command interface is four motor angular velocities in RPM. No data fitting is required; all parameters are measurable physical quantities.

Motor forces and torques are quadratic polynomials in RPM:

\[ f_{p,i} = k_0 + k_1 \Omega_i + k_2 \Omega_i^2, \qquad \tau_{p,i} = m_0 + m_1 \Omega_i + m_2 \Omega_i^2. \]

When rotor dynamics are modelled, each motor RPM evolves as:

\[ \dot{\Omega}_i = \begin{cases} c_1 (\Omega_{\mathrm{cmd},i} - \Omega_i) + c_2 (\Omega_{\mathrm{cmd},i}^2 - \Omega_i^2) & \Omega_{\mathrm{cmd},i} \geq \Omega_i \\[4pt] c_3 (\Omega_{\mathrm{cmd},i} - \Omega_i) + c_4 (\Omega_{\mathrm{cmd},i}^2 - \Omega_i^2) & \Omega_{\mathrm{cmd},i} < \Omega_i \end{cases} \]

The rigid-body equations of motion are:

\[ \begin{aligned} \dot{\mathbf{p}} &= \mathbf{v}, \\ \dot{\mathbf{q}} &= \tfrac{1}{2} \begin{bmatrix}0 \\ {}^{\mathcal{B}}\boldsymbol{\omega}\end{bmatrix} \otimes \mathbf{q}, \\ m\dot{\mathbf{v}} &= m\mathbf{g} + R\,{}^{\mathcal{B}}\mathbf{f}_t + R\,{}^{\mathcal{B}}\mathbf{f}_a, \\ \mathbf{J}\,{}^{\mathcal{B}}\dot{\boldsymbol{\omega}} &= {}^{\mathcal{B}}\mathbf{t}_\Sigma - {}^{\mathcal{B}}\boldsymbol{\omega} \times \mathbf{J}\,{}^{\mathcal{B}}\boldsymbol{\omega}, \end{aligned} \]

where \(R = {}^{\mathcal{I}}R_{\mathcal{B}}(\mathbf{q})\) is the rotation from body to world frame, and the forces and torques are:

\[ \begin{aligned} {}^{\mathcal{B}}\mathbf{f}_t &= \mathbf{e}_z \textstyle\sum_{i=1}^{4} f_{p,i}, \\ {}^{\mathcal{B}}\mathbf{f}_a &= D_b\,R^{\top}\mathbf{v}, \\ {}^{\mathcal{B}}\mathbf{t}_\Sigma &= {}^{\mathcal{B}}\mathbf{t}_t + {}^{\mathcal{B}}\mathbf{t}_d + {}^{\mathcal{B}}\mathbf{t}_i, \end{aligned} \]

with:

\[ \begin{aligned} {}^{\mathcal{B}}\mathbf{t}_t &= \frac{l}{\sqrt{2}} \begin{bmatrix}1&0&0\\0&1&0\\0&0&0\end{bmatrix} M\,\mathbf{f}_p, \\ {}^{\mathcal{B}}\mathbf{t}_d &= \begin{bmatrix}0&0&0\\0&0&0\\0&0&1\end{bmatrix} M\,\boldsymbol{\tau}_p, \\ {}^{\mathcal{B}}\mathbf{t}_i &= J_p \begin{bmatrix} -{}^{\mathcal{B}}\omega_y\;\mathbf{m}_z^{\top}\boldsymbol{\Omega} \\ -{}^{\mathcal{B}}\omega_x\;\mathbf{m}_z^{\top}\boldsymbol{\Omega} \\ \mathbf{m}_z^{\top}\dot{\boldsymbol{\Omega}} \end{bmatrix}, \end{aligned} \]

where \(D_b\) is the body-frame drag matrix, \(l\) is the motor arm length, \(J_p\) is the propeller moment of inertia, \(M\) is the \(3\times 4\) mixing matrix, and \(\mathbf{m}_z\) is its last row.

dynamics(pos, quat, vel, ang_vel, cmd, rotor_vel=None, dist_f=None, dist_t=None, *, mass, L, prop_inertia, gravity_vec, J, J_inv, rpm2thrust, rpm2torque, mixing_matrix, drag_matrix, rotor_dyn_coef)

First principles model for a quatrotor.

The command is four motor angular velocities in RPM. Forces and torques are computed internally using quadratic thrust and torque curves, the mixing matrix, and the motor arm length.

Based on the quaternion model from https://www.dynsyslab.org/wp-content/papercite-data/pdf/mckinnon-robot20.pdf

Parameters:

Name	Type	Description	Default
pos	Array	Position of the drone (m).	required
quat	Array	Quaternion of the drone (xyzw).	required
vel	Array	Velocity of the drone (m/s).	required
ang_vel	Array	Angular velocity of the drone (rad/s).	required
cmd	Array	Motor speeds (RPMs).	required
rotor_vel	Array | None	Angular velocity of the 4 motors (RPMs). If None, the commanded thrust is directly applied. If value is given, thrust dynamics are calculated.	None
dist_f	Array | None	Disturbance force (N) in the world frame acting on the CoM.	None
dist_t	Array | None	Disturbance torque (Nm) in the world frame acting on the CoM.	None
mass	float	Mass of the drone (kg).	required
L	float	Distance from the CoM to the motor (m).	required
prop_inertia	float	Inertia of one propeller in z direction (kg m^2).	required
gravity_vec	Array	Gravity vector (m/s^2). We assume the gravity vector points downwards, e.g. [0, 0, -9.81].	required
J	Array	Inertia matrix (kg m^2).	required
J_inv	Array	Inverse inertia matrix (1/kg m^2).	required
rpm2thrust	Array	Propeller force constant (N min^2).	required
rpm2torque	Array	Propeller torque constant (Nm min^2).	required
mixing_matrix	Array	Mixing matrix denoting the turn direction of the motors (4x3).	required
drag_matrix	Array	Drag matrix containing the linear drag coefficients (3x3).	required
rotor_dyn_coef	Array	Rotor dynamics coefficients.	required

Warning

Do not use quat_dot directly for integration! Only usage of ang_vel is mathematically correct. If you still decide to use quat_dot to integrate, ensure unit length! More information: https://ahrs.readthedocs.io/en/latest/filters/angular.html

Source code in drone_models/first_principles/model.py

@supports(rotor_dynamics=True)
def dynamics(
    pos: Array,
    quat: Array,
    vel: Array,
    ang_vel: Array,
    cmd: Array,
    rotor_vel: Array | None = None,
    dist_f: Array | None = None,
    dist_t: Array | None = None,
    *,
    mass: float,
    L: float,
    prop_inertia: float,
    gravity_vec: Array,
    J: Array,
    J_inv: Array,
    rpm2thrust: Array,
    rpm2torque: Array,
    mixing_matrix: Array,
    drag_matrix: Array,
    rotor_dyn_coef: Array,
) -> tuple[Array, Array, Array, Array, Array | None]:
    r"""First principles model for a quatrotor.

    The command is four motor angular velocities in RPM. Forces and torques are
    computed internally using quadratic thrust and torque curves, the mixing matrix,
    and the motor arm length.

    Based on the quaternion model from <https://www.dynsyslab.org/wp-content/papercite-data/pdf/mckinnon-robot20.pdf>

    Args:
        pos: Position of the drone (m).
        quat: Quaternion of the drone (xyzw).
        vel: Velocity of the drone (m/s).
        ang_vel: Angular velocity of the drone (rad/s).
        cmd: Motor speeds (RPMs).
        rotor_vel: Angular velocity of the 4 motors (RPMs). If None, the commanded thrust is
            directly applied. If value is given, thrust dynamics are calculated.
        dist_f: Disturbance force (N) in the world frame acting on the CoM.
        dist_t: Disturbance torque (Nm) in the world frame acting on the CoM.

        mass: Mass of the drone (kg).
        L: Distance from the CoM to the motor (m).
        prop_inertia: Inertia of one propeller in z direction (kg m^2).
        gravity_vec: Gravity vector (m/s^2). We assume the gravity vector points downwards, e.g.
            [0, 0, -9.81].
        J: Inertia matrix (kg m^2).
        J_inv: Inverse inertia matrix (1/kg m^2).
        rpm2thrust: Propeller force constant (N min^2).
        rpm2torque: Propeller torque constant (Nm min^2).
        mixing_matrix: Mixing matrix denoting the turn direction of the motors (4x3).
        drag_matrix: Drag matrix containing the linear drag coefficients (3x3).
        rotor_dyn_coef: Rotor dynamics coefficients.


    Warning:
        Do not use quat_dot directly for integration! Only usage of ang_vel is mathematically correct.
        If you still decide to use quat_dot to integrate, ensure unit length!
        More information: <https://ahrs.readthedocs.io/en/latest/filters/angular.html>
    """
    xp = array_namespace(pos)
    mass, L, prop_inertia, gravity_vec, rpm2thrust, rpm2torque = to_xp(
        mass, L, prop_inertia, gravity_vec, rpm2thrust, rpm2torque, xp=xp, device=device(pos)
    )
    J, J_inv, mixing_matrix, rotor_dyn_coef, drag_matrix = to_xp(
        J, J_inv, mixing_matrix, rotor_dyn_coef, drag_matrix, xp=xp, device=device(pos)
    )
    rot = R.from_quat(quat)  # from body to world
    rot_mat = rot.inv().as_matrix()  # from world to body
    # Rotor dynamics
    if rotor_vel is None:
        rotor_vel, rotor_vel_dot = cmd, None
    else:
        rotor_vel_dot = xp.where(
            cmd > rotor_vel,
            rotor_dyn_coef[0] * (cmd - rotor_vel) + rotor_dyn_coef[1] * (cmd**2 - rotor_vel**2),
            rotor_dyn_coef[2] * (cmd - rotor_vel) + rotor_dyn_coef[3] * (cmd**2 - rotor_vel**2),
        )
    # Creating force and torque vector
    forces_motor = rpm2thrust[0] + rpm2thrust[1] * rotor_vel + rpm2thrust[2] * rotor_vel**2
    forces_motor_tot = xp.sum(forces_motor, axis=-1)
    zeros = xp.zeros_like(forces_motor_tot)
    forces_motor_vec = xp.stack((zeros, zeros, forces_motor_tot), axis=-1)
    forces_motor_vec_world = rot.apply(forces_motor_vec)
    force_gravity = gravity_vec * mass
    force_drag = (rot_mat.mT @ (drag_matrix @ (rot_mat @ vel[..., None])))[..., 0]

    torques_motor = rpm2torque[0] + rpm2torque[1] * rotor_vel + rpm2torque[2] * rotor_vel**2
    torque_thrust = (mixing_matrix @ (forces_motor)[..., None])[..., 0] * xp.stack(
        [L, L, xp.asarray(0.0)]
    )
    torque_drag = (mixing_matrix @ (torques_motor)[..., None])[..., 0] * xp.stack(
        [xp.asarray(0.0), xp.asarray(0.0), xp.asarray(1.0)]
    )
    # convert rotor speed from RPM to rad/s for physical calculations
    rpm_to_rad = 2 * xp.pi / 60
    rotor_vel_rads = rotor_vel * rpm_to_rad
    rotor_vel_dot_rads = (
        rotor_vel_dot * rpm_to_rad if rotor_vel_dot is not None else xp.zeros_like(rotor_vel)
    )
    torque_inertia = prop_inertia * xp.stack(
        [
            -ang_vel[..., 1] * xp.sum(mixing_matrix[..., -1, :] * rotor_vel_rads, axis=-1),
            -ang_vel[..., 0] * xp.sum(mixing_matrix[..., -1, :] * rotor_vel_rads, axis=-1),
            xp.sum(mixing_matrix[..., -1, :] * rotor_vel_dot_rads, axis=-1),
        ],
        axis=-1,
    )
    torque_vec = torque_thrust + torque_drag + torque_inertia

    # Linear equation of motion
    forces_sum = forces_motor_vec_world + force_gravity + force_drag
    if dist_f is not None:
        forces_sum = forces_sum + dist_f

    pos_dot = vel
    vel_dot = forces_sum / mass

    # Rotational equation of motion
    if dist_t is not None:
        torque_vec = torque_vec + rot.apply(dist_t, inverse=True)
    quat_dot = rotation.ang_vel2quat_dot(quat, ang_vel)
    torque_vec = torque_vec - xp.linalg.cross(ang_vel, (J @ ang_vel[..., None])[..., 0])
    ang_vel_dot = (J_inv @ torque_vec[..., None])[..., 0]
    return pos_dot, quat_dot, vel_dot, ang_vel_dot, rotor_vel_dot

symbolic_dynamics(model_rotor_vel=True, model_dist_f=False, model_dist_t=False, *, mass, L, prop_inertia, gravity_vec, J, J_inv, rpm2thrust, rpm2torque, mixing_matrix, rotor_dyn_coef, drag_matrix)

Return CasADi symbolic expressions for the first-principles model.

Implements the same dynamics as dynamics using CasADi MX symbolic expressions, validated to be numerically equivalent.

Parameters:

Name	Type	Description	Default
model_rotor_vel	bool	If True, the four motor RPM states are included in X and rotor dynamics are modelled. If False, the commanded RPMs are used directly. Defaults to True.	True
model_dist_f	bool	If True, a 3-D force disturbance is appended to X.	False
model_dist_t	bool	If True, a 3-D torque disturbance is appended to X.	False
mass	float	Drone mass in kg.	required
L	float	Distance from centre of mass to motor in metres.	required
prop_inertia	float	Moment of inertia of one propeller about its spin axis in kg m².	required
gravity_vec	Array	Gravity vector, shape (3,).	required
J	Array	Inertia matrix, shape (3, 3).	required
J_inv	Array	Inverse inertia matrix, shape (3, 3).	required
rpm2thrust	Array	Polynomial coefficients [a, b, c] for the thrust curve f = a + b * rpm + c * rpm².	required
rpm2torque	Array	Polynomial coefficients [a, b, c] for the drag-torque curve τ = a + b * rpm + c * rpm².	required
mixing_matrix	Array	Matrix of shape (3, 4) mapping per-motor forces to body torques.	required
rotor_dyn_coef	Array	Four rotor dynamics coefficients [k_acc1, k_acc2, k_dec1, k_dec2] used in the piecewise-linear spin-up/down model.	required
drag_matrix	Array	Diagonal (3, 3) matrix of linear drag coefficients.	required

Returns:

Type	Description
MX	Tuple (X_dot, X, U, Y) of CasADi MX expressions:
MX	X_dot: State derivative, length 17 when model_rotor_vel=True (13 otherwise), plus 3 per enabled disturbance.
MX	X: State vector [pos(3), quat(4), vel(3), ang_vel(3)], with rotor_vel(4) appended if model_rotor_vel=True.
MX	U: Input vector [rpm_1, rpm_2, rpm_3, rpm_4].
tuple[MX, MX, MX, MX]	Y: Output [pos(3), quat(4)].

Source code in drone_models/first_principles/model.py

def symbolic_dynamics(
    model_rotor_vel: bool = True,
    model_dist_f: bool = False,
    model_dist_t: bool = False,
    *,
    mass: float,
    L: float,
    prop_inertia: float,
    gravity_vec: Array,
    J: Array,
    J_inv: Array,
    rpm2thrust: Array,
    rpm2torque: Array,
    mixing_matrix: Array,
    rotor_dyn_coef: Array,
    drag_matrix: Array,
) -> tuple[cs.MX, cs.MX, cs.MX, cs.MX]:
    """Return CasADi symbolic expressions for the first-principles model.

    Implements the same dynamics as [dynamics][drone_models.first_principles.dynamics] using CasADi ``MX``
    symbolic expressions, validated to be numerically equivalent.

    Args:
        model_rotor_vel: If ``True``, the four motor RPM states are included in
            ``X`` and rotor dynamics are modelled.  If ``False``, the commanded
            RPMs are used directly.  Defaults to ``True``.
        model_dist_f: If ``True``, a 3-D force disturbance is appended to ``X``.
        model_dist_t: If ``True``, a 3-D torque disturbance is appended to ``X``.
        mass: Drone mass in kg.
        L: Distance from centre of mass to motor in metres.
        prop_inertia: Moment of inertia of one propeller about its spin axis
            in kg m².
        gravity_vec: Gravity vector, shape ``(3,)``.
        J: Inertia matrix, shape ``(3, 3)``.
        J_inv: Inverse inertia matrix, shape ``(3, 3)``.
        rpm2thrust: Polynomial coefficients ``[a, b, c]`` for the thrust curve
            ``f = a + b * rpm + c * rpm²``.
        rpm2torque: Polynomial coefficients ``[a, b, c]`` for the drag-torque
            curve ``τ = a + b * rpm + c * rpm²``.
        mixing_matrix: Matrix of shape ``(3, 4)`` mapping per-motor forces to
            body torques.
        rotor_dyn_coef: Four rotor dynamics coefficients ``[k_acc1, k_acc2,
            k_dec1, k_dec2]`` used in the piecewise-linear spin-up/down model.
        drag_matrix: Diagonal ``(3, 3)`` matrix of linear drag coefficients.

    Returns:
        Tuple ``(X_dot, X, U, Y)`` of CasADi ``MX`` expressions:

        * ``X_dot``: State derivative, length 17 when ``model_rotor_vel=True``
          (13 otherwise), plus 3 per enabled disturbance.
        * ``X``: State vector ``[pos(3), quat(4), vel(3), ang_vel(3)]``, with
          ``rotor_vel(4)`` appended if ``model_rotor_vel=True``.
        * ``U``: Input vector ``[rpm_1, rpm_2, rpm_3, rpm_4]``.
        * ``Y``: Output ``[pos(3), quat(4)]``.
    """
    # States and Inputs
    X = cs.vertcat(symbols.pos, symbols.quat, symbols.vel, symbols.ang_vel)
    if model_rotor_vel:
        X = cs.vertcat(X, symbols.rotor_vel)
    if model_dist_f:
        X = cs.vertcat(X, symbols.dist_f)
    if model_dist_t:
        X = cs.vertcat(X, symbols.dist_t)
    U = symbols.cmd_rotor_vel

    # Defining the dynamics function
    if model_rotor_vel:
        # Rotor dynamics
        rotor_vel_dot = cs.if_else(
            U > symbols.rotor_vel,
            rotor_dyn_coef[0] * (U - symbols.rotor_vel)
            + rotor_dyn_coef[1] * (U**2 - symbols.rotor_vel**2),
            rotor_dyn_coef[2] * (U - symbols.rotor_vel)
            + rotor_dyn_coef[3] * (U**2 - symbols.rotor_vel**2),
        )
    else:
        _saved_rotor_vel = symbols.rotor_vel
        symbols.rotor_vel = U
    # Creating force and torque vector
    forces_motor = (
        rpm2thrust[0] + rpm2thrust[1] * symbols.rotor_vel + rpm2thrust[2] * symbols.rotor_vel**2
    )
    forces_motor_vec = cs.vertcat(0.0, 0.0, cs.sum1(forces_motor))
    forces_motor_vec_world = symbols.rot @ forces_motor_vec
    force_gravity = gravity_vec * mass
    force_drag = symbols.rot @ (drag_matrix @ (symbols.rot.T @ symbols.vel))

    torques_motor = (
        rpm2torque[0] + rpm2torque[1] * symbols.rotor_vel + rpm2torque[2] * symbols.rotor_vel**2
    )
    torques_thrust = mixing_matrix @ forces_motor * cs.vertcat(L, L, 0.0)
    torques_drag = mixing_matrix @ torques_motor * cs.vertcat(0.0, 0.0, 1.0)
    # convert rotor speed from RPM to rad/s for physical calculations
    rpm_to_rad = 2 * cs.pi / 60
    rotor_vel_rads = symbols.rotor_vel * rpm_to_rad
    rotor_vel_dot_rads = rotor_vel_dot * rpm_to_rad if model_rotor_vel else symbols.rotor_vel * 0.0
    torque_inertia = prop_inertia * cs.vertcat(
        -symbols.ang_vel[1] * cs.sum(mixing_matrix[-1, :] * rotor_vel_rads),
        -symbols.ang_vel[0] * cs.sum(mixing_matrix[-1, :] * rotor_vel_rads),
        cs.sum(mixing_matrix[-1, :] * rotor_vel_dot_rads),
    )
    torques_motor_vec = torques_thrust + torques_drag + torque_inertia

    # Linear equation of motion
    forces_sum = forces_motor_vec_world + force_gravity + force_drag
    if model_dist_f:
        forces_sum = forces_sum + symbols.dist_f

    pos_dot = symbols.vel
    vel_dot = forces_sum / mass

    # Rotational equation of motion
    xi = cs.vertcat(
        cs.horzcat(0, -symbols.ang_vel.T), cs.horzcat(symbols.ang_vel, -cs.skew(symbols.ang_vel))
    )
    quat_dot = 0.5 * (xi @ symbols.quat)
    torques_sum = torques_motor_vec
    if model_dist_t:
        torques_sum = torques_sum + symbols.rot.T @ symbols.dist_t
    ang_vel_dot = J_inv @ (torques_sum - cs.cross(symbols.ang_vel, J @ symbols.ang_vel))

    if model_rotor_vel:
        X_dot = cs.vertcat(pos_dot, quat_dot, vel_dot, ang_vel_dot, rotor_vel_dot)
    else:
        X_dot = cs.vertcat(pos_dot, quat_dot, vel_dot, ang_vel_dot)
    Y = cs.vertcat(symbols.pos, symbols.quat)

    if not model_rotor_vel:
        symbols.rotor_vel = _saved_rotor_vel
    return X_dot, X, U, Y