Model has inconsistencies with small motions

[zschwid]

Very small steps can lead to commanding positions that the hexapod mechanically cannot keep up with. Rework model to account for this case