Done: RRT without obstacle works; Problem solved: after getting indices from knnSearch function, one should use index.getPoint() to obtain the nearest neighbor. I use dataset[indices] before  which is incorrect