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\) 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).

\[ \begin{aligned} \dot{F} &= \frac{1}{\tau}(F_{\mathrm{cmd}} - F), \\ \dot{\mathbf{p}} &= \mathbf{v}, \\ m\dot{\mathbf{v}} &= m\mathbf{g} + (c_{\mathrm{acc}} + c_f F)\,R\,\mathbf{e}_z, \\ \ddot{\boldsymbol{\psi}} &= c_{\psi}\,\boldsymbol{\psi} + c_{\dot{\psi}}\,\dot{\boldsymbol{\psi}} + c_u\,\mathbf{u}_{\mathrm{rpy}}, \end{aligned} \]

where \(\tau\) is the thrust time constant, \(\boldsymbol{\psi} = [\phi,\theta,\psi]^{\top}\) are the roll/pitch/yaw angles with rates \(\dot{\boldsymbol{\psi}}\), and \(R = {}^{\mathcal{I}}R_{\mathcal{B}}(\boldsymbol{\psi})\) is the rotation from body to world frame.

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

Classes

Params

Attributes

J instance-attribute

Inertia matrix of the drone.

J_inv instance-attribute

Inverse of the inertia matrix of the drone.

acc_coef instance-attribute

Acceleration coefficient of the drone.

cmd_f_coef instance-attribute

Collective thrust coefficient of the drone.

cmd_rpy_coef instance-attribute

Roll pitch yaw command coefficient of the drone.

gravity_vec instance-attribute

Gravity vector of the drone.

mass instance-attribute

Mass of the drone.

rpy_coef instance-attribute

Roll pitch yaw coefficient of the drone.

rpy_rates_coef instance-attribute

Roll pitch yaw rates coefficient of the drone.

thrust_time_coef instance-attribute

Rotor coefficient of the drone.

Methods:

create(n_worlds, n_drones, drone, device) staticmethod

Create a default set of parameters for the simulation.

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

@staticmethod
def create(n_worlds: int, n_drones: int, drone: str, device: Device) -> Params:
    """Create a default set of parameters for the simulation."""
    p = load_params(dynamics, drone)
    J = jax.device_put(jnp.tile(p["J"][None, None, :, :], (n_worlds, n_drones, 1, 1)), device)
    return Params(
        mass=jnp.full((n_worlds, n_drones, 1), p["mass"], device=device),
        gravity_vec=jnp.asarray(p["gravity_vec"], device=device),
        J=J,
        J_inv=jnp.linalg.inv(J),
        thrust_time_coef=jnp.asarray(p["thrust_time_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:

sim_dynamics(data)

Compute the forces and torques from the so_rpy_rotor dynamics.

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

def sim_dynamics(data: SimData) -> SimData:
    """Compute the forces and torques from the so_rpy_rotor dynamics."""
    params: Params = data.params
    vel, _, acc, ang_acc, rotor_acc = dynamics(
        pos=data.states.pos,
        quat=data.states.quat,
        vel=data.states.vel,
        ang_vel=data.states.ang_vel,
        rotor_vel=data.states.rotor_vel,
        cmd=data.controls.attitude.cmd,
        dist_f=data.states.force,
        dist_t=data.states.torque,
        **params.__dict__,
    )
    states_deriv = data.states_deriv.replace(
        vel=vel, ang_vel=data.states.ang_vel, acc=acc, ang_acc=ang_acc, rotor_acc=rotor_acc
    )
    return data.replace(states_deriv=states_deriv)

symbolic_dynamics(model_rotor_vel=True, model_dist_f=False, model_dist_t=False, *, mass, gravity_vec, J, J_inv, thrust_time_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 scalar 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_time_coef	Array	First-order thrust lag time constant coefficient (1/s).	required
acc_coef	Array	Scalar acceleration offset coefficient.	required
cmd_f_coef	Array	Collective-thrust-to-acceleration coefficient.	required
rpy_coef	Array	RPY state feedback coefficient, shape (3,).	required
rpy_rates_coef	Array	RPY-rate feedback coefficient, shape (3,).	required
cmd_rpy_coef	Array	RPY command feedforward coefficient, shape (3,).	required

Returns:

Type	Description
MX	Tuple (X_dot, X, U, Y) of CasADi MX expressions:
MX	X_dot: State derivative, length 14 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(1) appended if model_rotor_vel=True. Note that rotor_vel here represents the thrust state in Newtons.
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_time_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 scalar 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_time_coef: First-order thrust lag time constant coefficient (1/s).
        acc_coef: Scalar acceleration offset coefficient.
        cmd_f_coef: Collective-thrust-to-acceleration coefficient.
        rpy_coef: RPY state feedback coefficient, shape ``(3,)``.
        rpy_rates_coef: RPY-rate feedback coefficient, shape ``(3,)``.
        cmd_rpy_coef: RPY command feedforward coefficient, shape ``(3,)``.

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

        * ``X_dot``: State derivative, length 14 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(1)``
            appended if ``model_rotor_vel=True``. Note that ``rotor_vel`` here represents the thrust
            state in Newtons.
        * ``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_time_coef=thrust_time_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(0, -symbols.ang_vel.T), cs.horzcat(symbols.ang_vel, -cs.skew(symbols.ang_vel))
    )
    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_time_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 scalar 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_time_coef	Array	First-order thrust lag time constant coefficient (1/s).	required
acc_coef	Array	Scalar acceleration offset coefficient.	required
cmd_f_coef	Array	Collective-thrust-to-acceleration coefficient.	required
rpy_coef	Array	RPY state feedback coefficient, shape (3,).	required
rpy_rates_coef	Array	RPY-rate feedback coefficient, shape (3,).	required
cmd_rpy_coef	Array	RPY command feedforward coefficient, shape (3,).	required

Returns:

Type	Description
MX	Tuple (X_dot, X, U, Y) of CasADi MX expressions:
MX	X_dot: State derivative, length 13 when model_rotor_vel=True (12 otherwise).
MX	X: State vector [pos(3), rpy(3), vel(3), drpy(3)], with rotor_vel(1) appended if model_rotor_vel=True. Note that rotor_vel here represents the thrust state in Newtons.
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_time_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 scalar 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_time_coef: First-order thrust lag time constant coefficient (1/s).
        acc_coef: Scalar acceleration offset coefficient.
        cmd_f_coef: Collective-thrust-to-acceleration coefficient.
        rpy_coef: RPY state feedback coefficient, shape ``(3,)``.
        rpy_rates_coef: RPY-rate feedback coefficient, shape ``(3,)``.
        cmd_rpy_coef: RPY command feedforward coefficient, shape ``(3,)``.

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

        * ``X_dot``: State derivative, length 13 when ``model_rotor_vel=True`` (12 otherwise).
        * ``X``: State vector ``[pos(3), rpy(3), vel(3), drpy(3)]``, with ``rotor_vel(1)`` appended
            if ``model_rotor_vel=True``. Note that ``rotor_vel`` here represents the thrust state in
            Newtons.
        * ``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 = 1 / thrust_time_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