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, **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,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.numIters = 0
        self.bestPathCost = infty
        self.bestPath = None
        if len(params) != 0:
            print "Warning, unused params",params
 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 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 destroy(self):
     TreePlanner.destroy(self)
     self.nearestNeighbors.reset()
     self.goalNodes = []
 def destroy(self):
     TreePlanner.destroy(self)
     self.goalNodes = []
     self.witnessSet = []
 def getPath(self, n=None):
     if n == None:
         return self.bestPath
     return TreePlanner.getPath(self, n)
 def destroy(self):
     TreePlanner.destroy(self)
     self.nearestNeighbors.reset()
     self.goalNodes = []
 def destroy(self):
     TreePlanner.destroy(self)
     self.goalNodes = []
     self.witnessSet = []
 def getPath(self,n=None):
     if n == None:
         return self.bestPath
     return TreePlanner.getPath(self,n)
 def getPath(self,n=None):
     if n == None:
         if len(self.goalNodes)==0:
             return None
         return TreePlanner.getPath(self,self.goalNodes[0])
     return TreePlanner.getPath(self,n)