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 controller function with the default controller parameters for a drone model.

Parameters:

Name Type Description Default
fn Callable[P, R]

The controller 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 
controller_fn = parametrize(state2attitude, drone_model="cf2x_L250")
command_rpyt, int_pos_err = controller_fn(
    pos=pos,
    quat=quat,
    vel=vel,
    ang_vel=ang_vel,
    cmd=cmd,
    ctrl_errors=(int_pos_err,),
    ctrl_freq=100,
)

Returns:

Type Description
Callable[P, R]

The parametrized controller function with all keyword argument only parameters filled in.
Source code in drone_models/core.py 
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
def parametrize(
    fn: Callable[P, R], drone_model: str, xp: ModuleType | None = None, device: str | None = None
) -> Callable[P, R]:
    """Parametrize a controller function with the default controller parameters for a drone model.

    Args:
        fn: The controller 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
        controller_fn = parametrize(state2attitude, drone_model="cf2x_L250")
        command_rpyt, int_pos_err = controller_fn(
            pos=pos,
            quat=quat,
            vel=vel,
            ang_vel=ang_vel,
            cmd=cmd,
            ctrl_errors=(int_pos_err,),
            ctrl_freq=100,
        )
        ```

    Returns:
        The parametrized controller function with all keyword argument only parameters filled in.
    """
    model_id = fn.__module__ + "." + fn.__name__
    try:
        params = model_parameter_registry[model_id].load(drone_model)
        if xp is not None:  # Convert to any array API framework
            params = named_tuple2xp(params, xp=xp, device=device)
    except KeyError as e:
        raise KeyError(
            f"Model `{model_id}` 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 `{model_id}`") from e
    return partial(fn, **params._asdict())

core

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

ModelParams

Bases: Protocol

Protocol for model parameters.

load(drone_model) staticmethod

Load the parameters from the config file.

Source code in drone_models/core.py 
104
105
106
@staticmethod
def load(drone_model: str) -> ModelParams:
    """Load the parameters from the config file."""

named_tuple2xp(params, xp, device=None)

Convert a named tuple to an array API framework.

Source code in drone_models/core.py 
51
52
53
54
55
def named_tuple2xp(params: ModelParams, xp: ModuleType, device: str | None = None) -> ModelParams:
    """Convert a named tuple to an array API framework."""
    return params.__class__(
        **{k: xp.asarray(v, device=device) for k, v in params._asdict().items()}
    )

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

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

Parameters:

Name Type Description Default
fn Callable[P, R]

The controller 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 
controller_fn = parametrize(state2attitude, drone_model="cf2x_L250")
command_rpyt, int_pos_err = controller_fn(
    pos=pos,
    quat=quat,
    vel=vel,
    ang_vel=ang_vel,
    cmd=cmd,
    ctrl_errors=(int_pos_err,),
    ctrl_freq=100,
)

Returns:

Type Description
Callable[P, R]

The parametrized controller function with all keyword argument only parameters filled in.
Source code in drone_models/core.py 
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
def parametrize(
    fn: Callable[P, R], drone_model: str, xp: ModuleType | None = None, device: str | None = None
) -> Callable[P, R]:
    """Parametrize a controller function with the default controller parameters for a drone model.

    Args:
        fn: The controller 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
        controller_fn = parametrize(state2attitude, drone_model="cf2x_L250")
        command_rpyt, int_pos_err = controller_fn(
            pos=pos,
            quat=quat,
            vel=vel,
            ang_vel=ang_vel,
            cmd=cmd,
            ctrl_errors=(int_pos_err,),
            ctrl_freq=100,
        )
        ```

    Returns:
        The parametrized controller function with all keyword argument only parameters filled in.
    """
    model_id = fn.__module__ + "." + fn.__name__
    try:
        params = model_parameter_registry[model_id].load(drone_model)
        if xp is not None:  # Convert to any array API framework
            params = named_tuple2xp(params, xp=xp, device=device)
    except KeyError as e:
        raise KeyError(
            f"Model `{model_id}` 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 `{model_id}`") from e
    return partial(fn, **params._asdict())

register_model_parameters(params)

Register the default model parameters for this model.

Warning

The model parameters must be a named tuple with a function load that takes in the drone model name and returns an instance of itself, or a class that implements the ModelParams protocol.

Parameters:

Name Type Description Default
params ModelParams | type[ModelParams]

The model parameter type.

 required

Returns:

Type Description
Callable[[Callable[P, R]], Callable[P, R]]

A decorator function that registers the parameters and returns the function unchanged.
Source code in drone_models/core.py 
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
def register_model_parameters(
    params: ModelParams | type[ModelParams],
) -> Callable[[Callable[P, R]], Callable[P, R]]:
    """Register the default model parameters for this model.

    Warning:
        The model parameters **must** be a named tuple with a function `load` that takes in the
        drone model name and returns an instance of itself, or a class that implements the
        ModelParams protocol.

    Args:
        params: The model parameter type.

    Returns:
        A decorator function that registers the parameters and returns the function unchanged.
    """
    if not isinstance(params, ModelParams):
        raise ValueError(f"{params} does not implement the ModelParams protocol")

    def decorator(fn: Callable[P, R]) -> Callable[P, R]:
        controller_id = fn.__module__ + "." + fn.__name__
        if controller_id in model_parameter_registry:
            raise ValueError(f"Model `{controller_id}` already registered")
        model_parameter_registry[controller_id] = params
        return fn

    return decorator

supports(rotor_dynamics=True)

Decorator to indicate which features are supported.

Source code in drone_models/core.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
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, KF, KM, L, mixing_matrix, thrust_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
KF float

Motor force constant (N/rad^2).

 required
KM float

Motor torque constant (Nm/rad^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
thrust_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 
 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
@register_model_parameters(FirstPrinciplesParams)
@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,
    KF: float,
    KM: float,
    L: float,
    mixing_matrix: Array,
    thrust_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).
        KF: Motor force constant (N/rad^2).
        KM: Motor torque constant (Nm/rad^2).
        L: Distance from the CoM to the motor (m).
        mixing_matrix: Mixing matrix denoting the turn direction of the motors (4x3).
        thrust_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, KF, KM, L, mixing_matrix, thrust_tau = to_xp(
        mass, gravity_vec, J, J_inv, KF, KM, L, mixing_matrix, thrust_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 / thrust_tau * (cmd - rotor_vel)  # - 1 / KM * rotor_vel**2
    # Creating force and torque vector
    forces_motor = KF * 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 = (mixing_matrix @ (rotor_vel**2)[..., None])[..., 0] * xp.stack([KF * L, KF * L, KM])

    # 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 = torque + rot.apply(dist_t, inverse=True)
    quat_dot = rotation.ang_vel2quat_dot(quat, ang_vel)
    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, KF, KM, L, mixing_matrix, thrust_tau)

TODO take from numeric.

Source code in drone_models/first_principles/model.py 
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
@register_model_parameters(FirstPrinciplesParams)
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,
    KF: float,
    KM: float,
    L: float,
    mixing_matrix: Array,
    thrust_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 / thrust_tau * (U - symbols.rotor_vel)  # - 1 / KM * symbols.rotor_vel**2
        forces_motor = KF * symbols.rotor_vel**2
    else:
        forces_motor = KF * U**2

    # Creating force and torque vector
    forces_motor_vec = cs.vertcat(0, 0, cs.sum1(forces_motor))
    torques_motor_vec = mixing_matrix @ forces_motor * cs.vertcat(L, L, KM / KF)

    # 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, KF, KM, L, mixing_matrix, thrust_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
KF float

Motor force constant (N/rad^2).

 required
KM float

Motor torque constant (Nm/rad^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
thrust_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 
 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
@register_model_parameters(FirstPrinciplesParams)
@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,
    KF: float,
    KM: float,
    L: float,
    mixing_matrix: Array,
    thrust_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).
        KF: Motor force constant (N/rad^2).
        KM: Motor torque constant (Nm/rad^2).
        L: Distance from the CoM to the motor (m).
        mixing_matrix: Mixing matrix denoting the turn direction of the motors (4x3).
        thrust_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, KF, KM, L, mixing_matrix, thrust_tau = to_xp(
        mass, gravity_vec, J, J_inv, KF, KM, L, mixing_matrix, thrust_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 / thrust_tau * (cmd - rotor_vel)  # - 1 / KM * rotor_vel**2
    # Creating force and torque vector
    forces_motor = KF * 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 = (mixing_matrix @ (rotor_vel**2)[..., None])[..., 0] * xp.stack([KF * L, KF * L, KM])

    # 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 = torque + rot.apply(dist_t, inverse=True)
    quat_dot = rotation.ang_vel2quat_dot(quat, ang_vel)
    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, KF, KM, L, mixing_matrix, thrust_tau)

TODO take from numeric.

Source code in drone_models/first_principles/model.py 
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
@register_model_parameters(FirstPrinciplesParams)
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,
    KF: float,
    KM: float,
    L: float,
    mixing_matrix: Array,
    thrust_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 / thrust_tau * (U - symbols.rotor_vel)  # - 1 / KM * symbols.rotor_vel**2
        forces_motor = KF * symbols.rotor_vel**2
    else:
        forces_motor = KF * U**2

    # Creating force and torque vector
    forces_motor_vec = cs.vertcat(0, 0, cs.sum1(forces_motor))
    torques_motor_vec = mixing_matrix @ forces_motor * cs.vertcat(L, L, KM / KF)

    # 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

params

First principles model parameters.

FirstPrinciplesParams

Bases: NamedTuple

Parameters for the FirstPrinciples model.

load(drone_model) staticmethod

Load the parameters for the drone model from the params.toml file.

Source code in drone_models/first_principles/params.py 
28
29
30
31
32
33
34
35
36
37
@staticmethod
def load(drone_model: str) -> FirstPrinciplesParams:
    """Load the parameters for the drone model from the params.toml file."""
    with open(Path(__file__).parent / "params.toml", "rb") as f:
        params = tomllib.load(f)
    if drone_model not in params:
        raise KeyError(f"Drone model `{drone_model}` not found in params.toml")
    params = {k: np.asarray(v) for k, v in params[drone_model].items()}
    params["J_inv"] = np.linalg.inv(params["J"])
    return FirstPrinciplesParams(**params)

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 double integrator (DI) model with optional motor delay (D).

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
108
@register_model_parameters(SoRpyParams)
@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 double integrator (DI) model with optional motor delay (D).

    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)

The fitted double integrator (DI) model with optional motor delay (D).

TODO.

Source code in drone_models/so_rpy/model.py 
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
@register_model_parameters(SoRpyParams)
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]:
    """The fitted double integrator (DI) model with optional motor delay (D).

    TODO.
    """
    # 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 = cs.vertcat(symbols.cmd_roll, symbols.cmd_pitch, symbols.cmd_yaw, symbols.cmd_thrust)

    # Defining the dynamics function
    # Creating force vector
    forces_motor_vec = cs.vertcat(0, 0, acc_coef + cmd_f_coef * symbols.cmd_thrust)

    # Linear equation of motion
    pos_dot = symbols.vel
    vel_dot = symbols.rot @ forces_motor_vec / mass + gravity_vec
    if model_dist_f:
        # Adding force disturbances to the state
        vel_dot = vel_dot + symbols.dist_f / mass

    # Rotational equation of motion
    euler_angles = rotation.cs_quat2euler(symbols.quat)

    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)
    rpy_rates = rotation.cs_ang_vel2rpy_rates(symbols.quat, symbols.ang_vel)
    rpy_rates_dot = (
        rpy_coef * euler_angles
        + rpy_rates_coef * rpy_rates
        + cmd_rpy_coef * cs.vertcat(symbols.cmd_roll, symbols.cmd_pitch, symbols.cmd_yaw)
    )
    ang_vel_dot = rotation.cs_rpy_rates_deriv2ang_vel_deriv(symbols.quat, rpy_rates, rpy_rates_dot)
    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

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 double integrator (DI) model with optional motor delay (D).

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
108
@register_model_parameters(SoRpyParams)
@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 double integrator (DI) model with optional motor delay (D).

    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)

