Refactor TF calculation and swap upper and lower joints

The joint order in hForce/vForce must remain consistent between
this code and the thermal noise calculation

Also remove a couple of unused variables

Fixes #68