Fixed Inverse kinematics, add FK. FK may not work yet.