def __init__(self, controlSpace, objective, metric, edgeChecker, **params):
     """Given a ControlSpace controlSpace, a metric, and an edge checker"""
     TreePlanner.__init__(self)
     self.controlSpace = controlSpace
     if not isinstance(controlSpace, ControlSpace):
         print "Warning, controlSpace is not a ControlSpace"
     if not isinstance(edgeChecker, EdgeChecker):
         print "Warning, edgeChecker is not an EdgeChecker"
     self.cspace = controlSpace.configurationSpace()
     self.metric = metric
     self.objective = objective
     self.edgeChecker = edgeChecker
     self.controlSelector = RandomControlSelector(controlSpace, self.metric,
                                                  1)
     self.goal = None
     self.goalSampler = None
     self.pChooseGoal = popdefault(params, 'pChooseGoal', 0.1)
     self.goalNodes = []
     self.selectionRadius = popdefault(params, 'selectionRadius', 0.1)
     self.witnessRadius = popdefault(params, 'witnessRadius', 0.03)
     self.witnessSet = []
     self.configurationSampler = Sampler(
         self.controlSpace.configurationSpace())
     nnmethod = popdefault(params, 'nearestNeighborMethod', 'kdtree')
     self.nearestNeighbors = NearestNeighbors(self.metric, nnmethod)
     self.nearestWitness = NearestNeighbors(self.metric, nnmethod)
     self.stats = Profiler()
     self.numIters = self.stats.count('numIters')
     self.bestPathCost = infty
     self.bestPath = None
     if len(params) != 0:
         print "Warning, unused params", params
class StableSparseRRTStar:
    def __init__(self,controlSpace,objective,metric,edgeChecker,
                 **params):
        self.selectionRadius0 = popdefault(params,'selectionRadius',0.2)
        self.witnessRadius0 = popdefault(params,'witnessRadius',0.2)
        self.numIters0 = popdefault(params,'numSSTIters',300)
        self.shrinkage = popdefault(params,'shrinkage',0.8)
        self.ssrrt = StableSparseRRT(controlSpace,objective,metric,
                                     edgeChecker,selectionRadius=self.selectionRadius0,witnessRadius=self.witnessRadius0,**params)
        self.itersleft = self.numIters0
        self.stats = Profiler()
        self.stats.items['ssrrt'] = self.ssrrt.stats
        self.numIters = self.stats.count('numIters')
        self.restartCount = self.stats.count('restartCount')
        self.bestPath = None
        self.bestPathCost = infty
        if len(params) != 0:
            print "Warning, unused params",params
    def setBoundaryConditions(self,start,goal):
        self.ssrrt.setBoundaryConditions(start,goal)
    def setConfigurationSampler(self,sampler):
        self.ssrrt.setConfigurationSampler(sampler)
    def setControlSelector(self,selector):
        self.ssrrt.setControlSelector(selector)
    def reset(self):
        self.ssrrt.reset()
        self.ssrrt.selectionRadius = self.selectionRadius0
        self.ssrrt.witnessRadius = self.witnessRadius0
        self.itersleft = self.numIters0
        self.numIters.set(0)
        self.restartCount.set(0)
        self.bestPath = None
        self.bestPathCost = infty
    def planMore(self,iters):
        res = False
        for n in xrange(iters):
            self.numIters += 1
            if self.ssrrt.planMore(1):
                res = True
                if self.ssrrt.bestPathCost < self.bestPathCost:
                    self.bestPathCost = self.ssrrt.bestPathCost
                    self.bestPath = self.ssrrt.bestPath
            self.itersleft -= 1
            if self.itersleft <= 0:
                #go to next iteration of SS-RRT
                self.restartCount += 1
                self.ssrrt.reset()
                self.ssrrt.selectionRadius *= self.shrinkage
                self.ssrrt.witnessRadius *= self.shrinkage
                #formula from Algorithm 9
                self.itersleft = (1.0+math.log(self.restartCount.count))*pow(self.shrinkage,-(len(self.ssrrt.root.x)+1)*self.restartCount.count)*self.numIters0
        return res
    def getPath(self):
        return self.bestPath
    def getRoadmap(self):
        return self.ssrrt.getRoadmap()
