Added ZMP init pose as argument