A branch-and-prune algorithm for solving systems of distance constraints

Given a set of affine varieties in R³, i.e. planes, lines, and points, the problem tackled in this paper is that of finding all possible configurations for these varieties that satisfy a set of pairwise euclidean distances between them. Many problems in robotics - such as the forward kinematics of patroller manipulators or the contact formation problem between polyhedral models - can be formulated in this way. We propose herein a strategy that consists in finding some distances, that are unknown a priori, and whose derivation permits solving the problem rather trivially. Finding these distances relies on a branch-and-prune technique that iteratively eliminates from the space of distances entire regions which cannot contain any solution. The elimination is accomplished by applying redundant necessary conditions derived from the theory of Cayley-Menger determinants. The experimental results obtained qualify this approach as a promising one.