class StableSparseRRTStar:
    def __init__(self,controlSpace,objective,metric,edgeChecker,
                 selectionRadius,witnessRadius,numIters,shrinkage,
                 **params):
        self.ssrrt = StableSparseRRT(controlSpace,objective,metric,
                                     edgeChecker,selectionRadius=selectionRadius,witnessRadius=witnessRadius,**params)
        self.selectionRadius0 = selectionRadius
        self.witnessRadius0 = witnessRadius
        self.numIters0 = numIters
        self.shrinkage = shrinkage
        self.itersleft = numIters
        self.restartCount = 0
        self.stats = Profiler()
        self.stats.items['ssrrt'] = self.ssrrt.stats
        self.numIters = self.stats.count('numIters')
        self.stats.count('restartCount')
        self.bestPath = None
        self.bestPathCost = infty
    def setBoundaryConditions(self,start,goal):
        self.ssrrt.setBoundaryConditions(start,goal)
    def setConfigurationSampler(self,sampler):
        self.ssrrt.setConfigurationSampler(sampler)
    def setControlSelector(self,selector):
        self.ssrrt.setControlSelector(selector)
    def reset(self):
        self.ssrrt.reset()
        self.ssrrt.selectionRadius = self.selectionRadius0
        self.ssrrt.witnessRadius = self.witnessRadius0
        self.itersleft = self.numIters0
        self.numIters.set(0)
        self.restartCount = 0
        self.bestPath = None
        self.bestPathCost = infty
    def planMore(self,iters):
        res = False
        for n in xrange(iters):
            self.numIters += 1
            if self.ssrrt.planMore(1):
                res = True
                if self.ssrrt.bestPathCost < self.bestPathCost:
                    self.bestPathCost = self.ssrrt.bestPathCost
                    self.bestPath = self.ssrrt.bestPath
            self.itersleft -= 1
            if self.itersleft <= 0:
                #go to next iteration of SS-RRT
                self.restartCount += 1
                self.ssrrt.reset()
                self.ssrrt.selectionRadius *= self.shrinkage
                self.ssrrt.witnessRadius *= self.shrinkage
                #formula from Algorithm 9
                self.itersleft = (1.0+math.log(self.restartCount))*pow(self.shrinkage,-(len(self.ssrrt.root.x)+1)*self.restartCount)*self.numIters0
        return res
    def getPath(self):
        return self.bestPath
    def getRoadmap(self):
        return self.ssrrt.getRoadmap()
    def __init__(self,controlSpace,objective,metric,edgeChecker,
                 **params):
        """Given a ControlSpace controlSpace, a metric, and an edge checker"""
        TreePlanner.__init__(self)
        self.controlSpace = controlSpace
	if not isinstance(controlSpace,ControlSpace):
            print "Warning, controlSpace is not a ControlSpace"
	if not isinstance(edgeChecker,EdgeChecker):
            print "Warning, edgeChecker is not an EdgeChecker"
        self.cspace = controlSpace.configurationSpace()    
        self.metric = metric
        self.objective = objective
        self.edgeChecker = edgeChecker
	self.controlSelector = RandomControlSelector(controlSpace,self.metric,1)
        self.goal = None
        self.goalSampler = None
        self.pChooseGoal = popdefault(params,'pChooseGoal',0.1)
        self.goalNodes = []
        self.selectionRadius = popdefault(params,'selectionRadius',0.1)
        self.witnessRadius = popdefault(params,'witnessRadius',0.03)
        self.witnessSet = []
        self.configurationSampler = Sampler(self.controlSpace.configurationSpace())        
        nnmethod = popdefault(params,'nearestNeighborMethod','kdtree')
        self.nearestNeighbors = NearestNeighbors(self.metric,nnmethod)
        self.nearestWitness = NearestNeighbors(self.metric,nnmethod)
        self.stats = Profiler()
        self.numIters = self.stats.count('numIters')
        self.bestPathCost = infty
        self.bestPath = None
        if len(params) != 0:
            print "Warning, unused params",params
 def __init__(self,controlSpace,objective,metric,edgeChecker,
              selectionRadius,witnessRadius,numIters,shrinkage,
              **params):
     self.ssrrt = StableSparseRRT(controlSpace,objective,metric,
                                  edgeChecker,selectionRadius=selectionRadius,witnessRadius=witnessRadius,**params)
     self.selectionRadius0 = selectionRadius
     self.witnessRadius0 = witnessRadius
     self.numIters0 = numIters
     self.shrinkage = shrinkage
     self.itersleft = numIters
     self.restartCount = 0
     self.stats = Profiler()
     self.stats.items['ssrrt'] = self.ssrrt.stats
     self.numIters = self.stats.count('numIters')
     self.stats.count('restartCount')
     self.bestPath = None
     self.bestPathCost = infty
 def __init__(self, controlSpace, objective, metric, edgeChecker, **params):
     self.selectionRadius0 = popdefault(params, 'selectionRadius', 0.2)
     self.witnessRadius0 = popdefault(params, 'witnessRadius', 0.2)
     self.numIters0 = popdefault(params, 'numSSTIters', 300)
     self.shrinkage = popdefault(params, 'shrinkage', 0.8)
     self.ssrrt = StableSparseRRT(controlSpace,
                                  objective,
                                  metric,
                                  edgeChecker,
                                  selectionRadius=self.selectionRadius0,
                                  witnessRadius=self.witnessRadius0,
                                  **params)
     self.itersleft = self.numIters0
     self.stats = Profiler()
     self.stats.items['ssrrt'] = self.ssrrt.stats
     self.numIters = self.stats.count('numIters')
     self.restartCount = self.stats.count('restartCount')
     self.bestPath = None
     self.bestPathCost = infty
     if len(params) != 0:
         print "Warning, unused params", params
 def __init__(self,controlSpace,objective,metric,edgeChecker,
              **params):
     self.selectionRadius0 = popdefault(params,'selectionRadius',0.2)
     self.witnessRadius0 = popdefault(params,'witnessRadius',0.2)
     self.numIters0 = popdefault(params,'numSSTIters',300)
     self.shrinkage = popdefault(params,'shrinkage',0.8)
     self.ssrrt = StableSparseRRT(controlSpace,objective,metric,
                                  edgeChecker,selectionRadius=self.selectionRadius0,witnessRadius=self.witnessRadius0,**params)
     self.itersleft = self.numIters0
     self.stats = Profiler()
     self.stats.items['ssrrt'] = self.ssrrt.stats
     self.numIters = self.stats.count('numIters')
     self.restartCount = self.stats.count('restartCount')
     self.bestPath = None
     self.bestPathCost = infty
     if len(params) != 0:
         print "Warning, unused params",params
