An apparatus and method of optimizing collision free path planning comprising: initializing a plurality of feasible configuration points, sample points located in a free space, and sample points not located in the free space; determining a goodness rating of the various pluralities of points; generating new candidate points; determining if each new candidate point is a feasible configuration point and adding each to the plurality of feasible configuration points if it is; if not, determining if each new candidate point is located in the free space and adding it to the plurality of sample points located in the free space if it is; if not, adding to the plurality of sample points not located in the free space; determining an updated goodness rating of the various pluralities of points; determining if a termination criteria has been met and reporting a best solution or no solution if it has been met.