Polonomial function now fits the toe constraint data.  There is enough error between the polynomial values and the correct angles that the controller should use a lookup table.