class StableSparseRRT(TreePlanner):
    """An implementation of Littlefield et al 2014 Stable Sparse RRT.
    
    Nodes get a cost 'c' and an active flag 'active'.

    The witness set consists of pairs [point,rep].
    """
    def __init__(self, controlSpace, objective, metric, edgeChecker, **params):
        """Given a ControlSpace controlSpace, a metric, and an edge checker"""
        TreePlanner.__init__(self)
        self.controlSpace = controlSpace
        if not isinstance(controlSpace, ControlSpace):
            print "Warning, controlSpace is not a ControlSpace"
        if not isinstance(edgeChecker, EdgeChecker):
            print "Warning, edgeChecker is not an EdgeChecker"
        self.cspace = controlSpace.configurationSpace()
        self.metric = metric
        self.objective = objective
        self.edgeChecker = edgeChecker
        self.controlSelector = RandomControlSelector(controlSpace, self.metric,
                                                     1)
        self.goal = None
        self.goalSampler = None
        self.pChooseGoal = popdefault(params, 'pChooseGoal', 0.1)
        self.goalNodes = []
        self.selectionRadius = popdefault(params, 'selectionRadius', 0.1)
        self.witnessRadius = popdefault(params, 'witnessRadius', 0.03)
        self.witnessSet = []
        self.configurationSampler = Sampler(
            self.controlSpace.configurationSpace())
        nnmethod = popdefault(params, 'nearestNeighborMethod', 'kdtree')
        self.nearestNeighbors = NearestNeighbors(self.metric, nnmethod)
        self.nearestWitness = NearestNeighbors(self.metric, nnmethod)
        self.stats = Profiler()
        self.numIters = self.stats.count('numIters')
        self.bestPathCost = infty
        self.bestPath = None
        if len(params) != 0:
            print "Warning, unused params", params

    def destroy(self):
        TreePlanner.destroy(self)
        self.goalNodes = []
        self.witnessSet = []

    def reset(self):
        """Re-initializes the RRT* to the same start / goal, clears the
        planning tree."""
        x0 = self.root.x
        goal = self.goal
        self.bestPathCost = infty
        self.bestPath = None
        self.destroy()
        self.setBoundaryConditions(x0, goal)
        self.numIters.set(0)

    def setBoundaryConditions(self, x0, goal):
        """Initializes the tree from a start state x0 and a goal
        ConfigurationSubset.
        
        goal can be set to None to just explore.
        """
        self.setRoot(x0)
        self.root.c = 0
        self.root.active = True
        self.witnessSet = [[x0, self.root]]
        self.goal = goal
        if goal != None:
            if isinstance(goal, (list, tuple)):
                self.goal = SingletonSubset(self.cspace, goal)
            self.goalSampler = SubsetSampler(self.cspace, self.goal)
        self.nearestNeighbors.reset()
        self.nearestNeighbors.add(x0, self.root)
        self.nearestWitness.reset()
        self.nearestWitness.add(x0, self.witnessSet[0])

    def setConfigurationSampler(self, sampler):
        self.configurationSampler = sampler

    def setControlSelector(self, selector):
        self.controlSelector = selector

    def planMore(self, iters):
        for n in xrange(iters):
            self.numIters += 1
            n = self.expand()
            if n != None and self.goal != None:
                if self.goal.contains(n.x):
                    self.goalNodes.append(n)
                    if n.c + self.objective.terminal(n.x) < self.bestPathCost:
                        self.bestPathCost = n.c + self.objective.terminal(n.x)
                        print "New goal node with cost", self.bestPathCost
                        self.bestPath = TreePlanner.getPath(self, n)
                        if self.bestPath == None:
                            print "Uh... no path to goal?"
                        return True
        return False

    def expand(self):
        """Expands the tree via the Sparse-Stable-RRT technique.
        Returns the new node  or None otherwise."""
        if self.goalSampler and random.uniform(0.0, 1.0) < self.pChooseGoal:
            xrand = self.goalSampler.sample()
        else:
            xrand = self.configurationSampler.sample()
        #self.stats.stopwatch('pickNode').begin()
        nnear = self.pickNode(xrand)
        #self.stats.stopwatch('pickNode').end()
        if nnear == None:
            return None
        #self.stats.stopwatch('selectControl').begin()
        u = self.controlSelector.select(nnear.x, xrand)
        #self.stats.stopwatch('selectControl').end()
        #self.stats.stopwatch('edgeCheck').begin()
        edge = self.controlSpace.interpolator(nnear.x, u)
        if not self.edgeChecker.feasible(edge):
            #self.stats.stopwatch('edgeCheck').end()
            #self.stats.count('numInfeasible').add(1)
            return None
        #self.stats.stopwatch('edgeCheck').end()
        newcost = nnear.c + self.objective.incremental(nnear.x, u)
        #feasible edge, add it
        nnew = self.addEdge(nnear, u, edge)
        nnew.c = newcost
        nnew.active = True
        #self.stats.stopwatch('sst-domination-check').begin()
        localbest = self.nodeLocallyBest(nnew)
        #self.stats.stopwatch('sst-domination-check').end()
        if localbest == False:
            #dominated by some other node
            #self.stats.count('numDominated').add(1)
            assert nnew == self.nodes[-1]
            nnew.destroy()
            self.nodes.pop(-1)
            return None
        self.nearestNeighbors.add(nnew.x, nnew)
        #self.doPruning(nnew,localbest[1])
        #self.stats.stopwatch('sst-domination-pruning').begin()
        for s in localbest[1]:
            self.doPruning(nnew, s)
        #self.stats.stopwatch('sst-domination-pruning').end()
        return nnew

    def nodeLocallyBest(self, n):
        """Tests the node n against nearby witnesses.  If no witnesses
        are nearby, n gets added as a witness. Algorithm 7"""
        res = self.nearestWitness.nearest(n.x)
        assert res != None
        s = res[1]
        if self.metric(s[0], n.x) > self.witnessRadius:
            #self.stats.count('numWitnesses').add(1)
            self.witnessSet.append([n.x, None])
            self.nearestWitness.add(n.x, self.witnessSet[-1])
            return True, self.witnessSet[-1]
        wlist = self.nearestWitness.neighbors(n.x, self.witnessRadius)
        better = []
        for res in wlist:
            s = res[1]
            if s[1] == None: better.append(s)
            if n.c < s[1].c: better.append(s)
        if len(better) == 0: return False
        return True, better
        #if s[1] == None: return True,s
        #if n.c < s[1].c: return True,s
        return False

    def doPruning(self, n, snearest=None):
        """Algorithm 8"""
        if snearest != None:
            res = self.nearestWitness.nearest(n.x)
            assert res != None
            snearest = res[1]
        npeer = snearest[1]
        if npeer == n: return
        snearest[1] = n
        if npeer != None:
            #self.stats.count('numPruned').add(1)
            npeer.active = False
            #print "Remove",c
            while npeer != None and len(
                    npeer.children) == 0 and not npeer.active:
                #self.stats.count('numDeleted').add(1)
                p = npeer.parent
                #remove from NN data structure?
                cnt = self.nearestNeighbors.remove(npeer.x, npeer)
                #print "Remove",c
                npeer.destroy()
                npeer = p

    def pickNode(self, xrand):
        """Picks a within distance selectionRadius of xrand with the lowest
        cost.  Algorithm 6"""
        res = self.nearestNeighbors.neighbors(xrand, self.selectionRadius)
        activenear = []
        for pt, n in res:
            if n.active: activenear.append(n)
        if len(activenear) == 0:
            #return nearest of active neighbors
            res = self.nearestNeighbors.nearest(xrand,
                                                lambda pt, n: not n.active)
            if res == None: return None
            return res[1]
        else:
            #return node with minimum distance
            cmin = infty
            res = None
            for n in activenear:
                if n.c < cmin:
                    res = n
                    cmin = n.c
            return res

    def getPath(self):
        return self.bestPath
