Fixed initiation and finishing motion

I think it works fine on single joints.
The program still seems to have some bugs for multiple joints.
This probably has to do something with mode exchanging
(the timing of mode exchanging and goal sending sometimes occur in wrong order due to subscribe timing)
Maybe we should rethink about mode handling in the core program.