We propose the Selective Densiﬁcation method for fast motion planning through conﬁguration space. We create a sequence of roadmaps by iteratively adding conﬁgurations. We organize these roadmaps into layers and add edges between identical conﬁgurations between layers. We ﬁnd a path using best-ﬁrst search, guided by our proposed estimate of remaining planning time. This estimate prefers to expand nodes closer to the goal and nodes on sparser layers. We present proofs of the path quality and maximum depth of nodes expanded using our proposed graph and heuristic. We also present experiments comparing Selective Densiﬁcation to bidirectional RRT-connect, as well as many graph search approaches. In difﬁcult environments that require exploration on the dense layers we ﬁnd Selective Densiﬁcation ﬁnds solutions faster than all other approaches.
Download the pdf
Watch the demo: