Switch back to original robotpositioncontroller for real robot