Inverse kinematics model controls trajectory planning and servo system