Created by: pFernbach
Solve https://github.com/humanoid-path-planner/hpp-affordance/issues/4
All tests passed