Implement hrp2 motion generation