For a game I'm working on I want players (and AI) to be able to control modular spacecraft. By this I mean that I need to go from a desired control input (move forward) to a set of thruster outputs.

## The Easy Approach

In the past I've always used a very simplistic approach that goes something like:
```
let engine_lin_component = engine_local_vector.dot(output_thrust);
let engine_torque_output = engine_local_vector.cross(engine_local_position).normalize();
let engine_ang_component = engine_torque_output.dot(output_torque);
let engine_output = engine_lin_component + engine_ang_component;
let engine_output = engine_output.clamp(0.0, 1.0);
```

Whereby for each engine you figure out if it's pointing the right way and if it will make a positive contribution to moving the ship the right way, turn it on.

This approach is nice and simple, but has the issue that there is often unintended motion. For example if your spaceship is asymmetric, then attempting to move forward may cause it to spin. Clearly some other solution is needed.

## Jacobians

While puzzling on this problem, my thoughts swung back to my universities robotics course where we had to compute a set of minimal joint motions to arrive at a solution for redundant serial manipulators. To do this we used a Jacobian matrix. A jacobian matrix figures out how much each joint (thruster) will affect the end effector position (forces acting on the spaceship) by using the partial derivates (you can compute these numerically, so it's not so bad).

While scribbling on paper I realized that all of this worked out very neatly because a spaceship with non-moving thrusters, the jacobian is static, and because the thrusters are independant there is the amazing property that the jacobian is equivalent to the forward kinematics matrix.

## The forwards kinematics matrix

The forwards kinematics matrix describes how the spaceship will move for each thruster. It allows us to convert from a set of thruster power-percentages to the resulting forces on the spaceship:
```
forces_on_ship = forwards_kinematics_matrix * thruster_output_power
```

We can immediately tell some things about this forwards kinematics matrix:

- It has a column for each thruster, so that it can be multiplied by the thruster_output_power vector
- It is (for a 3D spaceships) 6 rows high - three rows for forces, three for torques

```
forces_on_ship = [force_x, force_y, force_z, torque_x, torque_y, torque_z]
thruster_output_power = [thrust_percent_1, thrust_percent_2, .... thrust_percent_n]
forward_kinematics_matrix = [
thruster_1_force_x, thruster_2_force_x ...
thruster_1_force_y, thruster_2_force_y ...
thruster_1_force_z, thruster_2_force_z ...
thruster_1_torque_x, thruster_2_torque_x ...
thruster_1_torque_y, thruster_2_torque_y ...
thruster_1_torque_z, thruster_2_torque_z ...
]
```

Which can be computed by something like:
```
out = []
for engine in engines:
max_force = engine.local_thrust_vector * engine.max_thrust
max_torque = max_force.cross(engine.local_position_vector)
influence = concatenate([max_force, max_torque])
out.append(influence)
forward_kinematics = out.transpose()
```

## Solving the inverse kinematics

To solve an inverse kinematics problem with the jacobian, we iteratively apply it on the difference between the motion we want and the motion we got:
```
desired_motion = np.concatenate([force, torque])
# Take an initial guess at thrust outputs
num_thrusters = len(self.inverse_mapping)
guess = np.zeros(num_thrusters)
prev_guess = guess
solver_state = "UNCONVERGED"
for i in range(100):
inv_jacobian = np.linalg.pinv(self.forward_kinematics) # Only true because thrusters are independant
thrust_output = self.forward_kinematics.dot(guess) # Why does numpy use .dot for matrix*vector?
thrust_error = desired_motion - thrust_output
guess += inv_jacobian.dot(thrust_error)
```

The problem at this point is that it doesn't have any idea about constraints - so some thrusters will apply negative thrust. The solution is to constrain our guess:
```
guess = np.clip(guess, 0.0, 1.0)
```

This means that on each sucessive iteration the thrusters that will try to "pull" will be reset to zero thrust. The jacobian will detect this and try to correct it. With luck it will do so by turning some other thrusters on.

Another minor problem is that it always runs for 100 iterations. Most of the time it will converge a fair bit sooner, so we can reduce the processing time with:
```
if sum(abs(thrust_error)) < 0.0001:
solver_state = "SUFFICIENTLY_CONVERGED"
break
delta = prev_guess - guess
if sum(abs(delta)) < 0.000001:
solver_state = "UNCHANGING"
break
```

## From force control to velocity control

We can now apply forces, but this can be unintuitive to a player/user. Unless there is sufficient drag to slow the vehicle down they often end up spinning out of control. The solution is to switch to velocity control.

I chose to use a very simple proportional controller:
```
if self.control_mode == "VELOCITY":
torque_request = (self.desired_angular_motion - angular_velocity) * ANGULAR_VELOCITY_CONTROLLER_GAIN
force_request = (self.desired_linear_motion - linear_velocity) * LINEAR_VELOCITY_CONTROLLER_GAIN
elif self.control_mode == "FORCE":
torque_request = self.desired_angular_motion * ANGULAR_FORCE_CONTROLLER_GAIN
force_request = self.desired_linear_motion * ANGULAR_FORCE_CONTROLLER_GAIN
```

## The Result

So what does this look like in practice?

## Limitations of this approach

This method is far far from perfect. There are a couple major limitations:

- If there aren't enough thrusters, there will be unintended motion. If there are insufficient thrusters that are acting in one direction then there will be "bleed through" where an attempt to translate on one axis may result in translation in another axis, or perhaps rotation. Capping this would look like computing the maximum torque_request and force_request that can be sent to the kinematics calculations, but I haven't yet thought of a good way to do this.
- The solutions may not be optimal. For example if you have four thrusters all pointing the same way it may decide to use all four of them. Or maybe it will only use two and the other two will be idle. One possible solution here would be to introduce a seventh column to our jacobian matrix which represents the total power applied to the spaceship. If this were non-linear this could be used to prioritize using more thrusters at lower power-levels, but it would require recomputing the jacobian each frame and introduce a bunch more computation.

Despite it's limitations this method is good enough for the game I'm working on (for now) and I'd consider it a successful couple evenings of mathematical fiddling.