Fixed inquery nearest accessible position.
Added more debug output for planner.
Enabled option of grounding a state without fully grounding all state variables.
Added "const" for some functions in space map.
Fixed 3D Pathfinder by checking if both start and target locations are standable.