enable no wrapping around obstacles

[caomuqing]

Some users want a behavior such that the robot does not wrap around any static obstacle.

To do so, I believe a quick modification is to change the following lines:

neptune/neptune/src/kinodynamic_search.cpp

Line 853 in 7a65220

for (int i=0; i<num_of_agents_; i++) // check entanglement

AND

neptune/neptune/src/kinodynamic_search.cpp

Line 947 in 7a65220

for (int i=0; i<num_of_agents_; i++) // check entanglement

TO
for (int i=0; i<num_of_agents_+num_of_static_obst_; i++) // check entanglement

If someone has tried it and it works, please let me know. Thanks.