Roadmap.py 文件源码

def get_path(self,q0,qf):
        """
        Searches for a path in the PRM between configurations q0 and qf.
        Returns a list of configuration tuples describing the path or []
        if no path is found.
        """
        q0 = np.reshape(np.array(q0),3)
        qf = np.reshape(np.array(qf),3)
        if all(q0 == qf):
            return [qf]

        n0 = len(self.G.node)
        nf = n0 + 1

        # Add the start and end configs to G, so we can just search it
        self.G.add_node(n0,cfg=q0)
        self.G.add_node(nf,cfg=qf)
        for k in [n0,nf]:
            self._connect(k,DobotModel.forward_kinematics(self.G.node[k]['cfg']))

        if not nx.has_path(self.G,n0,nf):
            path = [] # could not find a path
        else:
            nodes = nx.dijkstra_path(self.G,n0,nf,'weight')
            path = [self.G.node[k]['cfg'] for k in nodes]

        # Remove the start and end configs so G remains consistent with tree
        self.G.remove_node(n0)
        self.G.remove_node(nf)

        return path