Feature/downwash

[rducrist]

This implements downwash as an external wrench. It uses the far-field velocity model from https://arxiv.org/pdf/2403.13321 and the thrust loss computation from https://arxiv.org/pdf/2207.09645.
It works like this:

For each source-target rotor pair, the target rotor’s world position is

$$ \mathbf p_j^W = \mathbf p_{\mathrm{drone}}^W + R_{W\leftarrow B}\mathbf r_j^B. $$

The relative displacement from source to target rotor gives axial separation $$s$$ and lateral distance $$r$$. The far-field velocity model is

$$ \bar{s} = \frac{s}{d}, \qquad h = S(\bar{s}-S_0), \qquad U_c = U_H\frac{B_D}{\bar{s}-S_0}, $$

$$ \xi = \frac{r/d}{h}, \qquad U_D = \frac{U_c} {\left(1+(\sqrt{2}-1)\xi^2\right)^2}. $$

All downwash source velocities are summed to obtain $U_{D,j}$ at each target rotor.

Using the thrust-decay model,

$$ \Delta t_j = -b_v U_{D,j},t_j, \qquad t_j = k_0 + k_1\omega_j + k_2\omega_j^2. $$

Note that $-b_v$ is the thrust loss coefficient. It has to be fitted normally. However for proof of concept I went with a value suggested by AI.

The four individual thrust losses are mapped into a body-frame wrench:

$$ \Delta T = \sum_j \Delta t_j, \qquad \Delta\boldsymbol{\tau}^B = \begin{bmatrix} \Delta\tau_x\\ \Delta\tau_y\\ \Delta\tau_z \end{bmatrix}. $$

Roll/pitch torque comes from the motor arms and mixing matrix; yaw torque comes from the corresponding change in propeller reaction torque. Finally, both are rotated into the world frame and written to states.force and states.torque.

The experiment consists of a drone hovering and the other one flying forth and back underneath the downwash cone at different heights.

The resulting downwash cone of the hovering drone looks like this

[rducrist]

Screencast.from.24.09.2026.11.17.34.webm

[ratheron]

Thank for for working on this!

In general, the approach is sound. I've added some minor comments. Would be nice to have the specific equations such that it's easier to understand whats going on.

[ratheron]

The offsets are set freely by you, right? So we could just use the mixing matrix without sign changes and order flips, no?

[ratheron]

Why don't we use rotation.apply() here?

[ratheron]

Suggested change

	rotor_offsets_world = (R_body_to_world.as_matrix() @ offsets).mT
	rotor_offsets_world = R_body_to_world.apply(offsets)).as_matrix()

If we need the inverse, use .inverse()

[rducrist]

We can use apply here, it's just not as simple as written here. Since offsets is shape (3, 4) and R_body_to_world (1,2,4), we'll encounter some broadcasting issues.