The fitted double integrator (DI) model with optional motor delay (D).

TODO.

Source code in drone_models/so_rpy/model.py 
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
@register_model_parameters(SoRpyParams)
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]:
    """The fitted double integrator (DI) model with optional motor delay (D).

    TODO.
    """
    # 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 = cs.vertcat(symbols.cmd_roll, symbols.cmd_pitch, symbols.cmd_yaw, symbols.cmd_thrust)

    # Defining the dynamics function
    # Creating force vector
    forces_motor_vec = cs.vertcat(0, 0, acc_coef + cmd_f_coef * symbols.cmd_thrust)

    # Linear equation of motion
    pos_dot = symbols.vel
    vel_dot = symbols.rot @ forces_motor_vec / mass + gravity_vec
    if model_dist_f:
        # Adding force disturbances to the state
        vel_dot = vel_dot + symbols.dist_f / mass

    # Rotational equation of motion
    euler_angles = rotation.cs_quat2euler(symbols.quat)

    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)
    rpy_rates = rotation.cs_ang_vel2rpy_rates(symbols.quat, symbols.ang_vel)
    rpy_rates_dot = (
        rpy_coef * euler_angles
        + rpy_rates_coef * rpy_rates
        + cmd_rpy_coef * cs.vertcat(symbols.cmd_roll, symbols.cmd_pitch, symbols.cmd_yaw)
    )
    ang_vel_dot = rotation.cs_rpy_rates_deriv2ang_vel_deriv(symbols.quat, rpy_rates, rpy_rates_dot)
    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

params

TODO.

SoRpyParams

Bases: NamedTuple

Parameters for the SoRpy model.

load(drone_model) staticmethod

Load the parameters for the drone model from the params.toml file.

Source code in drone_models/so_rpy/params.py 
28
29
30
31
32
33
34
35
36
37
@staticmethod
def load(drone_model: str) -> SoRpyParams:
    """Load the parameters for the drone model from the params.toml file."""
    with open(Path(__file__).parent / "params.toml", "rb") as f:
        params = tomllib.load(f)
    if drone_model not in params:
        raise KeyError(f"Drone model `{drone_model}` not found in params.toml")
    params = {k: np.asarray(v) for k, v in params[drone_model].items()}
    params["J_inv"] = np.linalg.inv(params["J"])
    return SoRpyParams(**params)

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, KF, KM, rotor_coef, acc_coef, cmd_f_coef, rpy_coef, rpy_rates_coef, cmd_rpy_coef)

The fitted double integrator (DI) model with optional motor delay (D).

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

Motor force constant (N/rad^2).

 required
KM Array

Motor torque constant (Nm/rad^2).

 required
rotor_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 
 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
