Changed inverse kinematics function