# Thrust Mapper - Finding a Solution

Last post, we defined a couple matrices which map thruster forces to forces on the robot.

$$
A =
\begin{bmatrix}
0 & 1 & 0 & 1 \\

1 & 0 & 1 & 0 \\

r_{5z} & r_{6z} & -r_{7z} & -r_{8z} \end
{bmatrix}, \quad
B =
\begin{bmatrix}
r_{1x} & r_{2x} & -r_{3x} & -r_{4x} \\

-r_{1y} & -r_{2y} & r_{3y} & r_{4y} \\

1 & 1 & 1 & 1
\end{bmatrix}
$$

$$
A
\begin{bmatrix}
T_5 \\

T_6 \\

T_7 \\

T_8 \end
{bmatrix} =
\begin{bmatrix}
F_x \\

F_y \\

M_z
\end{bmatrix}, \quad
B
\begin{bmatrix}
T_1 \\

T_2 \\

T_3 \\

T_4 \end
{bmatrix} =
\begin{bmatrix}
M_x \\

M_y \\

F_z \end
{bmatrix}
$$

## The Inverse

If you remember your first linear algebra course, you’ll know that we need the inverses of \(A\) and \(B\) to find the thruster forces that gives us a solution.

$$
A^{-1}
\begin{bmatrix}
F_x \\

F_y \\

M_z \end
{bmatrix} =
\begin{bmatrix}
T_5 \\

T_6 \\

T_7 \\

T_8 \end
{bmatrix}
$$

But \(A\) isn’t a square matrix so it does *not* have an inverse!
This actually makes sense if we look at the physical system.
If there was an inverse, it would imply that there is a unique solution to every system.

The figure above illustrates the robot doing a yaw maneuver - spinning about the z-axis - using two configurations.
On the left, only the bow and stern thrusters are used.
On the right, only the port and starboard thrusters are used.^{1}

In fact, we can redistribute this effort between these thrusters in an infinite number of ways to get the same overall effect. So no, A is not invertible, but there are many solutions. This where the pseudo-inverse, \(A^+\) comes in handy.

## The pseudo-Inverse

I won’t go into all the details of pseudo-inverses here but, for our purposes, it’s helpful to note these facts:

if \(Ax=b\) has \(> 1\) solutions then \(x = A^+b\)

\(A^+\) is unique

\(A^+\) exists for all \(A\)

The popular python matrix library, `numpy`

, comes with a function for obtaining the pseudo-inverse, `numpy.linalg.pinv`

```
from numpy import matrix
from numpy.linalg import pinv
A = matrix([1, 1, 0, 0],
[0, 0, 1, 1],
[r5z, 56z, 57z, r8z])
Ap = pinv(A)
```

Now, whenever we want to find the equivalent thruster forces to get a desired response, we let numpy do the heavy lifting. For example, if we want the robot to dive deeper while accelerating forward, we might do this:

```
# create the desired response as a vector
Fx_Fy_Mz = matrix([1, 0, 1]).T
T5678 = Ap * Fx_Fy_Mz
```

And voilá, `T5678`

contains a vector for the thrusts required of thrusters 5, 6, 7, and 8 to perform our maneuver.

- Looking towards the front of a vessel, the nautical terms
*bow, stern, port, and starboard*mean*forward, backward, left, and right*respectively^{[return]}