Finish implementing ZMP with Choreonoid, fix #17