Use :end_effector instead of link origin in IK and fix mode transition

(Stabilizer.cpp) : Use :end_effector instead of link origin in IK and fix mode transition