Added modes. Now can switch without problems between walking and stand6.
Stand4 to be implemented, probably by adding neutralairpos for each leg 4 stance location.