Fix controlMode initialization
Fix control loop in friExampleTorque