Skip to content

drone_models

TODO.

model_features(model)

Get the features of a model.

Source code in drone_models/__init__.py 
39
40
41
42
43
def model_features(model: Callable) -> dict[str, bool]:
    """Get the features of a model."""
    if hasattr(model, "func"):  # Is a partial function
        return model_features(model.func)
    return getattr(model, "__drone_model_features__")

parametrize(fn, drone_model, xp=None, device=None)

Parametrize a dynamics function with the default dynamics parameters for a drone model.

Parameters:

Name Type Description Default
fn Callable[P, R]

The dynamics function to parametrize.

 required
drone_model str

The drone model to use.

 required
xp ModuleType | None

The array API module to use. If not provided, numpy is used.

 None
device str | None

The device to use. If none, the device is inferred from the xp module.

 None
Example 
from drone_models.core import parametrize
from drone_models.first_principles import dynamics

dynamics_fn = parametrize(dynamics, drone_model="cf2x_L250")
pos_dot, quat_dot, vel_dot, ang_vel_dot, rotor_vel_dot = dynamics_fn(
    pos=pos, quat=quat, vel=vel, ang_vel=ang_vel, cmd=cmd, rotor_vel=rotor_vel
)

Returns:

Type Description
Callable[P, R]

The parametrized dynamics function with all keyword argument only parameters filled in.
Source code in drone_models/core.py 
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
def parametrize(
    fn: Callable[P, R], drone_model: str, xp: ModuleType | None = None, device: str | None = None
) -> Callable[P, R]:
    """Parametrize a dynamics function with the default dynamics parameters for a drone model.

    Args:
        fn: The dynamics function to parametrize.
        drone_model: The drone model to use.
        xp: The array API module to use. If not provided, numpy is used.
        device: The device to use. If none, the device is inferred from the xp module.

    Example:
        ```python
        from drone_models.core import parametrize
        from drone_models.first_principles import dynamics

        dynamics_fn = parametrize(dynamics, drone_model="cf2x_L250")
        pos_dot, quat_dot, vel_dot, ang_vel_dot, rotor_vel_dot = dynamics_fn(
            pos=pos, quat=quat, vel=vel, ang_vel=ang_vel, cmd=cmd, rotor_vel=rotor_vel
        )
        ```

    Returns:
        The parametrized dynamics function with all keyword argument only parameters filled in.
    """
    try:
        xp = np if xp is None else xp
        physics = fn.__module__.split(".")[-2]
        sig = inspect.signature(fn)
        kwonly_params = [
            name
            for name, param in sig.parameters.items()
            if param.kind == inspect.Parameter.KEYWORD_ONLY
        ]
        params = load_params(physics, drone_model)
        params = {k: xp.asarray(v, device=device) for k, v in params.items() if k in kwonly_params}
    except KeyError as e:
        raise KeyError(
            f"Model `{physics}` does not exist in the parameter registry for drone `{drone_model}`"
        ) from e
    except ValueError as e:
        raise ValueError(f"Drone model `{drone_model}` not supported for `{physics}`") from e
    return partial(fn, **params)

core

Core tools for registering and capability checking for the drone models.

load_params(physics, drone_model, xp=None)

TODO.

Parameters:

Name Type Description Default
physics str

description

 required
drone_model str

description

 required
xp ModuleType | None

The array API module to use. If not provided, numpy is used.

 None

Returns:

Type Description
dict

dict[str, Array]: description
Source code in drone_models/core.py 
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
def load_params(physics: str, drone_model: str, xp: ModuleType | None = None) -> dict:
    """TODO.

    Args:
        physics: _description_
        drone_model: _description_
        xp: The array API module to use. If not provided, numpy is used.

    Returns:
        dict[str, Array]: _description_
    """
    xp = np if xp is None else xp
    with open(Path(__file__).parent / "data/params.toml", "rb") as f:
        physical_params = tomllib.load(f)
    if drone_model not in physical_params:
        raise KeyError(f"Drone model `{drone_model}` not found in data/params.toml")
    with open(Path(__file__).parent / f"{physics}/params.toml", "rb") as f:
        model_params = tomllib.load(f)
    if drone_model not in model_params:
        raise KeyError(f"Drone model `{drone_model}` not found in model params.toml")
    params = physical_params[drone_model] | model_params[drone_model]
    params["J_inv"] = np.linalg.inv(params["J"])
    params = {k: xp.asarray(v) for k, v in params.items()}  # if k in fields
    return params

parametrize(fn, drone_model, xp=None, device=None)

Parametrize a dynamics function with the default dynamics parameters for a drone model.

Parameters:

Name Type Description Default
fn Callable[P, R]

The dynamics function to parametrize.

 required
drone_model str

The drone model to use.

 required
xp ModuleType | None

The array API module to use. If not provided, numpy is used.

 None
device str | None

The device to use. If none, the device is inferred from the xp module.

 None
Example 
from drone_models.core import parametrize
from drone_models.first_principles import dynamics

dynamics_fn = parametrize(dynamics, drone_model="cf2x_L250")
pos_dot, quat_dot, vel_dot, ang_vel_dot, rotor_vel_dot = dynamics_fn(
    pos=pos, quat=quat, vel=vel, ang_vel=ang_vel, cmd=cmd, rotor_vel=rotor_vel
)

Returns:

Type Description
Callable[P, R]

The parametrized dynamics function with all keyword argument only parameters filled in.
Source code in drone_models/core.py 
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
def parametrize(
    fn: Callable[P, R], drone_model: str, xp: ModuleType | None = None, device: str | None = None
) -> Callable[P, R]:
    """Parametrize a dynamics function with the default dynamics parameters for a drone model.

    Args:
        fn: The dynamics function to parametrize.
        drone_model: The drone model to use.
        xp: The array API module to use. If not provided, numpy is used.
        device: The device to use. If none, the device is inferred from the xp module.

    Example:
        ```python
        from drone_models.core import parametrize
        from drone_models.first_principles import dynamics

        dynamics_fn = parametrize(dynamics, drone_model="cf2x_L250")
        pos_dot, quat_dot, vel_dot, ang_vel_dot, rotor_vel_dot = dynamics_fn(
            pos=pos, quat=quat, vel=vel, ang_vel=ang_vel, cmd=cmd, rotor_vel=rotor_vel
        )
        ```

    Returns:
        The parametrized dynamics function with all keyword argument only parameters filled in.
    """
    try:
        xp = np if xp is None else xp
        physics = fn.__module__.split(".")[-2]
        sig = inspect.signature(fn)
        kwonly_params = [
            name
            for name, param in sig.parameters.items()
            if param.kind == inspect.Parameter.KEYWORD_ONLY
        ]
        params = load_params(physics, drone_model)
        params = {k: xp.asarray(v, device=device) for k, v in params.items() if k in kwonly_params}
    except KeyError as e:
        raise KeyError(
            f"Model `{physics}` does not exist in the parameter registry for drone `{drone_model}`"
        ) from e
    except ValueError as e:
        raise ValueError(f"Drone model `{drone_model}` not supported for `{physics}`") from e
    return partial(fn, **params)

supports(rotor_dynamics=True)

Decorator to indicate which features are supported.

Source code in drone_models/core.py 
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
def supports(rotor_dynamics: bool = True) -> Callable[[F], F]:
    """Decorator to indicate which features are supported."""

    def decorator(fn: F) -> F:
        @wraps(fn)
        def wrapper(
            pos: Array,
            quat: Array,
            vel: Array,
            ang_vel: Array,
            cmd: Array,
            rotor_vel: Array | None = None,
            *args: Any,
            **kwargs: Any,
        ) -> tuple[Array, Array, Array, Array, Array | None]:
            if not rotor_dynamics and rotor_vel is not None:
                raise ValueError("Rotor dynamics not supported, but rotor_vel is provided.")
            if rotor_dynamics and rotor_vel is None:
                warnings.warn("Rotor velocity not provided, using commanded rotor velocity.")
            return fn(pos, quat, vel, ang_vel, cmd, rotor_vel, *args, **kwargs)

        wrapper.__drone_model_features__ = {"rotor_dynamics": rotor_dynamics}

        return wrapper  # type: ignore

    return decorator

drones

List of available drones.

TODO: Add description.

first_principles

First principles model.

TODO: Add description.

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

First principles model for a quatrotor.

The input consists of four forces in [N]. TODO more detail.

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
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
L float

Distance from the CoM to the motor (m).

 required
mixing_matrix Array

Mixing matrix denoting the turn direction of the motors (4x3).

 required
rotor_tau float

Thrust time constant (s).

 required

