porting ZMP controller to C++