Add test for ZMP computation using Choreonoid (#17)
Improve ZMP computations