.. math:: \sum_{i=1}^{\infty} x_{i} TODO

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 
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
@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,
    gravity_vec: Array,
    J: Array,
    J_inv: Array,
    rpm2thrust: Array,
    rpm2torque: Array,
    L: float,
    mixing_matrix: Array,
    rotor_tau: float,
) -> tuple[Array, Array, Array, Array, Array | None]:
    r"""First principles model for a quatrotor.

    The input consists of four forces in [N]. TODO more detail.

    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).
        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).
        L: Distance from the CoM to the motor (m).
        mixing_matrix: Mixing matrix denoting the turn direction of the motors (4x3).
        rotor_tau: Thrust time constant (s).

    .. math::
        \sum_{i=1}^{\\infty} x_{i} TODO

    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, gravity_vec, J, J_inv, rpm2thrust, rpm2torque, L, mixing_matrix, rotor_tau = to_xp(
        mass,
        gravity_vec,
        J,
        J_inv,
        rpm2thrust,
        rpm2torque,
        L,
        mixing_matrix,
        rotor_tau,
        xp=xp,
        device=device(pos),
    )
    rot = R.from_quat(quat)
    # Rotor dynamics
    if rotor_vel is None:
        rotor_vel, rotor_vel_dot = cmd, None
    else:
        rotor_vel_dot = 1 / rotor_tau * (cmd - rotor_vel)  # - 1 / KM * rotor_vel**2
    # Creating force and torque vector
    forces_motor = rpm2thrust[0] + rpm2thrust[1] * rotor_vel + rpm2thrust[2] * rotor_vel**2
    torques_motor = rpm2torque[0] + rpm2torque[1] * rotor_vel + rpm2torque[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)

    torque_vec = (mixing_matrix @ (forces_motor)[..., None])[..., 0] * xp.stack(
        [L, L, xp.asarray(0.0)]
    ) + (mixing_matrix @ (torques_motor)[..., None])[..., 0] * xp.stack(
        [xp.asarray(0.0), xp.asarray(0.0), xp.asarray(1.0)]
    )

    # Linear equation of motion
    forces_motor_vec_world = rot.apply(forces_motor_vec)
    forces_sum = forces_motor_vec_world + gravity_vec * mass
    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, gravity_vec, J, J_inv, rpm2thrust, rpm2torque, L, mixing_matrix, rotor_tau)

TODO take from numeric.

Source code in drone_models/first_principles/model.py 
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
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,
    rpm2thrust: Array,
    rpm2torque: Array,
    L: float,
    mixing_matrix: Array,
    rotor_tau: float,
) -> tuple[cs.MX, cs.MX, cs.MX, cs.MX]:
    """TODO take from numeric."""
    # 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:
        # Thrust dynamics
        rotor_vel_dot = 1 / rotor_tau * (U - symbols.rotor_vel)  # - 1 / KM * symbols.rotor_vel**2
        forces_motor = (
            rpm2thrust[0] + rpm2thrust[1] * symbols.rotor_vel + rpm2thrust[2] * symbols.rotor_vel**2
        )
        torques_motor = (
            rpm2torque[0] + rpm2torque[1] * symbols.rotor_vel + rpm2torque[2] * symbols.rotor_vel**2
        )
    else:
        forces_motor = rpm2thrust[0] + rpm2thrust[1] * U + rpm2thrust[2] * U**2
        torques_motor = rpm2torque[0] + rpm2torque[1] * U + rpm2torque[2] * U**2

    # Creating force and torque vector
    forces_motor_vec = cs.vertcat(0.0, 0.0, cs.sum1(forces_motor))
    torques_motor_vec = mixing_matrix @ forces_motor * cs.vertcat(
        L, L, 0.0
    ) + mixing_matrix @ torques_motor * cs.vertcat(0.0, 0.0, 1.0)

    # Linear equation of motion
    forces_motor_vec_world = symbols.rot @ forces_motor_vec
    forces_sum = forces_motor_vec_world + gravity_vec * mass
    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)

    return X_dot, X, U, Y

model

TODO.

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

First principles model for a quatrotor.

The input consists of four forces in [N]. TODO more detail.

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
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
L float

Distance from the CoM to the motor (m).

 required
mixing_matrix Array

Mixing matrix denoting the turn direction of the motors (4x3).

 required
rotor_tau float

Thrust time constant (s).

 required

.. math:: \sum_{i=1}^{\infty} x_{i} TODO

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 
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
@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,
    gravity_vec: Array,
    J: Array,
    J_inv: Array,
    rpm2thrust: Array,
    rpm2torque: Array,
    L: float,
    mixing_matrix: Array,
    rotor_tau: float,
) -> tuple[Array, Array, Array, Array, Array | None]:
    r"""First principles model for a quatrotor.

    The input consists of four forces in [N]. TODO more detail.

    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).
        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).
        L: Distance from the CoM to the motor (m).
        mixing_matrix: Mixing matrix denoting the turn direction of the motors (4x3).
        rotor_tau: Thrust time constant (s).

    .. math::
        \sum_{i=1}^{\\infty} x_{i} TODO

    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, gravity_vec, J, J_inv, rpm2thrust, rpm2torque, L, mixing_matrix, rotor_tau = to_xp(
        mass,
        gravity_vec,
        J,
        J_inv,
        rpm2thrust,
        rpm2torque,
        L,
        mixing_matrix,
        rotor_tau,
        xp=xp,
        device=device(pos),
    )
    rot = R.from_quat(quat)
    # Rotor dynamics
    if rotor_vel is None:
        rotor_vel, rotor_vel_dot = cmd, None
    else:
        rotor_vel_dot = 1 / rotor_tau * (cmd - rotor_vel)  # - 1 / KM * rotor_vel**2
    # Creating force and torque vector
    forces_motor = rpm2thrust[0] + rpm2thrust[1] * rotor_vel + rpm2thrust[2] * rotor_vel**2
    torques_motor = rpm2torque[0] + rpm2torque[1] * rotor_vel + rpm2torque[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)

    torque_vec = (mixing_matrix @ (forces_motor)[..., None])[..., 0] * xp.stack(
        [L, L, xp.asarray(0.0)]
    ) + (mixing_matrix @ (torques_motor)[..., None])[..., 0] * xp.stack(
        [xp.asarray(0.0), xp.asarray(0.0), xp.asarray(1.0)]
    )

    # Linear equation of motion
    forces_motor_vec_world = rot.apply(forces_motor_vec)
    forces_sum = forces_motor_vec_world + gravity_vec * mass
    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, gravity_vec, J, J_inv, rpm2thrust, rpm2torque, L, mixing_matrix, rotor_tau)

TODO take from numeric.

Source code in drone_models/first_principles/model.py 
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
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,
    rpm2thrust: Array,
    rpm2torque: Array,
    L: float,
    mixing_matrix: Array,
    rotor_tau: float,
) -> tuple[cs.MX, cs.MX, cs.MX, cs.MX]:
    """TODO take from numeric."""
    # 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:
        # Thrust dynamics
        rotor_vel_dot = 1 / rotor_tau * (U - symbols.rotor_vel)  # - 1 / KM * symbols.rotor_vel**2
        forces_motor = (
            rpm2thrust[0] + rpm2thrust[1] * symbols.rotor_vel + rpm2thrust[2] * symbols.rotor_vel**2
        )
        torques_motor = (
            rpm2torque[0] + rpm2torque[1] * symbols.rotor_vel + rpm2torque[2] * symbols.rotor_vel**2
        )
    else:
        forces_motor = rpm2thrust[0] + rpm2thrust[1] * U + rpm2thrust[2] * U**2
        torques_motor = rpm2torque[0] + rpm2torque[1] * U + rpm2torque[2] * U**2

    # Creating force and torque vector
    forces_motor_vec = cs.vertcat(0.0, 0.0, cs.sum1(forces_motor))
    torques_motor_vec = mixing_matrix @ forces_motor * cs.vertcat(
        L, L, 0.0
    ) + mixing_matrix @ torques_motor * cs.vertcat(0.0, 0.0, 1.0)

    # Linear equation of motion
    forces_motor_vec_world = symbols.rot @ forces_motor_vec
    forces_sum = forces_motor_vec_world + gravity_vec * mass
    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)

    return X_dot, X, U, Y

so_rpy

SoRpy model.

TODO: Add description.

dynamics(pos, quat, vel, ang_vel, cmd, rotor_vel=None, dist_f=None, dist_t=None, *, mass, gravity_vec, J, J_inv, acc_coef, cmd_f_coef, rpy_coef, rpy_rates_coef, cmd_rpy_coef)

The fitted model with linear, second order rpy dynamics.

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

Roll pitch yaw (rad) and collective thrust (N) command.

 required
rotor_vel Array | None

Speed of the 4 motors (RPMs). Kept for compatibility with the model signature.

 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
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
acc_coef Array

Coefficient for the acceleration (1/s^2).

 required
cmd_f_coef Array

Coefficient for the collective thrust (N/rad^2).

 required
rpy_coef Array

Coefficient for the roll pitch yaw dynamics (1/s).

 required
rpy_rates_coef Array

Coefficient for the roll pitch yaw rates dynamics (1/s^2).

 required
cmd_rpy_coef Array

Coefficient for the roll pitch yaw command dynamics (1/s).

 required

Returns:

Type Description
tuple[Array, Array, Array, Array, Array | None]

The derivatives of all state variables.
Source code in drone_models/so_rpy/model.py 
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
@supports(rotor_dynamics=False)
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,
    gravity_vec: Array,
    J: Array,
    J_inv: Array,
    acc_coef: Array,
    cmd_f_coef: Array,
    rpy_coef: Array,
    rpy_rates_coef: Array,
    cmd_rpy_coef: Array,
) -> tuple[Array, Array, Array, Array, Array | None]:
    """The fitted model with linear, second order rpy dynamics.

    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: Roll pitch yaw (rad) and collective thrust (N) command.
        rotor_vel: Speed of the 4 motors (RPMs). Kept for compatibility with the model signature.
        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).
        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).
        acc_coef: Coefficient for the acceleration (1/s^2).
        cmd_f_coef: Coefficient for the collective thrust (N/rad^2).
        rpy_coef: Coefficient for the roll pitch yaw dynamics (1/s).
        rpy_rates_coef: Coefficient for the roll pitch yaw rates dynamics (1/s^2).
        cmd_rpy_coef: Coefficient for the roll pitch yaw command dynamics (1/s).

    Returns:
        The derivatives of all state variables.
    """
    xp = array_namespace(pos)
    # Convert parameters to correct xp framework
    mass, gravity_vec, J, J_inv = to_xp(mass, gravity_vec, J, J_inv, xp=xp, device=device(pos))
    acc_coef, cmd_f_coef, rpy_coef, rpy_rates_coef, cmd_rpy_coef = to_xp(
        acc_coef, cmd_f_coef, rpy_coef, rpy_rates_coef, cmd_rpy_coef, xp=xp, device=device(pos)
    )
    cmd_f = cmd[..., -1]
    cmd_rpy = cmd[..., 0:3]
    rot = R.from_quat(quat)
    euler_angles = rot.as_euler("xyz")

    rotor_vel_dot = None
    thrust = acc_coef + cmd_f_coef * cmd_f

    drone_z_axis = rot.as_matrix()[..., -1]

    pos_dot = vel
    vel_dot = 1.0 / mass * thrust[..., None] * drone_z_axis + gravity_vec

    if dist_f is not None:
        vel_dot = vel_dot + dist_f / mass  # Adding force disturbances to the state
    vel_dot = xp.asarray(vel_dot)

    # Rotational equation of motion
    quat_dot = rotation.ang_vel2quat_dot(quat, ang_vel)
    rpy_rates = rotation.ang_vel2rpy_rates(quat, ang_vel)
    rpy_rates_dot = rpy_coef * euler_angles + rpy_rates_coef * rpy_rates + cmd_rpy_coef * cmd_rpy
    ang_vel_dot = rotation.rpy_rates_deriv2ang_vel_deriv(quat, rpy_rates, rpy_rates_dot)
    if dist_t is not None:
        # adding torque disturbances to the state
        # angular acceleration can be converted to total torque given the inertia matrix
        torque = (J @ ang_vel_dot[..., None])[..., 0]
        torque = torque + xp.linalg.cross(ang_vel, (J @ ang_vel[..., None])[..., 0])
        # adding torque. TODO: This should be a linear transformation. Can't we just transform the
        # disturbance torque to an ang_vel_dot summand directly?
        torque = torque + rot.apply(dist_t, inverse=True)
        # back to angular acceleration
        torque = torque - xp.linalg.cross(ang_vel, (J @ ang_vel[..., None])[..., 0])
        ang_vel_dot = (J_inv @ torque[..., None])[..., 0]

    return pos_dot, quat_dot, vel_dot, ang_vel_dot, rotor_vel_dot

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

Fitted model with linear, second order rpy dynamics.

For info on the args, see above.

This wrapper converts the actual symbolic model, defined below, into the quat and ang_vel form.

