Implemented Position -> Force control

highly unstable, don't know why. Will haev to analyze Tf tomorrow