Planner

Optimized the 3DPathfinder to find shorter path.
Fixed the issue  "should not stand on water" in embodiment.