dynamics planning takes in polygonal constraint on the relative position between CoM and feet