add a function addKinematicPolygon in the P step to encode the relative position between contact bodies. Will need to work on RelativeFootPositionPolygon to specify such position constraint