Computing optimal path in a configuration space is fundamental to solving motion planning problems in robotics and autonomy. Graph-based search algorithms have been widely used to that end, but they suffer from drawbacks. We present an algorithm for computing the shortest path through a metric simplicial complex that can be used to construct a piece-wise linear discrete model of the configuration manifold. In particular, given an undirected metric graph, G, which is constructed as a discrete representation of an underlying configuration manifold (a larger “continuous” space typically of dimension greater than one), we consider the Rips complex, [Formula: see text], associated with it. Such a complex, and hence shortest paths in it, represent the underlying metric space more closely than what the graph does. Our algorithm requires only a local connectivity-based description of an abstract graph, [Formula: see text], and a cost/length function, [Formula: see text], as inputs. No global information such as an embedding or a global coordinate chart is required. The local nature of the proposed algorithm makes it suitable for configuration spaces of arbitrary topology, geometry, and dimension. We not only develop the search algorithm for computing shortest distances, but we also present a path reconstruction algorithm for explicitly computing the shortest paths through the simplicial complex. The complexity of the presented algorithm is comparable with that of Dijkstra’s search, but, as the results presented in this paper demonstrate, the shortest paths obtained using the proposed algorithm represent the geodesic paths in the original metric space significantly more closely.
Indexing algorithm based on storing additional distances in metric space for multi-vantage-point tree
