added test-hack to stop robot in smoothing-phase at the end of trajectory if close enough to goal