Update current position after switching modes

[rkojcev]

When I switch modes, for example from Position to TeachArm then swtich back from Position Control, the robot tries to go back to the last pose commanded in the Position mode.

It would be great if you can read out the current position achieved in the TeachArm mode and set that when we switch to Position ctrl mode (or just clear out the last position vector).

[ahundt]

Is this best left as user defined behavior? If the user application wants to feed the current pose into the goal, their code could do that.

Perhaps it is still worthwhile to add a simple flag for this purpose into the c++ driver.

[ahundt]

closing in favor of #99 since there is more discussion