WIP on hip servo during stance