Improve wait motion done

Rather than waiting a fixed amount of time after commanding a desired
joint configuration, I periodically read the current joint configuration
until it is equal to the desired one (up to a certain threshold).
Moreover, I increased the motion for stiction estimation from 2 degrees
to 3 degrees, so that it should be less jerky.