Now it is comparing with goal point, but the point is different from the last point of RRT Calculated path