125
126
127
128
@register_model_parameters(SoRpyRotorParams)
@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,
    KF: Array,
    KM: Array,
    rotor_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]:
    """The fitted double integrator (DI) model with optional motor delay (D).

    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).
        KF: Motor force constant (N/rad^2).
        KM: Motor torque constant (Nm/rad^2).
        rotor_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, KF, KM, J, J_inv = to_xp(
        mass, gravity_vec, KF, KM, J, J_inv, xp=xp, device=device
    )
    rotor_coef, acc_coef, cmd_f_coef = to_xp(rotor_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_rotor_vel = motor_force2rotor_vel(cmd_f / 4, KF)
    cmd_rpy = cmd[..., 0:3]
    rot = R.from_quat(quat)
    euler_angles = rot.as_euler("xyz")

    if rotor_vel is None:
        rotor_vel, rotor_vel_dot = cmd_rotor_vel[..., None], None
    else:
        rotor_vel_dot = 1 / rotor_coef * (cmd_rotor_vel[..., None] - rotor_vel) - KM * rotor_vel**2

    forces_motor = KF * xp.sum(rotor_vel**2, axis=-1)
    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, KF, KM, rotor_coef, acc_coef, cmd_f_coef, rpy_coef, rpy_rates_coef, cmd_rpy_coef)

The fitted double integrator (DI) model with optional motor delay (D).

TODO.

Source code in drone_models/so_rpy_rotor/model.py 
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
204
205
206
207
208
209
210
@register_model_parameters(SoRpyRotorParams)
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,
    KF: Array,
    KM: Array,
    rotor_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 double integrator (DI) model with optional motor delay (D).

    TODO.
    """
    # 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 = cs.vertcat(symbols.cmd_roll, symbols.cmd_pitch, symbols.cmd_yaw, symbols.cmd_thrust)
    cmd_rpy = cs.vertcat(symbols.cmd_roll, symbols.cmd_pitch, symbols.cmd_yaw)

    # Defining the dynamics function
    if model_rotor_vel:
        # motor_force2rotor_vel
        cmd_rotor_vel = cs.sqrt(symbols.cmd_thrust / 4 / KF)
        rotor_vel_dot = (
            1 / rotor_coef * (cmd_rotor_vel - symbols.rotor_vel) - KM * symbols.rotor_vel**2
        )
        forces_motor = KF * cs.sum1(symbols.rotor_vel**2)
    else:
        forces_motor = symbols.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 = symbols.rot @ forces_motor_vec / mass + gravity_vec
    if model_dist_f:
        # Adding force disturbances to the state
        vel_dot = vel_dot + symbols.dist_f / mass

    # Rotational equation of motion
    euler_angles = rotation.cs_quat2euler(symbols.quat)

    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)
    rpy_rates = rotation.cs_ang_vel2rpy_rates(symbols.quat, symbols.ang_vel)
    rpy_rates_dot = rpy_coef * euler_angles + rpy_rates_coef * rpy_rates + cmd_rpy_coef * cmd_rpy
    ang_vel_dot = rotation.cs_rpy_rates_deriv2ang_vel_deriv(symbols.quat, rpy_rates, rpy_rates_dot)
    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, 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, KF, KM, rotor_coef, acc_coef, cmd_f_coef, rpy_coef, rpy_rates_coef, cmd_rpy_coef)

The fitted double integrator (DI) model with optional motor delay (D).

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

Motor force constant (N/rad^2).

 required
KM Array

Motor torque constant (Nm/rad^2).

 required
rotor_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 
 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
125
126
127
128
@register_model_parameters(SoRpyRotorParams)
@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,
    KF: Array,
    KM: Array,
    rotor_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]:
    """The fitted double integrator (DI) model with optional motor delay (D).

    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).
        KF: Motor force constant (N/rad^2).
        KM: Motor torque constant (Nm/rad^2).
        rotor_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, KF, KM, J, J_inv = to_xp(
        mass, gravity_vec, KF, KM, J, J_inv, xp=xp, device=device
    )
    rotor_coef, acc_coef, cmd_f_coef = to_xp(rotor_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_rotor_vel = motor_force2rotor_vel(cmd_f / 4, KF)
    cmd_rpy = cmd[..., 0:3]
    rot = R.from_quat(quat)
    euler_angles = rot.as_euler("xyz")

    if rotor_vel is None:
        rotor_vel, rotor_vel_dot = cmd_rotor_vel[..., None], None
    else:
        rotor_vel_dot = 1 / rotor_coef * (cmd_rotor_vel[..., None] - rotor_vel) - KM * rotor_vel**2

    forces_motor = KF * xp.sum(rotor_vel**2, axis=-1)
    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, KF, KM, rotor_coef, acc_coef, cmd_f_coef, rpy_coef, rpy_rates_coef, cmd_rpy_coef)

The fitted double integrator (DI) model with optional motor delay (D).

TODO.

Source code in drone_models/so_rpy_rotor/model.py 
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
204
205
206
207
208
209
210
@register_model_parameters(SoRpyRotorParams)
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,
    KF: Array,
    KM: Array,
    rotor_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 double integrator (DI) model with optional motor delay (D).

    TODO.
    """
    # 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 = cs.vertcat(symbols.cmd_roll, symbols.cmd_pitch, symbols.cmd_yaw, symbols.cmd_thrust)
    cmd_rpy = cs.vertcat(symbols.cmd_roll, symbols.cmd_pitch, symbols.cmd_yaw)

    # Defining the dynamics function
    if model_rotor_vel:
        # motor_force2rotor_vel
        cmd_rotor_vel = cs.sqrt(symbols.cmd_thrust / 4 / KF)
        rotor_vel_dot = (
            1 / rotor_coef * (cmd_rotor_vel - symbols.rotor_vel) - KM * symbols.rotor_vel**2
        )
        forces_motor = KF * cs.sum1(symbols.rotor_vel**2)
    else:
        forces_motor = symbols.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 = symbols.rot @ forces_motor_vec / mass + gravity_vec
    if model_dist_f:
        # Adding force disturbances to the state
        vel_dot = vel_dot + symbols.dist_f / mass

    # Rotational equation of motion
    euler_angles = rotation.cs_quat2euler(symbols.quat)

    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)
    rpy_rates = rotation.cs_ang_vel2rpy_rates(symbols.quat, symbols.ang_vel)
    rpy_rates_dot = rpy_coef * euler_angles + rpy_rates_coef * rpy_rates + cmd_rpy_coef * cmd_rpy
    ang_vel_dot = rotation.cs_rpy_rates_deriv2ang_vel_deriv(symbols.quat, rpy_rates, rpy_rates_dot)
    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, 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

