dynamics.so_rpy_rotor

crazyflow.dynamics.so_rpy_rotor

Second-order fitted RPY dynamics with first-order thrust dynamics.

Extends so_rpy by adding a scalar thrust state \(f_\Sigma\) that captures motor spin-up and spin-down with a first-order lag. Rotational dynamics remain a fitted second-order linear system driven by RPY commands. The command interface is [roll_rad, pitch_rad, yaw_rad, thrust_N]. The rotor_vel state is the current thrust in Newtons (not motor RPMs), carried as four entries of which only the first enters the dynamics.

\[ \begin{aligned} \dot{\mathbf{p}} &= \mathbf{v}, \\ m\dot{\mathbf{v}} &= \mathbf{f}_\mathrm{g} + \mathbf{R}\,\mathbf{e}_\mathrm{z} (c_\mathrm{acc} + c_\mathrm{f} f_\Sigma), \\ \dot{f}_\Sigma &= c_\tau (f_{\Sigma,\mathrm{cmd}} - f_\Sigma), \\ \ddot{\boldsymbol{\Psi}} &= \boldsymbol{c}_{\boldsymbol{\Psi},1}\,\boldsymbol{\Psi} + \boldsymbol{c}_{\boldsymbol{\Psi},2}\,\dot{\boldsymbol{\Psi}} + \boldsymbol{c}_{\boldsymbol{\Psi},3}\,\boldsymbol{\Psi}_\mathrm{cmd}, \end{aligned} \]

where \(\mathbf{f}_\mathrm{g} = m\mathbf{g}\).

This is the native Euler-angle form. For how the simulation integrates this state in quaternion + angular velocity coordinates, see so_rpy.

Variable	Name	Description
\(\mathbf{p}\)	pos	Position in m
\(\mathbf{v}\)	vel	Velocity in m/s
\(\boldsymbol{\Psi} = [\phi,\theta,\psi]^{\top}\)		Roll, pitch, and yaw in rad, from quat
\(\dot{\boldsymbol{\Psi}}\)		Roll, pitch, and yaw rates in rad/s, from ang_vel
\(f_\Sigma\)	rotor_vel[0]	Collective thrust in N
\(\boldsymbol{\Psi}_\mathrm{cmd}\)	cmd[:3]	Commanded roll, pitch, and yaw in rad
\(f_{\Sigma,\mathrm{cmd}}\)	cmd[3]	Commanded collective thrust in N
\(\mathbf{f}_\mathrm{g}\)		Gravitational force
\(\mathbf{R}\)		Rotation from body to world frame
\(\mathbf{e}_\mathrm{z}\)		Unit vector in z direction

Parameter	Name	Description
\(m\)	mass	Mass in kg
\(\mathbf{g}\)	gravity_vec	Gravity vector in m/s²
\(c_\tau\)	thrust_dyn_coef	Thrust dynamics coefficient in 1/s
\(c_\mathrm{acc}\)	acc_coef	Thrust offset
\(c_\mathrm{f}\)	cmd_f_coef	Thrust scaling coefficient
\(\boldsymbol{c}_{\boldsymbol{\Psi},1}\)	rpy_coef	Rotational dynamics coefficients
\(\boldsymbol{c}_{\boldsymbol{\Psi},2}\)	rpy_rates_coef	Rotational dynamics coefficients
\(\boldsymbol{c}_{\boldsymbol{\Psi},3}\)	cmd_rpy_coef	Rotational dynamics coefficients

Classes

Params

Attributes

J = field(metadata={CORE_NDIM_KEY: 2}) class-attribute instance-attribute

Inertia matrix of the drone.

J_inv = field(metadata={CORE_NDIM_KEY: 2}) class-attribute instance-attribute

Inverse of the inertia matrix of the drone.

acc_coef = field(metadata={CORE_NDIM_KEY: 1}) class-attribute instance-attribute

Thrust offset.

cmd_f_coef = field(metadata={CORE_NDIM_KEY: 1}) class-attribute instance-attribute

Thrust scaling coefficient.

cmd_rpy_coef = field(metadata={CORE_NDIM_KEY: 1}) class-attribute instance-attribute

Rotational dynamics coefficients of the commanded roll, pitch, and yaw.

gravity_vec = field(metadata={CORE_NDIM_KEY: 1}) class-attribute instance-attribute

Gravity vector of the drone.

mass = field(metadata={CORE_NDIM_KEY: 1}) class-attribute instance-attribute

Mass of the drone.

rpy_coef = field(metadata={CORE_NDIM_KEY: 1}) class-attribute instance-attribute

Rotational dynamics coefficients of the roll, pitch, and yaw angles.

