Added constraint that cylinder should not penetrate ground