Oscillations at the goal pose

[padhupradheep]

While testing it on a powered casters, when the wheels try to reorient, the wheels oscillates while trying to re-orient itself to the goal.

This happens when we are looking for achieveing goal tolerance close to 0.03 radians.