Source code in drone_models/so_rpy/model.py 
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
def symbolic_dynamics(
    model_rotor_vel: bool = False,
    model_dist_f: bool = False,
    model_dist_t: bool = False,
    *,
    mass: float,
    gravity_vec: Array,
    J: Array,
    J_inv: 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]:
    """Fitted model with linear, second order rpy dynamics.

    For info on the args, see above.

    This wrapper converts the actual symbolic model, defined below, into the quat and ang_vel form.
    """
    # We need to set the rpy and drpy symbols before building the euler model
    symbols.rpy = rotation.cs_quat2euler(symbols.quat)
    symbols.drpy = rotation.cs_ang_vel2rpy_rates(symbols.quat, symbols.ang_vel)
    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,
        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,
    )

    # States and Inputs
    X = cs.vertcat(symbols.pos, symbols.quat, symbols.vel, symbols.ang_vel)
    if model_rotor_vel:
        logging.getLogger(__name__).warning("The so_rpy model does not support thrust dynamics")
        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, symbols.drpy, 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))

    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=False, *, mass, gravity_vec, J, J_inv, acc_coef, cmd_f_coef, rpy_coef, rpy_rates_coef, cmd_rpy_coef)

The fitted linear, second order rpy dynamics.

For info on the args, see above.

This function returns the actual model, as defined in the paper, for direct use.

Source code in drone_models/so_rpy/model.py 
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
def symbolic_dynamics_euler(
    model_rotor_vel: bool = False,
    *,
    mass: float,
    gravity_vec: Array,
    J: Array,
    J_inv: 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]:
    """The fitted linear, second order rpy dynamics.

    For info on the args, see above.

    This function returns the actual model, as defined in the paper, for direct use.
    """
    # States and Inputs
    X = cs.vertcat(symbols.pos, symbols.rpy, symbols.vel, symbols.drpy)
    if model_rotor_vel:
        logging.getLogger(__name__).warning("The so_rpy model does not support thrust dynamics")
        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
    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

    X_dot = cs.vertcat(pos_dot, symbols.drpy, vel_dot, ddrpy)
    Y = cs.vertcat(symbols.pos, symbols.rpy)

    return X_dot, X, U, Y

model

TODO.

dynamics(pos, quat, vel, ang_vel, cmd, rotor_vel=None, dist_f=None, dist_t=None, *, mass, gravity_vec, J, J_inv, acc_coef, cmd_f_coef, rpy_coef, rpy_rates_coef, cmd_rpy_coef)

The fitted model with linear, second order rpy dynamics.

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

Roll pitch yaw (rad) and collective thrust (N) command.

 required
rotor_vel Array | None

Speed of the 4 motors (RPMs). Kept for compatibility with the model signature.

 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
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
acc_coef Array

Coefficient for the acceleration (1/s^2).

 required
cmd_f_coef Array

Coefficient for the collective thrust (N/rad^2).

 required
rpy_coef Array

Coefficient for the roll pitch yaw dynamics (1/s).

 required
rpy_rates_coef Array

Coefficient for the roll pitch yaw rates dynamics (1/s^2).

 required
cmd_rpy_coef Array

Coefficient for the roll pitch yaw command dynamics (1/s).

 required

Returns:

Type Description
tuple[Array, Array, Array, Array, Array | None]

The derivatives of all state variables.
Source code in drone_models/so_rpy/model.py 
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
@supports(rotor_dynamics=False)
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,
    gravity_vec: Array,
    J: Array,
    J_inv: Array,
    acc_coef: Array,
    cmd_f_coef: Array,
    rpy_coef: Array,
    rpy_rates_coef: Array,
    cmd_rpy_coef: Array,
) -> tuple[Array, Array, Array, Array, Array | None]:
    """The fitted model with linear, second order rpy dynamics.

    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: Roll pitch yaw (rad) and collective thrust (N) command.
        rotor_vel: Speed of the 4 motors (RPMs). Kept for compatibility with the model signature.
        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).
        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).
        acc_coef: Coefficient for the acceleration (1/s^2).
        cmd_f_coef: Coefficient for the collective thrust (N/rad^2).
        rpy_coef: Coefficient for the roll pitch yaw dynamics (1/s).
        rpy_rates_coef: Coefficient for the roll pitch yaw rates dynamics (1/s^2).
        cmd_rpy_coef: Coefficient for the roll pitch yaw command dynamics (1/s).

    Returns:
        The derivatives of all state variables.
    """
    xp = array_namespace(pos)
    # Convert parameters to correct xp framework
    mass, gravity_vec, J, J_inv = to_xp(mass, gravity_vec, J, J_inv, xp=xp, device=device(pos))
    acc_coef, cmd_f_coef, rpy_coef, rpy_rates_coef, cmd_rpy_coef = to_xp(
        acc_coef, cmd_f_coef, rpy_coef, rpy_rates_coef, cmd_rpy_coef, xp=xp, device=device(pos)
    )
    cmd_f = cmd[..., -1]
    cmd_rpy = cmd[..., 0:3]
    rot = R.from_quat(quat)
    euler_angles = rot.as_euler("xyz")

    rotor_vel_dot = None
    thrust = acc_coef + cmd_f_coef * cmd_f

    drone_z_axis = rot.as_matrix()[..., -1]

    pos_dot = vel
    vel_dot = 1.0 / mass * thrust[..., None] * drone_z_axis + gravity_vec

    if dist_f is not None:
        vel_dot = vel_dot + dist_f / mass  # Adding force disturbances to the state
    vel_dot = xp.asarray(vel_dot)

    # Rotational equation of motion
    quat_dot = rotation.ang_vel2quat_dot(quat, ang_vel)
    rpy_rates = rotation.ang_vel2rpy_rates(quat, ang_vel)
    rpy_rates_dot = rpy_coef * euler_angles + rpy_rates_coef * rpy_rates + cmd_rpy_coef * cmd_rpy
    ang_vel_dot = rotation.rpy_rates_deriv2ang_vel_deriv(quat, rpy_rates, rpy_rates_dot)
    if dist_t is not None:
        # adding torque disturbances to the state
        # angular acceleration can be converted to total torque given the inertia matrix
        torque = (J @ ang_vel_dot[..., None])[..., 0]
        torque = torque + xp.linalg.cross(ang_vel, (J @ ang_vel[..., None])[..., 0])
        # adding torque. TODO: This should be a linear transformation. Can't we just transform the
        # disturbance torque to an ang_vel_dot summand directly?
        torque = torque + rot.apply(dist_t, inverse=True)
        # back to angular acceleration
        torque = torque - xp.linalg.cross(ang_vel, (J @ ang_vel[..., None])[..., 0])
        ang_vel_dot = (J_inv @ torque[..., None])[..., 0]

    return pos_dot, quat_dot, vel_dot, ang_vel_dot, rotor_vel_dot

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

Fitted model with linear, second order rpy dynamics.

For info on the args, see above.

This wrapper converts the actual symbolic model, defined below, into the quat and ang_vel form.

Source code in drone_models/so_rpy/model.py 
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
def symbolic_dynamics(
    model_rotor_vel: bool = False,
    model_dist_f: bool = False,
    model_dist_t: bool = False,
    *,
    mass: float,
    gravity_vec: Array,
    J: Array,
    J_inv: 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]:
    """Fitted model with linear, second order rpy dynamics.

    For info on the args, see above.

    This wrapper converts the actual symbolic model, defined below, into the quat and ang_vel form.
    """
    # We need to set the rpy and drpy symbols before building the euler model
    symbols.rpy = rotation.cs_quat2euler(symbols.quat)
    symbols.drpy = rotation.cs_ang_vel2rpy_rates(symbols.quat, symbols.ang_vel)
    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,
        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,
    )

    # States and Inputs
    X = cs.vertcat(symbols.pos, symbols.quat, symbols.vel, symbols.ang_vel)
    if model_rotor_vel:
        logging.getLogger(__name__).warning("The so_rpy model does not support thrust dynamics")
        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, symbols.drpy, 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))

    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=False, *, mass, gravity_vec, J, J_inv, acc_coef, cmd_f_coef, rpy_coef, rpy_rates_coef, cmd_rpy_coef)

The fitted linear, second order rpy dynamics.

For info on the args, see above.

This function returns the actual model, as defined in the paper, for direct use.

Source code in drone_models/so_rpy/model.py 
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
def symbolic_dynamics_euler(
    model_rotor_vel: bool = False,
    *,
    mass: float,
    gravity_vec: Array,
    J: Array,
    J_inv: 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]:
    """The fitted linear, second order rpy dynamics.

    For info on the args, see above.

    This function returns the actual model, as defined in the paper, for direct use.
    """
    # States and Inputs
    X = cs.vertcat(symbols.pos, symbols.rpy, symbols.vel, symbols.drpy)
    if model_rotor_vel:
        logging.getLogger(__name__).warning("The so_rpy model does not support thrust dynamics")
        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
    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

    X_dot = cs.vertcat(pos_dot, symbols.drpy, vel_dot, ddrpy)
    Y = cs.vertcat(symbols.pos, symbols.rpy)

    return X_dot, X, U, Y

so_rpy_rotor

SoRpyRotor model.

TODO: Add description.

dynamics(pos, quat, vel, ang_vel, cmd, rotor_vel=None, dist_f=None, dist_t=None, *, mass, gravity_vec, J, J_inv, thrust_time_coef, acc_coef, cmd_f_coef, rpy_coef, rpy_rates_coef, cmd_rpy_coef)

Fitted model with linear, second order rpy dynamics with thrust dynamics.

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

Roll pitch yaw (rad) and collective thrust (N) command.

 required
rotor_vel Array | None

Speed of the 4 motors (RPMs). If None, the commanded thrust is directly applied (not recommended). If value is given, rotor 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
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
thrust_time_coef Array

Coefficient for the rotor dynamics (1/s).

 required
acc_coef Array

Coefficient for the acceleration (1/s^2).

 required
cmd_f_coef Array

Coefficient for the collective thrust (N/rad^2).

 required
rpy_coef Array

Coefficient for the roll pitch yaw dynamics (1/s).

 required
rpy_rates_coef Array

Coefficient for the roll pitch yaw rates dynamics (1/s^2).

 required
cmd_rpy_coef Array

Coefficient for the roll pitch yaw command dynamics (1/s).

 required

Returns:

Type Description
tuple[Array, Array, Array, Array, Array | None]

