0. Obtain a semi-accurate CAD/SDF

Base link needs to be aligned to roughly the CG position. This is impossible to get right, and will be corrected for emperically.
Thruster positions should be accurately identified with respect to the base link position.

1. Tune the vehicle to be upright in the water

Adjust physical weights and floats on the vehicle to make it upright in the water.
Try to minimise weights if possible; these shift the CG position and make the vehicle heavier.
This process ensures that the COB is directly above the CG.

Since thruster positions are defined w.r.t base link, the force that results from the allocator is also applied through base link.
Rightfully, a force applied through the CG should never cause a change in orientation of the vehicle, because it should not produce a torque about the vehicle.
However, since there is almost certainly a mismatch between the base link and the CG, which will cause a torque \(\boldsymbol{\tau} = \mathbf{r} \times \mathbf{F}\) about the CG, resulting in the vehicle pitching/rolling/yawing even if we are requesting pure translational forces.
This will result in very harmful coupling amongst the axis, causing controls to end up oscillating.
Tuning the CG in this step accounts for this discrepancy.
To tune, send a large (saturating) translational force in one axis through the allocator (100N works), and then watch for the instantaneous movement of the vehicle (not the long term movement).

Note: You should not be observing the long-term behaviour of the vehicle (more than 3s), because at this point the vehicle has likely picked up enough speed for other nasty hydrodynamic coupling effects to come into play and start affecting the dynamics. So just look at the instantaneous reaction of the vehicle to a large force (up to 1s) to see how the CG needs to be adjusted.

Some people can reason about which direction to adjust the CG offset based on the observations they see. This is tricky to get right; I suggest just setting ridiculously large offsets in either direction (e.g. set \(+5\)cm and \(-5\)cm), see the direction the vehicle rolls/pitches/yaws at both offsets, and then binary search the answer from there – see which direction the vehicle turns at the midpoint of the range, then recursively halve the search bounds.
The same concept was applied to write a CG autotuner, but we have never tried it in the water. Somewhat works in the sim

Note: The controller still produces forces about base link which is not quite correct but doesnt matter because a lot bigger sources of error exist.

TODO

TODO

Should tune all with buoyancy controller on, since behaviour underwater and at surface is quite different.

TODO