class RRTStar(TreePlanner):
    """The RRT* planner with a kinematic space

    Stores a tree of Nodes.  Expands the tree at random using the RRT strategy
    with a goal bias of probability pChooseGoal.
    """
    def __init__(self, cspace, metric, edgeChecker, **params):
        """Given a ControlSpace controlSpace, a metric, and an edge checker"""
        TreePlanner.__init__(self)
        if not isinstance(cspace, ConfigurationSpace):
            print "Warning, cspace is not a ConfigurationSpace"
        if not isinstance(edgeChecker, EdgeChecker):
            print "Warning, edgeChecker is not an EdgeChecker"
        self.cspace = cspace
        self.metric = metric
        self.edgeChecker = edgeChecker
        self.goal = None
        self.goalSampler = None
        self.pChooseGoal = popdefault(params, 'pChooseGoal', 0.1)
        self.goalNodes = []
        self.configurationSampler = Sampler(self.cspace)
        nnmethod = popdefault(params, 'nearestNeighborMethod', 'kdtree')
        self.nearestNeighbors = NearestNeighbors(self.metric, nnmethod)
        self.bestPathCost = infty
        self.bestPath = None
        if len(params) != 0:
            print "Warning, unused params", params
        self.stats = Profiler()
        self.numIters = self.stats.count('numIters')

    def destroy(self):
        TreePlanner.destroy(self)
        self.nearestNeighbors.reset()
        self.goalNodes = []

    def reset(self):
        """Re-initializes the RRT* to the same start / goal, clears the
        planning tree."""
        x0 = self.root.x
        goal = self.goal
        self.bestPathCost = infty
        self.bestPath = None
        self.destroy()
        self.setBoundaryConditions(x0, goal)
        self.numIters.set(0)

    def setBoundaryConditions(self, x0, goal):
        """Initializes the tree from a start state x0 and a goal
        ConfigurationSubset.
        
        goal can be set to None to just explore.
        """
        self.setRoot(x0)
        self.root.c = 0
        self.goal = goal
        if goal != None:
            if isinstance(goal, (list, tuple)):
                self.goal = SingletonSubset(self.cspace, goal)
            self.goalSampler = SubsetSampler(self.cspace, self.goal)
        self.nearestNeighbors.add(x0, self.root)

    def setConfigurationSampler(self, sampler):
        self.configurationSampler = sampler

    def planMore(self, iters):
        for n in xrange(iters):
            self.numIters.add(1)
            n = self.expand()
            if n != None and self.goal != None:
                if self.goal.contains(n.x):
                    self.goalNodes.append(n)
                    if n.c < self.bestPathCost:
                        self.bestPathCost = n.c
                        self.bestPath = self.getPath(n)
                        return True
        return False

    def expand(self):
        """Expands the tree via the RRT* technique.  Returns the new node
        or None otherwise."""
        if self.goalSampler and random.uniform(0.0, 1.0) < self.pChooseGoal:
            xrand = self.goalSampler.sample()
        else:
            xrand = self.configurationSampler.sample()
        nnear = self.pickNode(xrand)
        if nnear == None:
            return None
        if nnear.c > self.bestPathCost:
            return None
        #do rrt* connection
        N = float(self.numIters.count + 1)
        rad = 1.414 * (math.log(N) / N)**(1.0 / len(nnear.x))
        k = int(((1.0 + 1.0 / len(xrand)) * math.e) * math.log(N))
        if k <= 0: k = 1
        u = min(1.0, rad / self.metric(nnear.x, xrand))
        xend = self.cspace.interpolate(nnear.x, xrand, u)
        edge = self.cspace.interpolator(nnear.x, xend)
        if not self.edgeChecker.feasible(edge):
            return None
        #feasible edge, add it
        nnew = self.addEdge(nnear, xend, edge)
        nnew.c = nnear.c + edge.length()
        self.nearestNeighbors.add(nnew.x, nnew)
        self.rewire(nnew, k=k, rad=None, first=True, recursive=False)
        return nnew

    def rewire(self, n, k=None, rad=None, first=True, recursive=False):
        #gather incoming cost from parent
        changed = False
        if n.parent != None:
            d = n.parent.c + self.metric(n.x, n.parent.x)
            if d != n.c:
                #print "initial cost improvement",n.c,"to",d
                n.c = d
                changed = True
        if changed:
            #self.stats.count('numCostImprovements').add(1)
            pass

        if first or recursive:
            if rad != None:
                neighbors = self.nearestNeighbors.neighbors(n.x, rad)
            elif k != None:
                neighbors = self.nearestNeighbors.knearest(n.x, k)
            else:
                raise ValueError(
                    "Rewire: must have either k or rad arguments provided")
            #sort the neighbors by accumulated cost
            dneighbors = []
            #print "Node",n.x,"cost",n.c
            for x, nn in neighbors:
                if nn == n: continue
                if nn == n.parent: continue
                d = self.metric(n.x, nn.x) + nn.c
                #print nn.x,d,self.metric(n.x,nn.x)
                dneighbors.append((d, nn))
            dneighbors = sorted(dneighbors, key=lambda x: x[0])
            #print len(dneighbors),"neighbors, cost range",dneighbors[0][0],"to",dneighbors[-1][0]
            #add incoming edges from neighbors
            for (d, nn) in dneighbors:
                if d > n.c: break
                edge = self.cspace.interpolator(nn.x, n.x)
                if self.edgeChecker.feasible(edge):
                    #print "rrt* cost improvement",n.c,"to",d
                    n.setParent(nn, n.x, edge)
                    n.c = d
                    changed = True
                    #optimization: break on first added neighbor, since they're
                    #sorted by increasing cost
                    if changed:
                        #self.stats.count('numIncomingRewirings').add(1)
                        pass
                    break
        if n.c > self.bestPathCost:
            return

        if first or changed:
            for c in n.children:
                self.rewire(c, k=k, rad=rad, first=False, recursive=recursive)

        if first or recursive:
            #add outgoing edges from n
            for (d, nn) in dneighbors:
                newcost = n.c + self.metric(n.x, nn.x)
                if newcost < nn.c:
                    #print "Outgoing improvement from",nn.c,"to",newcost
                    #potential improvement, check edge and rewire if necessary
                    edge = self.cspace.interpolator(n.x, nn.x)
                    if self.edgeChecker.feasible(edge):
                        #self.stats.count('numOutgoingRewirings').add(1)
                        nn.setParent(n, nn.x, edge)
                        #nn.c = newcost
                        self.rewire(nn,
                                    k=k,
                                    rad=rad,
                                    first=False,
                                    recursive=recursive)

    def pickNode(self, xrand):
        """Picks a node closest to xrand.  If dynamicDomain is True,
        uses the radius associated with the node"""
        res = self.nearestNeighbors.nearest(xrand)
        if res == None: return None
        return res[1]

    def getPath(self, n=None):
        if n == None:
            return self.bestPath
        return TreePlanner.getPath(self, n)