params

TODO.

SoRpyRotorParams

Bases: NamedTuple

Parameters for the SoRpyRotor model.

load(drone_model) staticmethod

Load the parameters for the drone model from the params.toml file.

Source code in drone_models/so_rpy_rotor/params.py 
31
32
33
34
35
36
37
38
39
40
@staticmethod
def load(drone_model: str) -> SoRpyRotorParams:
    """Load the parameters for the drone model from the params.toml file."""
    with open(Path(__file__).parent / "params.toml", "rb") as f:
        params = tomllib.load(f)
    if drone_model not in params:
        raise KeyError(f"Drone model `{drone_model}` not found in params.toml")
    params = {k: np.asarray(v) for k, v in params[drone_model].items()}
    params["J_inv"] = np.linalg.inv(params["J"])
    return SoRpyRotorParams(**params)

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, KF, KM, thrust_time_coef, acc_coef, cmd_f_coef, rpy_coef, rpy_rates_coef, cmd_rpy_coef, drag_linear_coef, drag_square_coef)

The fitted double integrator (DI) model with optional motor delay (D).

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

Motor force constant (N/rad^2).

 required
KM Array

Motor torque constant (Nm/rad^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 
 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
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
@register_model_parameters(SoRpyRotorDragParams)
@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,
    KF: Array,
    KM: 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]:
    """The fitted double integrator (DI) model with optional motor delay (D).

    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).
        KF: Motor force constant (N/rad^2).
        KM: Motor torque constant (Nm/rad^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, KF, KM, J, J_inv = to_xp(
        mass, gravity_vec, KF, KM, 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_rotor_vel = motor_force2rotor_vel(cmd_f / 4, KF)
    cmd_rotor_vel = cmd_f  # this is just a hack for testing TODO remove
    cmd_rpy = cmd[..., 0:3]
    rot = R.from_quat(quat)
    euler_angles = rot.as_euler("xyz")

    if rotor_vel is None:
        rotor_vel, rotor_vel_dot = cmd_rotor_vel[..., None], None
    else:
        rotor_vel_dot = (
            1 / thrust_time_coef * (cmd_rotor_vel[..., None] - rotor_vel)
        )  # - KM * rotor_vel**2

    # forces_motor = KF * xp.sum(rotor_vel**2, axis=-1)
    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, KF, KM, thrust_time_coef, acc_coef, cmd_f_coef, rpy_coef, rpy_rates_coef, cmd_rpy_coef, drag_linear_coef, drag_square_coef)

The fitted double integrator (DI) model with optional motor delay (D).

TODO.

Source code in drone_models/so_rpy_rotor_drag/model.py 
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
225
226
227
228
229
230
231
232
233
234
235
@register_model_parameters(SoRpyRotorDragParams)
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,
    KF: Array,
    KM: 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 double integrator (DI) model with optional motor delay (D).

    TODO.
    """
    # 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 = cs.vertcat(symbols.cmd_roll, symbols.cmd_pitch, symbols.cmd_yaw, symbols.cmd_thrust)
    cmd_rpy = cs.vertcat(symbols.cmd_roll, symbols.cmd_pitch, symbols.cmd_yaw)

    # Defining the dynamics function
    if model_rotor_vel:
        # motor_force2rotor_vel
        # cmd_rotor_vel = cs.sqrt(symbols.cmd_thrust / 4 / KF)
        cmd_rotor_vel = symbols.cmd_thrust  # this is just a hack for testing TODO remove
        rotor_vel_dot = (
            1
            / thrust_time_coef
            * (cmd_rotor_vel - symbols.rotor_vel)  # - KM * symbols.rotor_vel**2
        )
        # forces_motor = KF * cs.sum1(symbols.rotor_vel**2)
        forces_motor = symbols.rotor_vel[0]
    else:
        forces_motor = symbols.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 = (
        symbols.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)
    )
    if model_dist_f:
        # Adding force disturbances to the state
        vel_dot = vel_dot + symbols.dist_f / mass

    # Rotational equation of motion
    euler_angles = rotation.cs_quat2euler(symbols.quat)

    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)
    rpy_rates = rotation.cs_ang_vel2rpy_rates(symbols.quat, symbols.ang_vel)
    rpy_rates_dot = rpy_coef * euler_angles + rpy_rates_coef * rpy_rates + cmd_rpy_coef * cmd_rpy
    ang_vel_dot = rotation.cs_rpy_rates_deriv2ang_vel_deriv(symbols.quat, rpy_rates, rpy_rates_dot)
    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, 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, KF, KM, thrust_time_coef, acc_coef, cmd_f_coef, rpy_coef, rpy_rates_coef, cmd_rpy_coef, drag_linear_coef, drag_square_coef)