rpy_rates_coef = field(metadata={CORE_NDIM_KEY: 1}) class-attribute instance-attribute

Rotational dynamics coefficients of the roll, pitch, and yaw rates.

thrust_dyn_coef = field(metadata={CORE_NDIM_KEY: 1}) class-attribute instance-attribute

Thrust dynamics coefficient.

Methods:

create(drone, device) staticmethod

Create the default parameters for the simulation.

Source code in crazyflow/dynamics/so_rpy_rotor/dynamics.py

@staticmethod
def create(drone: str, device: Device) -> Params:
    """Create the default parameters for the simulation."""
    p = load_fn_params(dynamics, drone)
    J = jnp.asarray(p["J"], device=device)
    return Params(
        mass=jnp.asarray([p["mass"]], device=device),
        gravity_vec=jnp.asarray(p["gravity_vec"], device=device),
        J=J,
        J_inv=jnp.linalg.inv(J),
        thrust_dyn_coef=jnp.asarray([p["thrust_dyn_coef"]], device=device),
        acc_coef=jnp.asarray([p["acc_coef"]], device=device),
        cmd_f_coef=jnp.asarray([p["cmd_f_coef"]], device=device),
        rpy_coef=jnp.asarray(p["rpy_coef"], device=device),
        rpy_rates_coef=jnp.asarray(p["rpy_rates_coef"], device=device),
        cmd_rpy_coef=jnp.asarray(p["cmd_rpy_coef"], device=device),
    )

Functions:

symbolic_dynamics(model_rotor_vel=True, model_dist_f=False, model_dist_t=False, *, mass, gravity_vec, J, J_inv, thrust_dyn_coef, acc_coef, cmd_f_coef, rpy_coef, rpy_rates_coef, cmd_rpy_coef)

Return CasADi symbolic expressions for the so_rpy_rotor dynamics in quaternion form.

Internally delegates to symbolic_dynamics_euler and converts the Euler-angle state to quaternion + angular-velocity state so that the interface matches that of symbolic_dynamics.

Parameters:

Name	Type	Description	Default
model_rotor_vel	bool	If True, the thrust state is included in X and first-order thrust dynamics are modelled. 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
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
thrust_dyn_coef	Array	Thrust dynamics coefficient in 1/s.	required
acc_coef	Array	Thrust offset in N.	required
cmd_f_coef	Array	Thrust scaling coefficient.	required
rpy_coef	Array	Rotational dynamics coefficients of the roll, pitch, and yaw angles in 1/s², shape (3,).	required
rpy_rates_coef	Array	Rotational dynamics coefficients of the roll, pitch, and yaw rates in 1/s, shape (3,).	required
cmd_rpy_coef	Array	Rotational dynamics coefficients of the commanded roll, pitch, and yaw in 1/s², shape (3,).	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. Note that rotor_vel here represents the thrust state in Newtons, and that only its first entry enters the dynamics.
MX	U: Input vector [roll_rad, pitch_rad, yaw_rad, thrust_N].
tuple[MX, MX, MX, MX]	Y: Output [pos(3), quat(4)].

Source code in crazyflow/dynamics/so_rpy_rotor/dynamics.py

