Fix ZMP computation with Choreonoid