Singularity check implementation

[incebellipipo]

With the new things I learn, I propose this to myself:

We simply compute $f$ by solving the linear equation below with quadratic programming, $\min_f f^\top B^\top B f + (-2{\tau^*}^\top B) f$ ,to do so since overactuated thruster configuration can have multiple solutions to the same problem.
$\tau = B(\alpha) f$

We can simply check the $\text{Rank}(B(\alpha))$ against the controlled degree of freedoms of the mode. If it doesn't apply, simply do not switch to that mode.

Matrix $B(\alpha)\in \mathbb{R}^{n \times r}$ is full rank if $n=\text{Rank}(B(\alpha))$. Note that the $B$ matrix is constantly being updated by the control mode or the TF updates.