Add 3D support for polygonal constraint on the relative position of the foot