tuple[Array, Array, Array, Array, Array | None]: description
Source code in drone_models/so_rpy_rotor/model.py 
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
@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,
    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[Array, Array, Array, Array, Array | None]:
    """Fitted model with linear, second order rpy dynamics with thrust dynamics.

    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: Roll pitch yaw (rad) and collective thrust (N) command.
        rotor_vel: Speed of the 4 motors (RPMs). If None, the commanded thrust is directly
            applied (not recommended). If value is given, rotor 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).
        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).
        thrust_time_coef: Coefficient for the rotor dynamics (1/s).
        acc_coef: Coefficient for the acceleration (1/s^2).
        cmd_f_coef: Coefficient for the collective thrust (N/rad^2).
        rpy_coef: Coefficient for the roll pitch yaw dynamics (1/s).
        rpy_rates_coef: Coefficient for the roll pitch yaw rates dynamics (1/s^2).
        cmd_rpy_coef: Coefficient for the roll pitch yaw command dynamics (1/s).

    Returns:
        tuple[Array, Array, Array, Array, Array | None]: _description_
    """
    xp = array_namespace(pos)
    # Convert constants to the correct framework and device
    device = xp_device(pos)
    mass, gravity_vec, J, J_inv = to_xp(mass, gravity_vec, J, J_inv, xp=xp, device=device)
    thrust_time_coef, acc_coef, cmd_f_coef = to_xp(
        thrust_time_coef, acc_coef, cmd_f_coef, xp=xp, device=device
    )
    rpy_coef, rpy_rates_coef, cmd_rpy_coef = to_xp(
        rpy_coef, rpy_rates_coef, cmd_rpy_coef, xp=xp, device=device
    )

    cmd_f = cmd[..., -1]
    cmd_rpy = cmd[..., 0:3]
    rot = R.from_quat(quat)
    euler_angles = rot.as_euler("xyz")

    # Note that we are abusing the rotor_vel state as the thrust
    if rotor_vel is None:
        rotor_vel, rotor_vel_dot = cmd_f[..., None], None
    else:
        rotor_vel_dot = 1 / thrust_time_coef * (cmd_f[..., None] - rotor_vel)

    forces_motor = rotor_vel[..., 0]
    thrust = acc_coef + cmd_f_coef * forces_motor

    drone_z_axis = rot.as_matrix()[..., -1]

    pos_dot = vel
    vel_dot = 1.0 / mass * thrust[..., None] * drone_z_axis + gravity_vec
    if dist_f is not None:
        # Adding force disturbances to the state
        vel_dot = vel_dot + dist_f / mass
    vel_dot = xp.asarray(vel_dot)

    # Rotational equation of motion
    quat_dot = rotation.ang_vel2quat_dot(quat, ang_vel)
    rpy_rates = rotation.ang_vel2rpy_rates(quat, ang_vel)
    rpy_rates_dot = rpy_coef * euler_angles + rpy_rates_coef * rpy_rates + cmd_rpy_coef * cmd_rpy
    ang_vel_dot = rotation.rpy_rates_deriv2ang_vel_deriv(quat, rpy_rates, rpy_rates_dot)
    if dist_t is not None:
        # adding torque disturbances to the state
        # angular acceleration can be converted to total torque given the inertia matrix
        torque = (J @ ang_vel_dot[..., None])[..., 0]
        torque = torque + xp.linalg.cross(ang_vel, (J @ ang_vel[..., None])[..., 0])
        # adding torque. TODO: This should be a linear transformation. Can't we just transform the
        # disturbance torque to an ang_vel_dot summand directly?
        torque = torque + rot.apply(dist_t, inverse=True)
        # back to angular acceleration
        torque = torque - xp.linalg.cross(ang_vel, (J @ ang_vel[..., None])[..., 0])
        ang_vel_dot = (J_inv @ torque[..., None])[..., 0]

    return pos_dot, quat_dot, vel_dot, ang_vel_dot, rotor_vel_dot

symbolic_dynamics(model_rotor_vel=False, 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)

Fitted model with linear, second order rpy dynamics with thrust dynamics.

For info on the args, see above.

This wrapper converts the actual symbolic model, defined below, into the quat and ang_vel form.

Source code in drone_models/so_rpy_rotor/model.py 
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
def symbolic_dynamics(
    model_rotor_vel: bool = False,
    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]:
    """Fitted model with linear, second order rpy dynamics with thrust dynamics.

    For info on the args, see above.

    This wrapper converts the actual symbolic model, defined below, into the quat and ang_vel form.
    """
    ## We need to set the rpy and drpy symbols before building the euler model
    symbols.rpy = rotation.cs_quat2euler(symbols.quat)
    symbols.drpy = rotation.cs_ang_vel2rpy_rates(symbols.quat, symbols.ang_vel)
    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,
    )

    # 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, symbols.drpy, 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)

The fitted linear, second order rpy dynamics with thrust dynamics.

For info on the args, see above.

This function returns the actual model, as defined in the paper, for direct use.

Source code in drone_models/so_rpy_rotor/model.py 
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
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]:
    """The fitted linear, second order rpy dynamics with thrust dynamics.

    For info on the args, see above.

    This function returns the actual model, as defined in the paper, for direct use.
    """
    # 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

model

TODO.

dynamics(pos, quat, vel, ang_vel, cmd, rotor_vel=None, dist_f=None, dist_t=None, *, mass, gravity_vec, J, J_inv, thrust_time_coef, acc_coef, cmd_f_coef, rpy_coef, rpy_rates_coef, cmd_rpy_coef)

Fitted model with linear, second order rpy dynamics with thrust dynamics.

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

Roll pitch yaw (rad) and collective thrust (N) command.

 required
rotor_vel Array | None

Speed of the 4 motors (RPMs). If None, the commanded thrust is directly applied (not recommended). If value is given, rotor 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
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
thrust_time_coef Array

Coefficient for the rotor dynamics (1/s).

 required
acc_coef Array

Coefficient for the acceleration (1/s^2).

 required
cmd_f_coef Array

Coefficient for the collective thrust (N/rad^2).

 required
rpy_coef Array

Coefficient for the roll pitch yaw dynamics (1/s).

 required
rpy_rates_coef Array

Coefficient for the roll pitch yaw rates dynamics (1/s^2).

 required
cmd_rpy_coef Array

Coefficient for the roll pitch yaw command dynamics (1/s).

 required

Returns:

Type Description
tuple[Array, Array, Array, Array, Array | None]

tuple[Array, Array, Array, Array, Array | None]: description
Source code in drone_models/so_rpy_rotor/model.py 
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
@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,
    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[Array, Array, Array, Array, Array | None]:
    """Fitted model with linear, second order rpy dynamics with thrust dynamics.

    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: Roll pitch yaw (rad) and collective thrust (N) command.
        rotor_vel: Speed of the 4 motors (RPMs). If None, the commanded thrust is directly
            applied (not recommended). If value is given, rotor 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).
        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).
        thrust_time_coef: Coefficient for the rotor dynamics (1/s).
        acc_coef: Coefficient for the acceleration (1/s^2).
        cmd_f_coef: Coefficient for the collective thrust (N/rad^2).
        rpy_coef: Coefficient for the roll pitch yaw dynamics (1/s).
        rpy_rates_coef: Coefficient for the roll pitch yaw rates dynamics (1/s^2).
        cmd_rpy_coef: Coefficient for the roll pitch yaw command dynamics (1/s).

    Returns:
        tuple[Array, Array, Array, Array, Array | None]: _description_
    """
    xp = array_namespace(pos)
    # Convert constants to the correct framework and device
    device = xp_device(pos)
    mass, gravity_vec, J, J_inv = to_xp(mass, gravity_vec, J, J_inv, xp=xp, device=device)
    thrust_time_coef, acc_coef, cmd_f_coef = to_xp(
        thrust_time_coef, acc_coef, cmd_f_coef, xp=xp, device=device
    )
    rpy_coef, rpy_rates_coef, cmd_rpy_coef = to_xp(
        rpy_coef, rpy_rates_coef, cmd_rpy_coef, xp=xp, device=device
    )

    cmd_f = cmd[..., -1]
    cmd_rpy = cmd[..., 0:3]
    rot = R.from_quat(quat)
    euler_angles = rot.as_euler("xyz")

    # Note that we are abusing the rotor_vel state as the thrust
    if rotor_vel is None:
        rotor_vel, rotor_vel_dot = cmd_f[..., None], None
    else:
        rotor_vel_dot = 1 / thrust_time_coef * (cmd_f[..., None] - rotor_vel)

    forces_motor = rotor_vel[..., 0]
    thrust = acc_coef + cmd_f_coef * forces_motor

    drone_z_axis = rot.as_matrix()[..., -1]

    pos_dot = vel
    vel_dot = 1.0 / mass * thrust[..., None] * drone_z_axis + gravity_vec
    if dist_f is not None:
        # Adding force disturbances to the state
        vel_dot = vel_dot + dist_f / mass
    vel_dot = xp.asarray(vel_dot)

    # Rotational equation of motion
    quat_dot = rotation.ang_vel2quat_dot(quat, ang_vel)
    rpy_rates = rotation.ang_vel2rpy_rates(quat, ang_vel)
    rpy_rates_dot = rpy_coef * euler_angles + rpy_rates_coef * rpy_rates + cmd_rpy_coef * cmd_rpy
    ang_vel_dot = rotation.rpy_rates_deriv2ang_vel_deriv(quat, rpy_rates, rpy_rates_dot)
    if dist_t is not None:
        # adding torque disturbances to the state
        # angular acceleration can be converted to total torque given the inertia matrix
        torque = (J @ ang_vel_dot[..., None])[..., 0]
        torque = torque + xp.linalg.cross(ang_vel, (J @ ang_vel[..., None])[..., 0])
        # adding torque. TODO: This should be a linear transformation. Can't we just transform the
        # disturbance torque to an ang_vel_dot summand directly?
        torque = torque + rot.apply(dist_t, inverse=True)
        # back to angular acceleration
        torque = torque - xp.linalg.cross(ang_vel, (J @ ang_vel[..., None])[..., 0])
        ang_vel_dot = (J_inv @ torque[..., None])[..., 0]

    return pos_dot, quat_dot, vel_dot, ang_vel_dot, rotor_vel_dot

symbolic_dynamics(model_rotor_vel=False, 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)

Fitted model with linear, second order rpy dynamics with thrust dynamics.

For info on the args, see above.

This wrapper converts the actual symbolic model, defined below, into the quat and ang_vel form.

Source code in drone_models/so_rpy_rotor/model.py 
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
def symbolic_dynamics(
    model_rotor_vel: bool = False,
    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]:
    """Fitted model with linear, second order rpy dynamics with thrust dynamics.

    For info on the args, see above.

    This wrapper converts the actual symbolic model, defined below, into the quat and ang_vel form.
    """
    ## We need to set the rpy and drpy symbols before building the euler model
    symbols.rpy = rotation.cs_quat2euler(symbols.quat)
    symbols.drpy = rotation.cs_ang_vel2rpy_rates(symbols.quat, symbols.ang_vel)
    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,
    )

    # 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, symbols.drpy, 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)

The fitted linear, second order rpy dynamics with thrust dynamics.

For info on the args, see above.

This function returns the actual model, as defined in the paper, for direct use.

Source code in drone_models/so_rpy_rotor/model.py 
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
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]:
    """The fitted linear, second order rpy dynamics with thrust dynamics.

    For info on the args, see above.

    This function returns the actual model, as defined in the paper, for direct use.
    """
    # 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

so_rpy_rotor_drag

SoRpyRotorDrag model.

TODO: Add description.

