added friction cones in the WF added heightMap Class added CostModelHeightMapDistance for obstacle avoidance