Dirty hack to test CoM/ZMP optimization