dynamics(pos, quat, vel, ang_vel, cmd, rotor_vel=None, dist_f=None, dist_t=None, *, mass, gravity_vec, J, J_inv, thrust_time_coef, acc_coef, cmd_f_coef, rpy_coef, rpy_rates_coef, cmd_rpy_coef, drag_linear_coef, drag_square_coef)

Fitted model with linear, second order rpy dynamics with thrust dynamics and drag.

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

Roll pitch yaw (rad) and collective thrust (N) command.

 required
rotor_vel Array | None

Speed of the 4 motors (RPMs). If None, the commanded thrust is directly applied (not recommended). If value is given, rotor 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
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
thrust_time_coef Array

Coefficient for the rotor dynamics (1/s).

 required
acc_coef Array

Coefficient for the acceleration (1/s^2).

 required
cmd_f_coef Array

Coefficient for the collective thrust (N/rad^2).

 required
rpy_coef Array

Coefficient for the roll pitch yaw dynamics (1/s).

 required
rpy_rates_coef Array

Coefficient for the roll pitch yaw rates dynamics (1/s^2).

 required
cmd_rpy_coef Array

Coefficient for the roll pitch yaw command dynamics (1/s).

 required
drag_linear_coef Array

Coefficient for the linear drag (1/s).

 required
drag_square_coef Array

Coefficient for the square drag (1/s).

 required

Returns:

Type Description
tuple[Array, Array, Array, Array, Array | None]

The derivatives of all state variables.
Source code in drone_models/so_rpy_rotor_drag/model.py 
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
@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,
    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,
    drag_linear_coef: Array,
    drag_square_coef: Array,
) -> tuple[Array, Array, Array, Array, Array | None]:
    """Fitted model with linear, second order rpy dynamics with thrust dynamics and drag.

    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: Roll pitch yaw (rad) and collective thrust (N) command.
        rotor_vel: Speed of the 4 motors (RPMs). If None, the commanded thrust is directly
            applied (not recommended). If value is given, rotor 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).
        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).
        thrust_time_coef: Coefficient for the rotor dynamics (1/s).
        acc_coef: Coefficient for the acceleration (1/s^2).
        cmd_f_coef: Coefficient for the collective thrust (N/rad^2).
        rpy_coef: Coefficient for the roll pitch yaw dynamics (1/s).
        rpy_rates_coef: Coefficient for the roll pitch yaw rates dynamics (1/s^2).
        cmd_rpy_coef: Coefficient for the roll pitch yaw command dynamics (1/s).
        drag_linear_coef: Coefficient for the linear drag (1/s).
        drag_square_coef: Coefficient for the square drag (1/s).

    Returns:
        The derivatives of all state variables.
    """
    xp = array_namespace(pos)
    device = xp_device(pos)
    # Convert constants to the correct framework and device
    mass, gravity_vec, J, J_inv = to_xp(mass, gravity_vec, J, J_inv, xp=xp, device=device)
    thrust_time_coef, acc_coef, cmd_f_coef = to_xp(
        thrust_time_coef, acc_coef, cmd_f_coef, xp=xp, device=device
    )
    rpy_coef, rpy_rates_coef, cmd_rpy_coef = to_xp(
        rpy_coef, rpy_rates_coef, cmd_rpy_coef, xp=xp, device=device
    )
    drag_linear_coef, drag_square_coef = to_xp(
        drag_linear_coef, drag_square_coef, xp=xp, device=device
    )
    cmd_f = cmd[..., -1]
    cmd_rpy = cmd[..., 0:3]
    rot = R.from_quat(quat)
    euler_angles = rot.as_euler("xyz")

    # Note that we are abusing the rotor_vel state as the thrust
    if rotor_vel is None:
        rotor_vel, rotor_vel_dot = cmd_f[..., None], None
    else:
        rotor_vel_dot = 1 / thrust_time_coef * (cmd_f[..., None] - rotor_vel)

    forces_motor = rotor_vel[..., 0]
    thrust = acc_coef + cmd_f_coef * forces_motor

    drone_z_axis = rot.inv().as_matrix()[..., -1, :]

    pos_dot = vel
    vel_dot = (
        1 / mass * thrust[..., None] * drone_z_axis
        + gravity_vec
        + 1 / mass * drag_linear_coef * vel
        + 1 / mass * drag_square_coef * vel * xp.abs(vel)
    )
    if dist_f is not None:
        vel_dot = vel_dot + dist_f / mass

    # Rotational equation of motion
    quat_dot = rotation.ang_vel2quat_dot(quat, ang_vel)
    rpy_rates = rotation.ang_vel2rpy_rates(quat, ang_vel)
    rpy_rates_dot = rpy_coef * euler_angles + rpy_rates_coef * rpy_rates + cmd_rpy_coef * cmd_rpy
    ang_vel_dot = rotation.rpy_rates_deriv2ang_vel_deriv(quat, rpy_rates, rpy_rates_dot)
    if dist_t is not None:
        # adding torque disturbances to the state
        # angular acceleration can be converted to total torque given the inertia matrix
        torque = (J @ ang_vel_dot[..., None])[..., 0]
        torque = torque + xp.linalg.cross(ang_vel, (J @ ang_vel[..., None])[..., 0])
        # adding torque. TODO: This should be a linear transformation. Can't we just transform the
        # disturbance torque to an ang_vel_dot summand directly?
        torque = torque + rot.apply(dist_t, inverse=True)
        # back to angular acceleration
        torque = torque - xp.linalg.cross(ang_vel, (J @ ang_vel[..., None])[..., 0])
        ang_vel_dot = (J_inv @ torque[..., 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, gravity_vec, J, J_inv, thrust_time_coef, acc_coef, cmd_f_coef, rpy_coef, rpy_rates_coef, cmd_rpy_coef, drag_linear_coef, drag_square_coef)

Fitted model with linear, second order rpy dynamics with thrust dynamics and drag.

For info on the args, see above.

This wrapper converts the actual symbolic model, defined below, into the quat and ang_vel form.

Source code in drone_models/so_rpy_rotor_drag/model.py 
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
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,
    drag_linear_coef: Array,
    drag_square_coef: Array,
) -> tuple[cs.MX, cs.MX, cs.MX, cs.MX]:
    """Fitted model with linear, second order rpy dynamics with thrust dynamics and drag.

    For info on the args, see above.

    This wrapper converts the actual symbolic model, defined below, into the quat and ang_vel form.
    """
    # We need to set the rpy and drpy symbols before building the euler model
    symbols.rpy = rotation.cs_quat2euler(symbols.quat)
    symbols.drpy = rotation.cs_ang_vel2rpy_rates(symbols.quat, symbols.ang_vel)
    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,
        drag_linear_coef=drag_linear_coef,
        drag_square_coef=drag_square_coef,
    )

    # 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, symbols.drpy, 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, drag_linear_coef, drag_square_coef)

The fitted linear, second order rpy dynamics with thrust dynamics and drag.

For info on the args, see above.

This function returns the actual model, as defined in the paper, for direct use.

Source code in drone_models/so_rpy_rotor_drag/model.py 
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
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,
    drag_linear_coef: Array,
    drag_square_coef: Array,
) -> tuple[cs.MX, cs.MX, cs.MX, cs.MX]:
    """The fitted linear, second order rpy dynamics with thrust dynamics and drag.

    For info on the args, see above.

    This function returns the actual model, as defined in the paper, for direct use.
    """
    # 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
        + 1 / mass * drag_linear_coef * symbols.vel
        + 1 / mass * drag_square_coef * symbols.vel * cs.fabs(symbols.vel)
    )

    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

model

TODO.

dynamics(pos, quat, vel, ang_vel, cmd, rotor_vel=None, dist_f=None, dist_t=None, *, mass, gravity_vec, J, J_inv, thrust_time_coef, acc_coef, cmd_f_coef, rpy_coef, rpy_rates_coef, cmd_rpy_coef, drag_linear_coef, drag_square_coef)

Fitted model with linear, second order rpy dynamics with thrust dynamics and drag.

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

Roll pitch yaw (rad) and collective thrust (N) command.

 required
rotor_vel Array | None

Speed of the 4 motors (RPMs). If None, the commanded thrust is directly applied (not recommended). If value is given, rotor 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
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
thrust_time_coef Array

Coefficient for the rotor dynamics (1/s).

 required
acc_coef Array

Coefficient for the acceleration (1/s^2).

 required
cmd_f_coef Array

Coefficient for the collective thrust (N/rad^2).

 required
rpy_coef Array

Coefficient for the roll pitch yaw dynamics (1/s).

 required
rpy_rates_coef Array

Coefficient for the roll pitch yaw rates dynamics (1/s^2).

 required
cmd_rpy_coef Array

Coefficient for the roll pitch yaw command dynamics (1/s).

 required
drag_linear_coef Array

Coefficient for the linear drag (1/s).

 required
drag_square_coef Array

Coefficient for the square drag (1/s).

 required

Returns:

Type Description
tuple[Array, Array, Array, Array, Array | None]