Exemplo n.º 10
0
class StableSparseRRT(TreePlanner):
    """An implementation of Littlefield et al 2014 Stable Sparse RRT.
    
    Nodes get a cost 'c' and an active flag 'active'.

    The witness set consists of pairs [point,rep].
    """
    def __init__(self,controlSpace,objective,metric,edgeChecker,
                 **params):
        """Given a ControlSpace controlSpace, a metric, and an edge checker"""
        TreePlanner.__init__(self)
        self.controlSpace = controlSpace
	if not isinstance(controlSpace,ControlSpace):
            print "Warning, controlSpace is not a ControlSpace"
	if not isinstance(edgeChecker,EdgeChecker):
            print "Warning, edgeChecker is not an EdgeChecker"
        self.cspace = controlSpace.configurationSpace()    
        self.metric = metric
        self.objective = objective
        self.edgeChecker = edgeChecker
	self.controlSelector = RandomControlSelector(controlSpace,self.metric,1)
        self.goal = None
        self.goalSampler = None
        self.pChooseGoal = popdefault(params,'pChooseGoal',0.1)
        self.goalNodes = []
        self.selectionRadius = popdefault(params,'selectionRadius',0.1)
        self.witnessRadius = popdefault(params,'witnessRadius',0.03)
        self.witnessSet = []
        self.configurationSampler = Sampler(self.controlSpace.configurationSpace())        
        nnmethod = popdefault(params,'nearestNeighborMethod','kdtree')
        self.nearestNeighbors = NearestNeighbors(self.metric,nnmethod)
        self.nearestWitness = NearestNeighbors(self.metric,nnmethod)
        self.stats = Profiler()
        self.numIters = self.stats.count('numIters')
        self.bestPathCost = infty
        self.bestPath = None
        if len(params) != 0:
            print "Warning, unused params",params

    def destroy(self):
        TreePlanner.destroy(self)
        self.goalNodes = []
        self.witnessSet = []
    
    def reset(self):
        """Re-initializes the RRT* to the same start / goal, clears the
        planning tree."""
	x0 = self.root.x
	goal = self.goal
        self.bestPathCost = infty
        self.bestPath = None
	self.destroy()
	self.setBoundaryConditions(x0,goal)
	self.numIters.set(0)
	
    def setBoundaryConditions(self,x0,goal):
        """Initializes the tree from a start state x0 and a goal
        ConfigurationSubset.
        
        goal can be set to None to just explore.
        """
        self.setRoot(x0)
        self.root.c = 0
        self.root.active = True
        self.witnessSet = [[x0,self.root]]
        self.goal = goal
        if goal != None:
            if isinstance(goal,(list,tuple)):
                self.goal = SingletonSubset(self.cspace,goal)
            self.goalSampler = SubsetSampler(self.cspace,self.goal)
        self.nearestNeighbors.reset()
        self.nearestNeighbors.add(x0,self.root)
        self.nearestWitness.reset()
        self.nearestWitness.add(x0,self.witnessSet[0])

    def setConfigurationSampler(self,sampler):
        self.configurationSampler = sampler
    def setControlSelector(self,selector):
        self.controlSelector = selector
    def planMore(self,iters):
        for n in xrange(iters):
            self.numIters += 1
            n = self.expand()
            if n != None and self.goal != None:
                if self.goal.contains(n.x):
                    self.goalNodes.append(n)
                    if n.c + self.objective.terminal(n.x) < self.bestPathCost:
                        self.bestPathCost = n.c + self.objective.terminal(n.x)
                        print "New goal node with cost",self.bestPathCost
                        self.bestPath = TreePlanner.getPath(self,n)
                        if self.bestPath == None:
                            print "Uh... no path to goal?"
                        return True
        return False
    def expand(self):
        """Expands the tree via the Sparse-Stable-RRT technique.
        Returns the new node  or None otherwise."""
        if self.goalSampler and random.uniform(0.0,1.0) < self.pChooseGoal:
            xrand = self.goalSampler.sample()
        else:
            xrand = self.configurationSampler.sample()
        #self.stats.stopwatch('pickNode').begin()
        nnear = self.pickNode(xrand)
        #self.stats.stopwatch('pickNode').end()
        if nnear == None:
            return None
        #self.stats.stopwatch('selectControl').begin()
        u = self.controlSelector.select(nnear.x,xrand)
        #self.stats.stopwatch('selectControl').end()
        #self.stats.stopwatch('edgeCheck').begin()
        edge = self.controlSpace.interpolator(nnear.x,u)
        if not self.edgeChecker.feasible(edge):
            #self.stats.stopwatch('edgeCheck').end()
            #self.stats.count('numInfeasible').add(1)
            return None
        #self.stats.stopwatch('edgeCheck').end()
        newcost = nnear.c + self.objective.incremental(nnear.x,u)
        #feasible edge, add it
        nnew = self.addEdge(nnear,u,edge)
        nnew.c = newcost
        nnew.active = True
        #self.stats.stopwatch('sst-domination-check').begin()
        localbest = self.nodeLocallyBest(nnew)
        #self.stats.stopwatch('sst-domination-check').end()
        if localbest == False:
            #dominated by some other node
            #self.stats.count('numDominated').add(1)
            assert nnew == self.nodes[-1]
            nnew.destroy()
            self.nodes.pop(-1)
            return None
        self.nearestNeighbors.add(nnew.x,nnew)
        #self.doPruning(nnew,localbest[1])
        #self.stats.stopwatch('sst-domination-pruning').begin()
        for s in localbest[1]:
            self.doPruning(nnew,s)
        #self.stats.stopwatch('sst-domination-pruning').end()
        return nnew

    def nodeLocallyBest(self,n):
        """Tests the node n against nearby witnesses.  If no witnesses
        are nearby, n gets added as a witness. Algorithm 7"""
        res = self.nearestWitness.nearest(n.x)
        assert res != None
        s = res[1]
        if self.metric(s[0],n.x) > self.witnessRadius:
            #self.stats.count('numWitnesses').add(1)
            self.witnessSet.append([n.x,None])
            self.nearestWitness.add(n.x,self.witnessSet[-1])
            return True,self.witnessSet[-1]
        wlist = self.nearestWitness.neighbors(n.x,self.witnessRadius)
        better = []
        for res in wlist:
            s = res[1]
            if s[1] == None: better.append(s)
            if n.c < s[1].c: better.append(s)
        if len(better)==0: return False
        return True,better
        #if s[1] == None: return True,s
        #if n.c < s[1].c: return True,s
        return False
    
    def doPruning(self,n,snearest = None):
        """Algorithm 8"""
        if snearest != None:
            res = self.nearestWitness.nearest(n.x)
            assert res != None
            snearest = res[1]
        npeer = snearest[1]
        if npeer == n: return
        snearest[1] = n
        if npeer != None:
            #self.stats.count('numPruned').add(1)
            npeer.active = False
            #print "Remove",c
            while npeer != None and len(npeer.children)==0 and not npeer.active:
                #self.stats.count('numDeleted').add(1)
                p = npeer.parent
                #remove from NN data structure?
                cnt = self.nearestNeighbors.remove(npeer.x,npeer)
                #print "Remove",c
                npeer.destroy()
                npeer = p
    
    def pickNode(self,xrand):
        """Picks a within distance selectionRadius of xrand with the lowest
        cost.  Algorithm 6"""
        res = self.nearestNeighbors.neighbors(xrand,self.selectionRadius)
        activenear = []
        for pt,n in res:
            if n.active: activenear.append(n)
        if len(activenear) == 0:
            #return nearest of active neighbors
            res = self.nearestNeighbors.nearest(xrand,lambda pt,n:not n.active)
            if res ==None: return None
            return res[1]
        else:
            #return node with minimum distance
            cmin = infty
            res = None
            for n in activenear:
                if n.c < cmin:
                    res = n
                    cmin = n.c
            return res

    def getPath(self):
        return self.bestPath
