end_freedrive_mode()

Set robot back in normal position control mode after freedrive mode.