The derivatives of all state variables.
Source code in drone_models/so_rpy_rotor_drag/model.py 
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
@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,
    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,
    drag_linear_coef: Array,
    drag_square_coef: Array,
) -> tuple[Array, Array, Array, Array, Array | None]:
    """Fitted model with linear, second order rpy dynamics with thrust dynamics and drag.

    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: Roll pitch yaw (rad) and collective thrust (N) command.
        rotor_vel: Speed of the 4 motors (RPMs). If None, the commanded thrust is directly
            applied (not recommended). If value is given, rotor 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).
        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).
        thrust_time_coef: Coefficient for the rotor dynamics (1/s).
        acc_coef: Coefficient for the acceleration (1/s^2).
        cmd_f_coef: Coefficient for the collective thrust (N/rad^2).
        rpy_coef: Coefficient for the roll pitch yaw dynamics (1/s).
        rpy_rates_coef: Coefficient for the roll pitch yaw rates dynamics (1/s^2).
        cmd_rpy_coef: Coefficient for the roll pitch yaw command dynamics (1/s).
        drag_linear_coef: Coefficient for the linear drag (1/s).
        drag_square_coef: Coefficient for the square drag (1/s).

    Returns:
        The derivatives of all state variables.
    """
    xp = array_namespace(pos)
    device = xp_device(pos)
    # Convert constants to the correct framework and device
    mass, gravity_vec, J, J_inv = to_xp(mass, gravity_vec, J, J_inv, xp=xp, device=device)
    thrust_time_coef, acc_coef, cmd_f_coef = to_xp(
        thrust_time_coef, acc_coef, cmd_f_coef, xp=xp, device=device
    )
    rpy_coef, rpy_rates_coef, cmd_rpy_coef = to_xp(
        rpy_coef, rpy_rates_coef, cmd_rpy_coef, xp=xp, device=device
    )
    drag_linear_coef, drag_square_coef = to_xp(
        drag_linear_coef, drag_square_coef, xp=xp, device=device
    )
    cmd_f = cmd[..., -1]
    cmd_rpy = cmd[..., 0:3]
    rot = R.from_quat(quat)
    euler_angles = rot.as_euler("xyz")

    # Note that we are abusing the rotor_vel state as the thrust
    if rotor_vel is None:
        rotor_vel, rotor_vel_dot = cmd_f[..., None], None
    else:
        rotor_vel_dot = 1 / thrust_time_coef * (cmd_f[..., None] - rotor_vel)

    forces_motor = rotor_vel[..., 0]
    thrust = acc_coef + cmd_f_coef * forces_motor

    drone_z_axis = rot.inv().as_matrix()[..., -1, :]

    pos_dot = vel
    vel_dot = (
        1 / mass * thrust[..., None] * drone_z_axis
        + gravity_vec
        + 1 / mass * drag_linear_coef * vel
        + 1 / mass * drag_square_coef * vel * xp.abs(vel)
    )
    if dist_f is not None:
        vel_dot = vel_dot + dist_f / mass

    # Rotational equation of motion
    quat_dot = rotation.ang_vel2quat_dot(quat, ang_vel)
    rpy_rates = rotation.ang_vel2rpy_rates(quat, ang_vel)
    rpy_rates_dot = rpy_coef * euler_angles + rpy_rates_coef * rpy_rates + cmd_rpy_coef * cmd_rpy
    ang_vel_dot = rotation.rpy_rates_deriv2ang_vel_deriv(quat, rpy_rates, rpy_rates_dot)
    if dist_t is not None:
        # adding torque disturbances to the state
        # angular acceleration can be converted to total torque given the inertia matrix
        torque = (J @ ang_vel_dot[..., None])[..., 0]
        torque = torque + xp.linalg.cross(ang_vel, (J @ ang_vel[..., None])[..., 0])
        # adding torque. TODO: This should be a linear transformation. Can't we just transform the
        # disturbance torque to an ang_vel_dot summand directly?
        torque = torque + rot.apply(dist_t, inverse=True)
        # back to angular acceleration
        torque = torque - xp.linalg.cross(ang_vel, (J @ ang_vel[..., None])[..., 0])
        ang_vel_dot = (J_inv @ torque[..., 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, gravity_vec, J, J_inv, thrust_time_coef, acc_coef, cmd_f_coef, rpy_coef, rpy_rates_coef, cmd_rpy_coef, drag_linear_coef, drag_square_coef)

Fitted model with linear, second order rpy dynamics with thrust dynamics and drag.

For info on the args, see above.

This wrapper converts the actual symbolic model, defined below, into the quat and ang_vel form.

Source code in drone_models/so_rpy_rotor_drag/model.py 
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
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,
    drag_linear_coef: Array,
    drag_square_coef: Array,
) -> tuple[cs.MX, cs.MX, cs.MX, cs.MX]:
    """Fitted model with linear, second order rpy dynamics with thrust dynamics and drag.

    For info on the args, see above.

    This wrapper converts the actual symbolic model, defined below, into the quat and ang_vel form.
    """
    # We need to set the rpy and drpy symbols before building the euler model
    symbols.rpy = rotation.cs_quat2euler(symbols.quat)
    symbols.drpy = rotation.cs_ang_vel2rpy_rates(symbols.quat, symbols.ang_vel)
    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,
        drag_linear_coef=drag_linear_coef,
        drag_square_coef=drag_square_coef,
    )

    # 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, symbols.drpy, 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, drag_linear_coef, drag_square_coef)

The fitted linear, second order rpy dynamics with thrust dynamics and drag.

For info on the args, see above.

This function returns the actual model, as defined in the paper, for direct use.

Source code in drone_models/so_rpy_rotor_drag/model.py 
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
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,
    drag_linear_coef: Array,
    drag_square_coef: Array,
) -> tuple[cs.MX, cs.MX, cs.MX, cs.MX]:
    """The fitted linear, second order rpy dynamics with thrust dynamics and drag.

    For info on the args, see above.

    This function returns the actual model, as defined in the paper, for direct use.
    """
    # 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
        + 1 / mass * drag_linear_coef * symbols.vel
        + 1 / mass * drag_square_coef * symbols.vel * cs.fabs(symbols.vel)
    )

    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

symbols

Symbols used in the symbolic drone models.

pos = cs.vertcat(px, py, pz) module-attribute

Symbolic drone position.

Can be used to define symbolic CasADi expressions that are passed to model-based optimizers such as Acados.

:meta hide-value:

transform

Transformations between physical parameters of the quadrotors.

Conversions such as from motor forces to rotor speeds, or from thrust to PWM, are bundled in this module.

force2pwm(thrust, thrust_max, pwm_max)

Convert thrust in N to thrust in PWM.

Parameters:

Name Type Description Default
thrust Array | float

Array or float of the thrust in [N]

 required
thrust_max Array | float

Maximum thrust in [N]

 required
pwm_max Array | float

Maximum PWM value

 required

Returns:

Type Description
Array

Thrust converted in PWM.
Source code in drone_models/transform.py 
67
68
69
70
71
72
73
74
75
76
77
78
def force2pwm(thrust: Array | float, thrust_max: Array | float, pwm_max: Array | float) -> Array:
    """Convert thrust in N to thrust in PWM.

    Args:
        thrust: Array or float of the thrust in [N]
        thrust_max: Maximum thrust in [N]
        pwm_max: Maximum PWM value

    Returns:
        Thrust converted in PWM.
    """
    return thrust / thrust_max * pwm_max

motor_force2rotor_vel(motor_forces, rpm2thrust)

Convert motor forces to rotor velocities, where f=arpm^2+brpm+c.

Parameters:

Name Type Description Default
motor_forces Array

Motor forces in SI units with shape (..., N).

 required
rpm2thrust Array

RPM to thrust conversion factors.

 required

Returns:

Type Description
Array

Array of rotor velocities in rad/s with shape (..., N).
Source code in drone_models/transform.py 
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
def motor_force2rotor_vel(motor_forces: Array, rpm2thrust: Array) -> Array:
    """Convert motor forces to rotor velocities, where f=a*rpm^2+b*rpm+c.

    Args:
        motor_forces: Motor forces in SI units with shape (..., N).
        rpm2thrust: RPM to thrust conversion factors.

    Returns:
        Array of rotor velocities in rad/s with shape (..., N).
    """
    xp = array_namespace(motor_forces)
    return (
        -rpm2thrust[1]
        + xp.sqrt(rpm2thrust[1] ** 2 - 4 * rpm2thrust[2] * (rpm2thrust[0] - motor_forces))
    ) / (2 * rpm2thrust[2])

pwm2force(pwm, thrust_max, pwm_max)

Convert pwm thrust command to actual thrust.

Parameters:

Name Type Description Default
pwm Array | float

Array or float of the pwm value

 required
thrust_max Array | float

Maximum thrust in [N]

 required
pwm_max Array | float

Maximum PWM value

 required

Returns:

Name Type Description
thrust Array | float

Array or float thrust in [N]
Source code in drone_models/transform.py 
81
82
83
84
85
86
87
88
89
90
91
92
93
94
def pwm2force(
    pwm: Array | float, thrust_max: Array | float, pwm_max: Array | float
) -> Array | float:
    """Convert pwm thrust command to actual thrust.

    Args:
        pwm: Array or float of the pwm value
        thrust_max: Maximum thrust in [N]
        pwm_max: Maximum PWM value

    Returns:
        thrust: Array or float thrust in [N]
    """
    return pwm / pwm_max * thrust_max

rotor_vel2body_force(rotor_vel, rpm2thrust)

Convert rotor velocities to motor forces.

Source code in drone_models/transform.py 
35
36
37
38
39
40
41
42
43
44
45
def rotor_vel2body_force(rotor_vel: Array, rpm2thrust: Array) -> Array:
    """Convert rotor velocities to motor forces."""
    xp = array_namespace(rotor_vel)
    body_force = xp.zeros(rotor_vel.shape[:-1] + (3,), dtype=rotor_vel.dtype)
    body_force = xpx.at(body_force)[..., 2].set(
        xp.sum(
            rpm2thrust[..., 0] + rpm2thrust[..., 1] * rotor_vel + rpm2thrust[..., 2] * rotor_vel**2,
            axis=-1,
        )
    )
    return body_force

rotor_vel2body_torque(rotor_vel, rpm2thrust, rpm2torque, L, mixing_matrix)

Convert rotor velocities to motor torques.

Source code in drone_models/transform.py 
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
def rotor_vel2body_torque(
    rotor_vel: Array, rpm2thrust: Array, rpm2torque: Array, L: float | Array, mixing_matrix: Array
) -> Array:
    """Convert rotor velocities to motor torques."""
    xp = array_namespace(rotor_vel)
    forces = rpm2thrust[..., 0] + rpm2thrust[..., 1] * rotor_vel + rpm2thrust[..., 2] * rotor_vel**2
    torques_xy = (
        xp.stack([xp.zeros_like(forces), xp.zeros_like(forces), forces])
        @ mixing_matrix
        * xp.stack([L, L, 0])
    )
    torques = (
        rpm2torque[..., 0] + rpm2torque[..., 1] * rotor_vel + rpm2torque[..., 2] * rotor_vel**2
    )
    torques_z = xp.stack([xp.zeros_like(torques), xp.zeros_like(torques), torques])
    body_torque = torques_xy + torques_z
    return body_torque

utils

Utility functions for the drone models.

to_xp(*args, xp, device)

Convert all arrays in the argument list to the given xp framework and device.

Source code in drone_models/utils/__init__.py 
 9
10
11
12
13
14
def to_xp(*args: Any, xp: ModuleType, device: Any) -> tuple[Array, ...] | Array:
    """Convert all arrays in the argument list to the given xp framework and device."""
    result = tuple(xp.asarray(x, device=device) for x in args)
    if len(result) == 1:
        return result[0]
    return result

rotation

In this file, some important rotations from scipy.spatial.transform.rotation are reimplemented in the Array API to be useable with jax, numpy, etc.

https://github.com/scipy/scipy/blob/main/scipy/spatial/transform/_rotation.pyx https://github.com/jax-ml/jax/blob/main/jax/_src/scipy/spatial/transform.py

ang_vel2quat_dot(quat, ang_vel)

Calculates the quaternion derivative based on an angular velocity.

