Following a target ZMP is working (midified version from paper "Online
Generation of Humanoid Walking Motion based on a Fast Generation Method
of Motion Pattern that Follows Desired ZMP")
Start implementing custom method for moving ZMP into SP