try to improve a robot behaviour when entering narrow passages (e.g., doors)

[rayvburn]

Currently, it sometimes happen that the robot is stuck near the obstacle despite not being in a collision yet.
The cause is that the planner regards the robot being in collision when the robot is located very close to an obstacle (due to #110 which provides collision avoidance in such situations).

This is a follow up of these issues:

• tune parameters to avoid robot being stuck in sharp corners #71,
• avoid trajectories that pass really close to obstacles #75,
• consider adding ObstacleSeparationCostFunction #110.