The fitted double integrator (DI) model with optional motor delay (D).

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

Motor force constant (N/rad^2).

 required
KM Array

Motor torque constant (Nm/rad^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 
 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
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
@register_model_parameters(SoRpyRotorDragParams)
@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,
    KF: Array,
    KM: 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]:
    """The fitted double integrator (DI) model with optional motor delay (D).

    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).
        KF: Motor force constant (N/rad^2).
        KM: Motor torque constant (Nm/rad^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, KF, KM, J, J_inv = to_xp(
        mass, gravity_vec, KF, KM, 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_rotor_vel = motor_force2rotor_vel(cmd_f / 4, KF)
    cmd_rotor_vel = cmd_f  # this is just a hack for testing TODO remove
    cmd_rpy = cmd[..., 0:3]
    rot = R.from_quat(quat)
    euler_angles = rot.as_euler("xyz")

    if rotor_vel is None:
        rotor_vel, rotor_vel_dot = cmd_rotor_vel[..., None], None
    else:
        rotor_vel_dot = (
            1 / thrust_time_coef * (cmd_rotor_vel[..., None] - rotor_vel)
        )  # - KM * rotor_vel**2

    # forces_motor = KF * xp.sum(rotor_vel**2, axis=-1)
    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, KF, KM, thrust_time_coef, acc_coef, cmd_f_coef, rpy_coef, rpy_rates_coef, cmd_rpy_coef, drag_linear_coef, drag_square_coef)

The fitted double integrator (DI) model with optional motor delay (D).

TODO.

Source code in drone_models/so_rpy_rotor_drag/model.py 
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
225
226
227
228
229
230
231
232
233
234
235
@register_model_parameters(SoRpyRotorDragParams)
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,
    KF: Array,
    KM: 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 double integrator (DI) model with optional motor delay (D).

    TODO.
    """
    # 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 = cs.vertcat(symbols.cmd_roll, symbols.cmd_pitch, symbols.cmd_yaw, symbols.cmd_thrust)
    cmd_rpy = cs.vertcat(symbols.cmd_roll, symbols.cmd_pitch, symbols.cmd_yaw)

    # Defining the dynamics function
    if model_rotor_vel:
        # motor_force2rotor_vel
        # cmd_rotor_vel = cs.sqrt(symbols.cmd_thrust / 4 / KF)
        cmd_rotor_vel = symbols.cmd_thrust  # this is just a hack for testing TODO remove
        rotor_vel_dot = (
            1
            / thrust_time_coef
            * (cmd_rotor_vel - symbols.rotor_vel)  # - KM * symbols.rotor_vel**2
        )
        # forces_motor = KF * cs.sum1(symbols.rotor_vel**2)
        forces_motor = symbols.rotor_vel[0]
    else:
        forces_motor = symbols.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 = (
        symbols.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)
    )
    if model_dist_f:
        # Adding force disturbances to the state
        vel_dot = vel_dot + symbols.dist_f / mass

    # Rotational equation of motion
    euler_angles = rotation.cs_quat2euler(symbols.quat)

    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)
    rpy_rates = rotation.cs_ang_vel2rpy_rates(symbols.quat, symbols.ang_vel)
    rpy_rates_dot = rpy_coef * euler_angles + rpy_rates_coef * rpy_rates + cmd_rpy_coef * cmd_rpy
    ang_vel_dot = rotation.cs_rpy_rates_deriv2ang_vel_deriv(symbols.quat, rpy_rates, rpy_rates_dot)
    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, 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

