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
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 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