Moving feed-forward torques section to before we add in the torque explicitly as T. Also, fixed a bug (my fault) that made the torques wrong. Now they look reasonable! Math is working, Wendy!!!
Moving feed-forward torques section to before we add in the torque explicitly as T. Also, fixed a bug (my fault) that made the torques wrong. Now they look reasonable! Math is working, Wendy!!!

MatthewRueben/me531-project