def symbolic_dynamics(
    model_rotor_vel: bool = True,
    model_dist_f: bool = False,
    model_dist_t: bool = False,
    *,
    mass: float,
    gravity_vec: Array,
    J: Array,
    J_inv: Array,
    thrust_dyn_coef: Array,
    acc_coef: Array,
    cmd_f_coef: Array,
    rpy_coef: Array,
    rpy_rates_coef: Array,
    cmd_rpy_coef: Array,
) -> tuple[cs.MX, cs.MX, cs.MX, cs.MX]:
    """Return CasADi symbolic expressions for the so_rpy_rotor dynamics in quaternion form.

    Internally delegates to
    [symbolic_dynamics_euler][crazyflow.dynamics.so_rpy_rotor.symbolic_dynamics_euler] and converts
    the Euler-angle state to quaternion + angular-velocity state so that the interface matches that
    of [symbolic_dynamics][crazyflow.dynamics.first_principles.symbolic_dynamics].

    Args:
        model_rotor_vel: If ``True``, the thrust state is included in ``X`` and first-order thrust
            dynamics are modelled. 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.
        gravity_vec: Gravity vector, shape ``(3,)``.
        J: Inertia matrix, shape ``(3, 3)``.
        J_inv: Inverse inertia matrix, shape ``(3, 3)``.
        thrust_dyn_coef: Thrust dynamics coefficient in 1/s.
        acc_coef: Thrust offset in N.
        cmd_f_coef: Thrust scaling coefficient.
        rpy_coef: Rotational dynamics coefficients of the roll, pitch, and yaw angles in 1/s², shape
            ``(3,)``.
        rpy_rates_coef: Rotational dynamics coefficients of the roll, pitch, and yaw rates in 1/s,
            shape ``(3,)``.
        cmd_rpy_coef: Rotational dynamics coefficients of the commanded roll, pitch, and yaw in
            1/s², shape ``(3,)``.

    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``. Note that ``rotor_vel`` here represents the thrust
            state in Newtons, and that only its first entry enters the dynamics.
        * ``U``: Input vector ``[roll_rad, pitch_rad, yaw_rad, thrust_N]``.
        * ``Y``: Output ``[pos(3), quat(4)]``.
    """
    ## We need to set the rpy and drpy symbols before building the euler dynamics
    _saved_rpy = symbols.rpy
    _saved_drpy = symbols.drpy
    _rpy_quat = rotation.cs_quat2euler(symbols.quat)
    _drpy_quat = rotation.cs_ang_vel2rpy_rates(symbols.quat, symbols.ang_vel)
    symbols.rpy = _rpy_quat
    symbols.drpy = _drpy_quat
    X_dot_euler, X_euler, U_euler, Y_euler = symbolic_dynamics_euler(
        model_rotor_vel=model_rotor_vel,
        mass=mass,
        gravity_vec=gravity_vec,
        J=J,
        J_inv=J_inv,
        thrust_dyn_coef=thrust_dyn_coef,
        acc_coef=acc_coef,
        cmd_f_coef=cmd_f_coef,
        rpy_coef=rpy_coef,
        rpy_rates_coef=rpy_rates_coef,
        cmd_rpy_coef=cmd_rpy_coef,
    )
    symbols.rpy = _saved_rpy
    symbols.drpy = _saved_drpy

    # 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 = U_euler

    # Linear equation of motion
    pos_dot = X_dot_euler[0:3]
    vel_dot = X_dot_euler[6:9]
    if model_dist_f:
        # Adding force disturbances to the state
        vel_dot = vel_dot + symbols.dist_f / mass

    # Rotational equation of motion
    xi = cs.vertcat(
        cs.horzcat(-cs.skew(symbols.ang_vel), symbols.ang_vel), cs.horzcat(-symbols.ang_vel.T, 0)
    )
    quat_dot = 0.5 * (xi @ symbols.quat)
    ang_vel_dot = rotation.cs_rpy_rates_deriv2ang_vel_deriv(
        symbols.quat, _drpy_quat, X_dot_euler[9:12]
    )
    if model_dist_t:
        # adding torque disturbances to the state
        # angular acceleration can be converted to total torque
        torque = J @ ang_vel_dot + cs.cross(symbols.ang_vel, J @ symbols.ang_vel)
        # adding torque
        torque = torque + symbols.rot.T @ symbols.dist_t
        # back to angular acceleration
        ang_vel_dot = J_inv @ (torque - 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, X_dot_euler[-4:])
    else:
        X_dot = cs.vertcat(pos_dot, quat_dot, vel_dot, ang_vel_dot)
    Y = cs.vertcat(symbols.pos, symbols.quat)

    return X_dot, X, U, Y

symbolic_dynamics_euler(model_rotor_vel=True, *, mass, gravity_vec, J, J_inv, thrust_dyn_coef, acc_coef, cmd_f_coef, rpy_coef, rpy_rates_coef, cmd_rpy_coef)

Return CasADi symbolic expressions for the so_rpy_rotor dynamics in Euler-angle form.

This is the native representation of the so_rpy_rotor dynamics. The state uses roll/pitch/yaw and their rates rather than quaternion + angular velocity, which avoids trigonometric overhead inside CasADi-based solvers.

Parameters:

Name	Type	Description	Default
model_rotor_vel	bool	If True, the thrust state is included in X and first-order thrust dynamics are modelled. Defaults to True.	True
mass	float	Drone mass in kg.	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
thrust_dyn_coef	Array	Thrust dynamics coefficient in 1/s.	required
acc_coef	Array	Thrust offset in N.	required
cmd_f_coef	Array	Thrust scaling coefficient.	required
rpy_coef	Array	Rotational dynamics coefficients of the roll, pitch, and yaw angles in 1/s², shape (3,).	required
rpy_rates_coef	Array	Rotational dynamics coefficients of the roll, pitch, and yaw rates in 1/s, shape (3,).	required
cmd_rpy_coef	Array	Rotational dynamics coefficients of the commanded roll, pitch, and yaw in 1/s², shape (3,).	required

Returns:

