Humanoid Path Planner (testing equilibrium)
