Better goal tolerance in impedance mode

[bsaund]

Currently the goal_tolerance in the trajectory follower for victor is set to 0.2 rad.
This is fine for joint position mode and many but not all configurations in impedance mode with stiff parameters

One option for impedance mode is let the kuka controller say when it has reached the destination after setting the last waypoint (and therefore we wont expect the kuka controller to improve much further). This would involve adding a new field passed from java.

Another option is make the goal tolerance settable and have utilities for setting reasonable goal tolerances