Source code in drone_models/utils/rotation.py 
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
def ang_vel2quat_dot(quat: Array, ang_vel: Array) -> Array:
    """Calculates the quaternion derivative based on an angular velocity."""
    xp = array_namespace(quat)
    # Split angular velocity
    x = ang_vel[..., 0:1]
    y = ang_vel[..., 1:2]
    z = ang_vel[..., 2:3]
    # Skew-symmetric matrix
    ang_vel_skew = xp.stack(
        [
            xp.concat((xp.zeros_like(x), -z, y), axis=-1),
            xp.concat((z, xp.zeros_like(x), -x), axis=-1),
            xp.concat((-y, x, xp.zeros_like(x)), axis=-1),
        ],
        axis=-2,
    )
    # First row of Xi
    xi1 = xp.concat((xp.zeros_like(x), -ang_vel), axis=-1)
    # Second to fourth rows of Xi
    ang_vel_col = xp.expand_dims(ang_vel, axis=-1)  # (..., 3, 1)
    xi2 = xp.concat((ang_vel_col, -ang_vel_skew), axis=-1)  # (..., 3, 4)
    # Combine into Xi
    xi1_exp = xp.expand_dims(xi1, axis=-2)  # (..., 1, 4)
    xi = xp.concat((xi1_exp, xi2), axis=-2)  # (..., 4, 4)
    # Quaternion derivative
    quat_exp = xp.expand_dims(quat, axis=-1)  # (..., 4, 1)
    result = 0.5 * xp.matmul(xi, quat_exp)  # (..., 4, 1)
    return xp.squeeze(result, axis=-1)  # (..., 4)

ang_vel2rpy_rates(quat, ang_vel)

Convert angular velocity to rpy rates with batch support.

Source code in drone_models/utils/rotation.py 
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
def ang_vel2rpy_rates(quat: Array, ang_vel: Array) -> Array:
    """Convert angular velocity to rpy rates with batch support."""
    xp = array_namespace(quat)
    rpy = R.from_quat(quat).as_euler("xyz")
    phi, theta = rpy[..., 0], rpy[..., 1]

    sin_phi = xp.sin(phi)
    cos_phi = xp.cos(phi)
    cos_theta = xp.cos(theta)
    tan_theta = xp.tan(theta)
    inv_cos_theta = 1 / cos_theta

    one = xp.ones_like(phi)
    zero = xp.zeros_like(phi)

    W = xp.stack(
        [
            xp.stack([one, sin_phi * tan_theta, cos_phi * tan_theta], axis=-1),
            xp.stack([zero, cos_phi, -sin_phi], axis=-1),
            xp.stack([zero, sin_phi * inv_cos_theta, cos_phi * inv_cos_theta], axis=-1),
        ],
        axis=-2,
    )

    return xp.matmul(W, ang_vel[..., None])[..., 0]

ang_vel_deriv2rpy_rates_deriv(quat, ang_vel, ang_vel_deriv)

Convert rpy rates derivatives to angular velocity derivatives.

.. math:: \dot{\psi} = \mathbf{\dot{W}}\mathbf{\omega} + \mathbf{W} \dot{\mathbf{\omega}}

Source code in drone_models/utils/rotation.py 
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
def ang_vel_deriv2rpy_rates_deriv(quat: Array, ang_vel: Array, ang_vel_deriv: Array) -> Array:
    r"""Convert rpy rates derivatives to angular velocity derivatives.

    .. math::
        \dot{\psi} = \mathbf{\dot{W}}\mathbf{\omega} + \mathbf{W} \dot{\mathbf{\omega}}
    """
    xp = quat.__array_namespace__()
    rpy = R.from_quat(quat).as_euler("xyz")
    phi, theta = rpy[..., 0], rpy[..., 1]
    rpy_rates = ang_vel2rpy_rates(quat, ang_vel)
    phi_dot, theta_dot = rpy_rates[..., 0], rpy_rates[..., 1]

    sin_phi = xp.sin(phi)
    cos_phi = xp.cos(phi)
    sin_theta = xp.sin(theta)
    cos_theta = xp.cos(theta)
    tan_theta = xp.tan(theta)

    zero = xp.zeros_like(phi)

    W_dot = xp.stack(
        [
            xp.stack(
                [
                    zero,
                    cos_phi * phi_dot * tan_theta + sin_phi * theta_dot / cos_theta**2,
                    -sin_phi * phi_dot * tan_theta + cos_phi * theta_dot / cos_theta**2,
                ],
                axis=-1,
            ),
            xp.stack([zero, -sin_phi * phi_dot, -cos_phi * phi_dot], axis=-1),
            xp.stack(
                [
                    zero,
                    cos_phi * phi_dot / cos_theta + sin_phi * theta_dot * sin_theta / cos_theta**2,
                    -sin_phi * phi_dot / cos_theta + cos_phi * sin_theta * theta_dot / cos_theta**2,
                ],
                axis=-1,
            ),
        ],
        axis=-2,
    )
    return xp.matmul(W_dot, ang_vel[..., None])[..., 0] + ang_vel2rpy_rates(quat, ang_vel_deriv)

create_cs_ang_vel2rpy_rates()

TODO.

Source code in drone_models/utils/rotation.py 
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
def create_cs_ang_vel2rpy_rates() -> cs.Function:
    """TODO."""
    qw = cs.MX.sym("qw")
    qx = cs.MX.sym("qx")
    qy = cs.MX.sym("qy")
    qz = cs.MX.sym("qz")
    quat = cs.vertcat(qx, qy, qz, qw)  # Quaternions
    rpy = cs_quat2euler(quat)
    phi, theta = rpy[0], rpy[1]
    p = cs.MX.sym("p")
    q = cs.MX.sym("q")
    r = cs.MX.sym("r")
    ang_vel = cs.vertcat(p, q, r)  # Angular velocity

    row1 = cs.horzcat(1, cs.sin(phi) * cs.tan(theta), cs.cos(phi) * cs.tan(theta))
    row2 = cs.horzcat(0, cs.cos(phi), -cs.sin(phi))
    row3 = cs.horzcat(0, cs.sin(phi) / cs.cos(theta), cs.cos(phi) / cs.cos(theta))

    W = cs.vertcat(row1, row2, row3)
    rpy_rates = W @ ang_vel

    return cs.Function("cs_ang_vel2rpy_rates", [quat, ang_vel], [rpy_rates])

create_cs_ang_vel_deriv2rpy_rates_deriv()

TODO.

Source code in drone_models/utils/rotation.py 
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
def create_cs_ang_vel_deriv2rpy_rates_deriv() -> cs.Function:
    """TODO."""
    qw = cs.MX.sym("qw")
    qx = cs.MX.sym("qx")
    qy = cs.MX.sym("qy")
    qz = cs.MX.sym("qz")
    quat = cs.vertcat(qx, qy, qz, qw)  # Quaternions
    rpy = cs_quat2euler(quat)
    phi, theta = rpy[0], rpy[1]
    p = cs.MX.sym("p")
    q = cs.MX.sym("q")
    r = cs.MX.sym("r")
    ang_vel = cs.vertcat(p, q, r)  # Angular velocity
    p_dot = cs.MX.sym("p_dot")
    q_dot = cs.MX.sym("q_dot")
    r_dot = cs.MX.sym("r_dot")
    ang_vel_deriv = cs.vertcat(p_dot, q_dot, r_dot)  # Angular acceleration
    rpy_rates = cs_ang_vel2rpy_rates(quat, ang_vel)
    phi_dot, theta_dot = rpy_rates[0], rpy_rates[1]

    row1 = cs.horzcat(
        0,
        cs.cos(phi) * phi_dot * cs.tan(theta) + cs.sin(phi) * theta_dot / cs.cos(theta) ** 2,
        -cs.sin(phi) * phi_dot * cs.tan(theta) + cs.cos(phi) * theta_dot / cs.cos(theta) ** 2,
    )
    row2 = cs.horzcat(0, -cs.sin(phi) * phi_dot, -cs.cos(phi) * phi_dot)
    row3 = cs.horzcat(
        0,
        cs.cos(phi) * phi_dot / cs.cos(theta)
        + cs.sin(phi) * theta_dot * cs.sin(theta) / cs.cos(theta) ** 2,
        -cs.sin(phi) * phi_dot / cs.cos(theta)
        + cs.cos(phi) * cs.sin(theta) * theta_dot / cs.cos(theta) ** 2,
    )

    W_dot = cs.vertcat(row1, row2, row3)
    rpy_rates_deriv = W_dot @ ang_vel + cs_ang_vel2rpy_rates(quat, ang_vel_deriv)

    return cs.Function("cs_ang_vel2rpy_rates", [quat, ang_vel, ang_vel_deriv], [rpy_rates_deriv])

create_cs_quat2matrix()

Generates a casadi numeric function from the cs_quat2matrix function.

Source code in drone_models/utils/rotation.py 
496
497
498
499
500
501
502
503
504
def create_cs_quat2matrix() -> cs.Function:
    """Generates a casadi numeric function from the cs_quat2matrix function."""
    qw = cs.MX.sym("qw")
    qx = cs.MX.sym("qx")
    qy = cs.MX.sym("qy")
    qz = cs.MX.sym("qz")
    quat = cs.vertcat(qx, qy, qz, qw)
    matrix = cs_quat2matrix(quat)
    return cs.Function("cs_quat2matrix", [quat], [matrix])

create_cs_rpy2matrix()

Generates a casadi numeric function from the cs_rpy2matrix function.

Source code in drone_models/utils/rotation.py 
510
511
512
513
514
515
516
517
def create_cs_rpy2matrix() -> cs.Function:
    """Generates a casadi numeric function from the cs_rpy2matrix function."""
    roll = cs.MX.sym("roll")
    pitch = cs.MX.sym("pitch")
    yaw = cs.MX.sym("yaw")
    rpy = cs.vertcat(roll, pitch, yaw)
    matrix = cs_rpy2matrix(rpy)
    return cs.Function("cs_rpy2matrix", [rpy], [matrix])

create_cs_rpy_rates2ang_vel()

TODO.

Source code in drone_models/utils/rotation.py 
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
def create_cs_rpy_rates2ang_vel() -> cs.Function:
    """TODO."""
    qw = cs.MX.sym("qw")
    qx = cs.MX.sym("qx")
    qy = cs.MX.sym("qy")
    qz = cs.MX.sym("qz")
    quat = cs.vertcat(qx, qy, qz, qw)  # Quaternions
    rpy = cs_quat2euler(quat)
    phi, theta = rpy[0], rpy[1]
    phi_dot = cs.MX.sym("phi_dot")
    theta_dot = cs.MX.sym("theta_dot")
    psi_dot = cs.MX.sym("psi_dot")
    rpy_rates = cs.vertcat(phi_dot, theta_dot, psi_dot)  # Euler rates

    row1 = cs.horzcat(1, 0, -cs.cos(theta) * cs.tan(theta))
    row2 = cs.horzcat(0, cs.cos(phi), cs.sin(phi) * cs.cos(theta))
    row3 = cs.horzcat(0, -cs.sin(phi), cs.cos(phi) * cs.cos(theta))

    W = cs.vertcat(row1, row2, row3)
    ang_vel = W @ rpy_rates
    return cs.Function("cs_rpy_rates2ang_vel", [quat, rpy_rates], [ang_vel])

create_cs_rpy_rates_deriv2ang_vel_deriv()

TODO.

