regriptpp.py 文件源码

def findshortestpath(self, startrotmat4, goalrotmat4, base):
        self.__addstartgoal(startrotmat4, goalrotmat4, base)

        # startgrip = random.select(self.startnodeids)
        # goalgrip = random.select(self.goalnodeids)
        startgrip = self.startnodeids[0]
        goalgrip = self.goalnodeids[0]
        self.shortestpaths = nx.all_shortest_paths(self.regg, source = startgrip, target = goalgrip)
        self.directshortestpaths = []
        # directshortestpaths removed the repeated start and goal transit
        try:
            for path in self.shortestpaths:
                print path
                for i, pathnode in enumerate(path):
                    if pathnode.startswith('start') and i < len(path)-1:
                        continue
                    else:
                        self.directshortestpaths.append(path[i-1:])
                        break
                for i, pathnode in enumerate(self.directshortestpaths[-1]):
                    if i > 0 and pathnode.startswith('goal'):
                        self.directshortestpaths[-1]=self.directshortestpaths[-1][:i+1]
                        break
        except:
            print "No path found!"
            pass