params

SoRpyRotorDrag model parameters.

SoRpyRotorDragParams

Bases: NamedTuple

Parameters for the SoRpyRotorDrag model.

load(drone_model) staticmethod

Load the parameters for the drone model from the params.toml file.

Source code in drone_models/so_rpy_rotor_drag/params.py 
33
34
35
36
37
38
39
40
41
42
@staticmethod
def load(drone_model: str) -> SoRpyRotorDragParams:
    """Load the parameters for the drone model from the params.toml file."""
    with open(Path(__file__).parent / "params.toml", "rb") as f:
        params = tomllib.load(f)
    if drone_model not in params:
        raise KeyError(f"Drone model `{drone_model}` not found in params.toml")
    params = {k: np.asarray(v) for k, v in params[drone_model].items()}
    params["J_inv"] = np.linalg.inv(params["J"])
    return SoRpyRotorDragParams(**params)

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 
49
50
51
52
53
54
55
56
57
58
59
60
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, kf)

Convert motor forces to rotor velocities.

Parameters:

Name Type Description Default
motor_forces Array

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

 required
kf float | Array

Thrust coefficient.

 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
def motor_force2rotor_vel(motor_forces: Array, kf: float | Array) -> Array:
    """Convert motor forces to rotor velocities.

    Args:
        motor_forces: Motor forces in SI units with shape (..., N).
        kf: Thrust coefficient.

    Returns:
        Array of rotor velocities in rad/s with shape (..., N).
    """
    xp = array_namespace(motor_forces)
    return xp.where(motor_forces == 0, 0, xp.sqrt(motor_forces / kf))

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 
63
64
65
66
67
68
69
70
71
72
73
74
75
76
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, kf)

Convert rotor velocities to motor forces.

Source code in drone_models/transform.py 
32
33
34
35
36
37
def rotor_vel2body_force(rotor_vel: Array, kf: float | 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(kf * rotor_vel**2, axis=-1))
    return body_force

rotor_vel2body_torque(rotor_vel, kf, km, L, mixing_matrix)

Convert rotor velocities to motor torques.

Source code in drone_models/transform.py 
40
41
42
43
44
45
46
def rotor_vel2body_torque(
    rotor_vel: Array, kf: float | Array, km: float | Array, L: float | Array, mixing_matrix: Array
) -> Array:
    """Convert rotor velocities to motor torques."""
    xp = array_namespace(rotor_vel)
    body_torque = rotor_vel**2 * kf @ mixing_matrix * xp.stack([L, L, km / kf])
    return body_torque

utils

Utility functions for the drone models and controllers.

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 
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
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 
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
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 
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
139
140
141
142
143
144
145
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 
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
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 
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
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_rpy_rates2ang_vel()

TODO.

Source code in drone_models/utils/rotation.py 
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
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 
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
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 
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
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
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 
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
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

rpy_rates2ang_vel(quat, rpy_rates)

Convert rpy rates to angular velocity with batch support.

Source code in drone_models/utils/rotation.py 
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
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 
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
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)