Fixed FK for hopefully the last time (added pi/2 to theta1 and theta3 in appropriate places