Source code in drone_models/utils/rotation.py 
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
def create_cs_rpy_rates_deriv2ang_vel_deriv() -> cs.Function:
    """TODO."""
    qw = cs.MX.sym("qw")
    qx = cs.MX.sym("qx")
    qy = cs.MX.sym("qy")
    qz = cs.MX.sym("qz")
    quat = cs.vertcat(qx, qy, qz, qw)  # Quaternions
    rpy = cs_quat2euler(quat)
    phi, theta = rpy[0], rpy[1]
    phi_dot = cs.MX.sym("phi_dot")
    theta_dot = cs.MX.sym("theta_dot")
    psi_dot = cs.MX.sym("psi_dot")
    rpy_rates = cs.vertcat(phi_dot, theta_dot, psi_dot)  # Euler rates
    phi_dot_dot = cs.MX.sym("phi_dot_dot")
    theta_dot_dot = cs.MX.sym("theta_dot_dot")
    psi_dot_dot = cs.MX.sym("psi_dot_dot")
    rpy_rates_deriv = cs.vertcat(phi_dot_dot, theta_dot_dot, psi_dot_dot)  # Euler rates derivative

    row1 = cs.horzcat(0, 0, -cs.cos(theta) * theta_dot)
    row2 = cs.horzcat(
        0,
        -cs.sin(phi) * phi_dot,
        cs.cos(phi) * phi_dot * cs.cos(theta) - cs.sin(phi) * cs.sin(theta) * theta_dot,
    )
    row3 = cs.horzcat(
        0,
        -cs.cos(phi) * phi_dot,
        -cs.sin(phi) * phi_dot * cs.cos(theta) - cs.cos(phi) * cs.sin(theta) * theta_dot,
    )

    W_dot = cs.vertcat(row1, row2, row3)
    ang_vel_deriv = W_dot @ rpy_rates + cs_rpy_rates2ang_vel(quat, rpy_rates_deriv)

    return cs.Function("cs_ang_vel2rpy_rates", [quat, rpy_rates, rpy_rates_deriv], [ang_vel_deriv])

cs_quat2euler(quat, seq='xyz', degrees=False)

TODO.

Source code in drone_models/utils/rotation.py 
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
def cs_quat2euler(quat: cs.MX, seq: str = "xyz", degrees: bool = False) -> cs.MX:
    """TODO."""
    if len(seq) != 3:
        raise ValueError(f"Expected 3 axes, got {len(seq)}.")

    intrinsic = re.match(r"^[XYZ]{1,3}$", seq) is not None
    extrinsic = re.match(r"^[xyz]{1,3}$", seq) is not None

    if not (intrinsic or extrinsic):
        raise ValueError(
            "Expected axes from `seq` to be from ['x', 'y', 'z'] or ['X', 'Y', 'Z'], got {}".format(
                seq
            )
        )

    if any(seq[i] == seq[i + 1] for i in range(2)):
        raise ValueError("Expected consecutive axes to be different, got {}".format(seq))

    seq = seq.lower()

    # Compute euler from quat
    if extrinsic:
        angle_first = 0
        angle_third = 2
    else:
        seq = seq[::-1]
        angle_first = 2
        angle_third = 0

    def elementary_basis_index(axis: str) -> int:
        """TODO."""
        if axis == "x":
            return 0
        elif axis == "y":
            return 1
        else:
            return 2

    i = elementary_basis_index(seq[0])
    j = elementary_basis_index(seq[1])
    k = elementary_basis_index(seq[2])

    symmetric = i == k

    if symmetric:
        k = 3 - i - j  # get third axis

    # Check if permutation is even (+1) or odd (-1)
    sign = (i - j) * (j - k) * (k - i) // 2

    eps = 1e-7

    if symmetric:
        a = quat[3]
        b = quat[i]
        c = quat[j]
        d = quat[k] * sign
    else:
        a = quat[3] - quat[j]
        b = quat[i] + quat[k] * sign
        c = quat[j] + quat[3]
        d = quat[k] * sign - quat[i]

    angles1 = 2.0 * cs.arctan2(cs.sqrt(c**2 + d**2), cs.sqrt(a**2 + b**2))

    case = cs.if_else(
        cs.fabs(angles1) <= eps, 1, cs.if_else(cs.fabs(angles1 - cs.np.pi) <= eps, 2, 0)
    )

    half_sum = cs.arctan2(b, a)
    half_diff = cs.arctan2(d, c)

    angles_case_0_ = [None, angles1, None]
    angles_case_0_[angle_first] = half_sum - half_diff
    angles_case_0_[angle_third] = half_sum + half_diff
    angles_case_0 = cs.vertcat(*angles_case_0_)

    angles_case_else_ = [None, angles1, 0.0]
    angles_case_else_[0] = cs.if_else(
        case == 1, 2.0 * half_sum, 2.0 * half_diff * (-1.0 if extrinsic else 1.0)
    )
    angles_case_else = cs.vertcat(*angles_case_else_)

    angles = cs.if_else(case == 0, angles_case_0, angles_case_else)

    if not symmetric:
        angles[angle_third] *= sign
        angles[1] -= cs.np.pi * 0.5

    for i in range(3):
        angles[i] += cs.if_else(
            angles[i] < -cs.np.pi,
            2.0 * cs.np.pi,
            cs.if_else(angles[i] > cs.np.pi, -2.0 * cs.np.pi, 0.0),
        )

    if degrees:
        angles = (cs.np.pi / 180.0) * cs.horzcat(angles)

    return angles

cs_quat2matrix(quat)

Creates a symbolic rotation matrix based on a symbolic quaternion.

From https://github.com/cmower/spatial-casadi/blob/master/spatial_casadi/spatial.py

Source code in drone_models/utils/rotation.py 
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
def cs_quat2matrix(quat: cs.MX) -> cs.MX:
    """Creates a symbolic rotation matrix based on a symbolic quaternion.

    From https://github.com/cmower/spatial-casadi/blob/master/spatial_casadi/spatial.py
    """
    x = quat[0] / cs.norm_2(quat)
    y = quat[1] / cs.norm_2(quat)
    z = quat[2] / cs.norm_2(quat)
    w = quat[3] / cs.norm_2(quat)

    x2 = x * x
    y2 = y * y
    z2 = z * z
    w2 = w * w

    xy = x * y
    zw = z * w
    xz = x * z
    yw = y * w
    yz = y * z
    xw = x * w

    matrix = cs.horzcat(
        cs.vertcat(x2 - y2 - z2 + w2, 2.0 * (xy + zw), 2.0 * (xz - yw)),
        cs.vertcat(2.0 * (xy - zw), -x2 + y2 - z2 + w2, 2.0 * (yz + xw)),
        cs.vertcat(2.0 * (xz + yw), 2.0 * (yz - xw), -x2 - y2 + z2 + w2),
    )

    return matrix

cs_rpy2matrix(rpy, degrees=False)

Creates a symbolic rotation matrix from roll, pitch, yaw (XYZ convention).

Should be equivalent to scipy.spatial.transform.Rotation.from_euler('xyz', rpy).as_matrix() TODO write tests!!

Source code in drone_models/utils/rotation.py 
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
def cs_rpy2matrix(rpy: cs.MX, degrees: bool = False) -> cs.MX:
    """Creates a symbolic rotation matrix from roll, pitch, yaw (XYZ convention).

    Should be equivalent to scipy.spatial.transform.Rotation.from_euler('xyz', rpy).as_matrix()
    TODO write tests!!
    """
    roll, pitch, yaw = rpy[0], rpy[1], rpy[2]
    if degrees:
        roll *= cs.pi / 180
        pitch *= cs.pi / 180
        yaw *= cs.pi / 180

    cr = cs.cos(roll)
    sr = cs.sin(roll)
    cp = cs.cos(pitch)
    sp = cs.sin(pitch)
    cy = cs.cos(yaw)
    sy = cs.sin(yaw)

    # Rotation matrix for R = Rz(yaw) * Ry(pitch) * Rx(roll)
    matrix = cs.vertcat(
        cs.horzcat(cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr),
        cs.horzcat(sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr),
        cs.horzcat(-sp, cp * sr, cp * cr),
    )

    return matrix

rpy_rates2ang_vel(quat, rpy_rates)

Convert rpy rates to angular velocity with batch support.

Source code in drone_models/utils/rotation.py 
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
def rpy_rates2ang_vel(quat: Array, rpy_rates: Array) -> Array:
    """Convert rpy rates to angular velocity with batch support."""
    xp = quat.__array_namespace__()
    rpy = R.from_quat(quat).as_euler("xyz")
    phi, theta = rpy[..., 0], rpy[..., 1]

    sin_phi = xp.sin(phi)
    cos_phi = xp.cos(phi)
    cos_theta = xp.cos(theta)
    tan_theta = xp.tan(theta)

    one = xp.ones_like(phi)
    zero = xp.zeros_like(phi)

    W = xp.stack(
        [
            xp.stack([one, zero, -cos_theta * tan_theta], axis=-1),
            xp.stack([zero, cos_phi, sin_phi * cos_theta], axis=-1),
            xp.stack([zero, -sin_phi, cos_phi * cos_theta], axis=-1),
        ],
        axis=-2,
    )

    return xp.matmul(W, rpy_rates[..., None])[..., 0]

rpy_rates_deriv2ang_vel_deriv(quat, rpy_rates, rpy_rates_deriv)

Convert rpy rates derivatives to angular velocity derivatives.

.. math:: \dot{\omega} = \mathbf{\dot{W}}\dot{\mathbf{\psi}} + \mathbf{W} \ddot{\mathbf{\psi}}

Source code in drone_models/utils/rotation.py 
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
def rpy_rates_deriv2ang_vel_deriv(quat: Array, rpy_rates: Array, rpy_rates_deriv: Array) -> Array:
    r"""Convert rpy rates derivatives to angular velocity derivatives.

    .. math::
        \dot{\omega} = \mathbf{\dot{W}}\dot{\mathbf{\psi}} + \mathbf{W} \ddot{\mathbf{\psi}}
    """
    xp = quat.__array_namespace__()
    rpy = R.from_quat(quat).as_euler("xyz")
    phi, theta = rpy[..., 0], rpy[..., 1]
    phi_dot, theta_dot = rpy_rates[..., 0], rpy_rates[..., 1]

    sin_phi = xp.sin(phi)
    cos_phi = xp.cos(phi)
    sin_theta = xp.sin(theta)
    cos_theta = xp.cos(theta)

    zero = xp.zeros_like(phi)

    W_dot = xp.stack(
        [
            xp.stack([zero, zero, -cos_theta * theta_dot], axis=-1),
            xp.stack(
                [
                    zero,
                    -sin_phi * phi_dot,
                    cos_phi * phi_dot * cos_theta - sin_phi * sin_theta * theta_dot,
                ],
                axis=-1,
            ),
            xp.stack(
                [
                    zero,
                    -cos_phi * phi_dot,
                    -sin_phi * phi_dot * cos_theta - cos_phi * sin_theta * theta_dot,
                ],
                axis=-1,
            ),
        ],
        axis=-2,
    )
    return xp.matmul(W_dot, rpy_rates[..., None])[..., 0] + rpy_rates2ang_vel(quat, rpy_rates_deriv)