Modified the setPosition function to use tolerance