First test of IK, constants seems wrong, but working