Fully support the case of non planar feet case in ZMP from forces.

The added terms should be negligible in most cases, even when feet are not planar, but this version is more general.