Fix ZMP constraint when some DOFs are disabled