Exemplo n.º 11
0
class RRTStar(TreePlanner):
    """The RRT* planner with a kinematic space

    Stores a tree of Nodes.  Expands the tree at random using the RRT strategy
    with a goal bias of probability pChooseGoal.
    """
    def __init__(self,cspace,metric,edgeChecker,
                 **params):
        """Given a ControlSpace controlSpace, a metric, and an edge checker"""
        TreePlanner.__init__(self)
	if not isinstance(cspace,ConfigurationSpace):
		print "Warning, cspace is not a ConfigurationSpace"
	if not isinstance(edgeChecker,EdgeChecker):
		print "Warning, edgeChecker is not an EdgeChecker"
        self.cspace = cspace
        self.metric = metric
        self.edgeChecker = edgeChecker
        self.goal = None
        self.goalSampler = None
        self.pChooseGoal = popdefault(params,'pChooseGoal',0.1)
        self.goalNodes = []
        self.configurationSampler = Sampler(self.cspace)
        nnmethod = popdefault(params,'nearestNeighborMethod','kdtree')
        self.nearestNeighbors = NearestNeighbors(self.metric,nnmethod)
        self.bestPathCost = infty
        self.bestPath = None
        if len(params) != 0:
            print "Warning, unused params",params
        self.stats = Profiler()
        self.numIters = self.stats.count('numIters')
    def destroy(self):
        TreePlanner.destroy(self)
        self.nearestNeighbors.reset()
        self.goalNodes = []
    def reset(self):
        """Re-initializes the RRT* to the same start / goal, clears the
        planning tree."""
	x0 = self.root.x
	goal = self.goal
        self.bestPathCost = infty
        self.bestPath = None
	self.destroy()
	self.setBoundaryConditions(x0,goal)
	self.numIters.set(0)
	
    def setBoundaryConditions(self,x0,goal):
        """Initializes the tree from a start state x0 and a goal
        ConfigurationSubset.
        
        goal can be set to None to just explore.
        """
        self.setRoot(x0)
        self.root.c = 0
        self.goal = goal
        if goal != None:
            if isinstance(goal,(list,tuple)):
                self.goal = SingletonSubset(self.cspace,goal)
            self.goalSampler = SubsetSampler(self.cspace,self.goal)
        self.nearestNeighbors.add(x0,self.root)
    def setConfigurationSampler(self,sampler):
        self.configurationSampler = sampler
    def planMore(self,iters):
        for n in xrange(iters):
            self.numIters.add(1)
            n = self.expand()
            if n != None and self.goal != None:
                if self.goal.contains(n.x):
                    self.goalNodes.append(n)
                    if n.c < self.bestPathCost:
                        self.bestPathCost = n.c
                        self.bestPath = self.getPath(n)
                        return True
        return False
    def expand(self):
        """Expands the tree via the RRT* technique.  Returns the new node
        or None otherwise."""
        if self.goalSampler and random.uniform(0.0,1.0) < self.pChooseGoal:
            xrand = self.goalSampler.sample()
        else:
            xrand = self.configurationSampler.sample()
        nnear = self.pickNode(xrand)
        if nnear == None:
            return None
        if nnear.c > self.bestPathCost:
            return None
        #do rrt* connection
        N = float(self.numIters.count+1)
        rad = 1.414*(math.log(N)/N) ** (1.0/len(nnear.x));
        k = int(((1.0+1.0/len(xrand))*math.e)*math.log(N))
        if k <=0: k=1
        u = min(1.0,rad / self.metric(nnear.x,xrand))
        xend = self.cspace.interpolate(nnear.x,xrand,u)
        edge = self.cspace.interpolator(nnear.x,xend)
        if not self.edgeChecker.feasible(edge):
            return None
        #feasible edge, add it
        nnew = self.addEdge(nnear,xend,edge)
        nnew.c = nnear.c + edge.length()
        self.nearestNeighbors.add(nnew.x,nnew)
        self.rewire(nnew,k=k,rad=None,first=True,recursive=False)
        return nnew
    
    def rewire(self,n,k=None,rad=None,first=True,recursive=False):
        #gather incoming cost from parent
        changed = False
        if n.parent != None:
            d = n.parent.c + self.metric(n.x,n.parent.x)
            if d != n.c:
                #print "initial cost improvement",n.c,"to",d
                n.c = d
                changed = True
        if changed:
            #self.stats.count('numCostImprovements').add(1)
            pass

        if first or recursive:
            if rad != None:
                neighbors = self.nearestNeighbors.neighbors(n.x,rad)
            elif k != None:
                neighbors = self.nearestNeighbors.knearest(n.x,k)
            else:
                raise ValueError("Rewire: must have either k or rad arguments provided")
            #sort the neighbors by accumulated cost
            dneighbors = []
            #print "Node",n.x,"cost",n.c
            for x,nn in neighbors:
                if nn == n: continue
                if nn == n.parent: continue
                d = self.metric(n.x,nn.x) + nn.c
                #print nn.x,d,self.metric(n.x,nn.x)
                dneighbors.append((d,nn))
            dneighbors = sorted(dneighbors,key=lambda x:x[0])
            #print len(dneighbors),"neighbors, cost range",dneighbors[0][0],"to",dneighbors[-1][0]
            #add incoming edges from neighbors
            for (d,nn) in dneighbors:
                if d > n.c: break
                edge = self.cspace.interpolator(nn.x,n.x)
                if self.edgeChecker.feasible(edge):
                    #print "rrt* cost improvement",n.c,"to",d
                    n.setParent(nn,n.x,edge)
                    n.c = d
                    changed = True
                    #optimization: break on first added neighbor, since they're
                    #sorted by increasing cost
                    if changed:
                        #self.stats.count('numIncomingRewirings').add(1)
                        pass
                    break
        if n.c > self.bestPathCost:
            return
        
        if first or changed:
            for c in n.children:
                self.rewire(c,k=k,rad=rad,first=False,recursive=recursive)

        if first or recursive:
            #add outgoing edges from n
            for (d,nn) in dneighbors:
                newcost = n.c + self.metric(n.x,nn.x)
                if newcost < nn.c:
                    #print "Outgoing improvement from",nn.c,"to",newcost
                    #potential improvement, check edge and rewire if necessary
                    edge = self.cspace.interpolator(n.x,nn.x)
                    if self.edgeChecker.feasible(edge):
                        #self.stats.count('numOutgoingRewirings').add(1)
                        nn.setParent(n,nn.x,edge)
                        #nn.c = newcost
                        self.rewire(nn,k=k,rad=rad,first=False,recursive=recursive)
        
    def pickNode(self,xrand):
        """Picks a node closest to xrand.  If dynamicDomain is True,
        uses the radius associated with the node"""
        res = self.nearestNeighbors.nearest(xrand)
        if res==None: return None
        return res[1]
    def getPath(self,n=None):
        if n == None:
            return self.bestPath
        return TreePlanner.getPath(self,n)