Type	Description
MX	Tuple (X_dot, X, U, Y) of CasADi MX expressions:
MX	X_dot: State derivative, length 16 when model_rotor_vel=True (12 otherwise).
MX	X: State vector [pos(3), rpy(3), vel(3), drpy(3)], with rotor_vel(4) appended if model_rotor_vel=True. Note that rotor_vel here represents the thrust state in Newtons, and that only its first entry enters the dynamics.
MX	U: Input vector [roll_rad, pitch_rad, yaw_rad, thrust_N].
tuple[MX, MX, MX, MX]	Y: Output [pos(3), rpy(3)].

Source code in crazyflow/dynamics/so_rpy_rotor/dynamics.py

def symbolic_dynamics_euler(
    model_rotor_vel: bool = True,
    *,
    mass: float,
    gravity_vec: Array,
    J: Array,
    J_inv: Array,
    thrust_dyn_coef: Array,
    acc_coef: Array,
    cmd_f_coef: Array,
    rpy_coef: Array,
    rpy_rates_coef: Array,
    cmd_rpy_coef: Array,
) -> tuple[cs.MX, cs.MX, cs.MX, cs.MX]:
    """Return CasADi symbolic expressions for the so_rpy_rotor dynamics in Euler-angle form.

    This is the native representation of the ``so_rpy_rotor`` dynamics.  The state uses
    roll/pitch/yaw and their rates rather than quaternion + angular velocity, which avoids
    trigonometric overhead inside CasADi-based solvers.

    Args:
        model_rotor_vel: If ``True``, the thrust state is included in ``X`` and first-order thrust
            dynamics are modelled. Defaults to ``True``.
        mass: Drone mass in kg.
        gravity_vec: Gravity vector, shape ``(3,)``.
        J: Inertia matrix, shape ``(3, 3)``.
        J_inv: Inverse inertia matrix, shape ``(3, 3)``.
        thrust_dyn_coef: Thrust dynamics coefficient in 1/s.
        acc_coef: Thrust offset in N.
        cmd_f_coef: Thrust scaling coefficient.
        rpy_coef: Rotational dynamics coefficients of the roll, pitch, and yaw angles in 1/s², shape
            ``(3,)``.
        rpy_rates_coef: Rotational dynamics coefficients of the roll, pitch, and yaw rates in 1/s,
            shape ``(3,)``.
        cmd_rpy_coef: Rotational dynamics coefficients of the commanded roll, pitch, and yaw in
            1/s², shape ``(3,)``.

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

        * ``X_dot``: State derivative, length 16 when ``model_rotor_vel=True`` (12 otherwise).
        * ``X``: State vector ``[pos(3), rpy(3), vel(3), drpy(3)]``, with ``rotor_vel(4)`` appended
            if ``model_rotor_vel=True``. Note that ``rotor_vel`` here represents the thrust state in
            Newtons, and that only its first entry enters the dynamics.
        * ``U``: Input vector ``[roll_rad, pitch_rad, yaw_rad, thrust_N]``.
        * ``Y``: Output ``[pos(3), rpy(3)]``.
    """
    # States and Inputs
    X = cs.vertcat(symbols.pos, symbols.rpy, symbols.vel, symbols.drpy)
    if model_rotor_vel:
        X = cs.vertcat(X, symbols.rotor_vel)
    U = symbols.cmd_rpyt
    cmd_rpy = U[:3]
    cmd_thrust = U[-1]
    rot = rotation.cs_rpy2matrix(symbols.rpy)

    # Defining the dynamics function
    # Note that we are abusing the rotor_vel state as the thrust
    if model_rotor_vel:
        rotor_vel_dot = thrust_dyn_coef * (cmd_thrust - symbols.rotor_vel)
        forces_motor = symbols.rotor_vel[0]  # We are only using the first element
    else:
        forces_motor = cmd_thrust

    # Creating force vector
    forces_motor_vec = cs.vertcat(0, 0, acc_coef + cmd_f_coef * forces_motor)

    # Linear equation of motion
    pos_dot = symbols.vel
    vel_dot = rot @ forces_motor_vec / mass + gravity_vec

    ddrpy = rpy_coef * symbols.rpy + rpy_rates_coef * symbols.drpy + cmd_rpy_coef * cmd_rpy

    if model_rotor_vel:
        X_dot = cs.vertcat(pos_dot, symbols.drpy, vel_dot, ddrpy, rotor_vel_dot)
    else:
        X_dot = cs.vertcat(pos_dot, symbols.drpy, vel_dot, ddrpy)
    Y = cs.vertcat(symbols.pos, symbols.rpy)

    return X_dot, X, U, Y