ikfast change for simplifying equations, add GetId() to environment