def Plan(self, start_config, goal_config, epsilon = .001):
start_time = time.time()
tree = RRTTree(self.planning_env, start_config)
plan = []
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
# TODO: Here you will implement the rrt planner
# The return path should be an array
# of dimension k x n where k is the number of waypoints
# and n is the dimension of the robots configuration space
plan.append(start_config)
plan.append(goal_config)
currConfig = start_config;
currID = tree.GetRootId();
while (self.planning_env.ComputeDistance(currConfig,goal_config) > epsilon):
if(random.random() < .9):
newCurrConfig = self.planning_env.GenerateRandomConfiguration();
else:
newCurrConfig = goal_config;
[nearID, nearConfig] = tree.GetNearestVertex(newCurrConfig);
extension = self.planning_env.Extend(nearConfig, newCurrConfig)
# print extension
if (extension != None):
currConfig = extension
currID = tree.AddVertex(currConfig);
tree.AddEdge(nearID, currID);
goalID = tree.AddVertex(goal_config);
tree.AddEdge(currID, goalID);
currConfig = goal_config
currID = goalID;
while 1:
currID = tree.edges[currID];
currConfig = tree.vertices[currID];
if (currID == tree.GetRootId()):
break;
else:
plan.insert(1, currConfig);
for config in plan:
print "config = [%.2f, %.2f]" %(config[0], config[1])
print("--- %s seconds ---" % (time.time() - start_time))
print("--- %s path length ---" % len(plan))
print("--- %s vertices ---" % len(tree.vertices))
return plan
def Plan(self, start_config, goal_config, epsilon=0.1):
tree = RRTTree(self.planning_env, start_config)
plan = []
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
# TODO: Here you will implement the rrt planner
# The return path should be an array
# of dimension k x n where k is the number of waypoints
# and n is the dimension of the robots configuration space
# TO DO:
FAR = True
i = 1
print goal_config
while FAR:
print "Iteration Number: " + str(i)
i += 1
# Sample a random configuration in Cfree
sample = self.planning_env.GenerateRandomConfiguration()
# Find the nearest(best) vertex and start extending from there
best_id, best_v = tree.GetNearestVertex(sample)
validSample = self.planning_env.Extend(best_v, sample)
if validSample is None:
pass
else:
validSample_id = tree.AddVertex(validSample)
tree.AddEdge(best_id, validSample_id)
if self.visualize:
self.planning_env.PlotEdge(best_v, validSample)
# Termination Condition
if self.planning_env.ComputeDistance(validSample,
goal_config) < epsilon:
goal_id = tree.AddVertex(goal_config)
tree.AddEdge(validSample_id, goal_id)
if self.visualize:
self.planning_env.PlotEdge(best_v, validSample)
FAR = False
# Generate the path back from the goal
curr_id = goal_id
while curr_id != 0:
plan.append(tree.vertices[curr_id])
curr_id = tree.edges[curr_id]
plan.append(start_config)
plan.reverse()
return plan
def Plan(self, start_config, goal_config, epsilon = 0.2):
print("Epsilon=", epsilon)
tree = RRTTree(self.planning_env, start_config)
plan = []
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
self.planning_env.SetGoalParameters(goal_config)
NUM_ITERATIONS = 100000
for iter in range(NUM_ITERATIONS):
# Check if close enough to goal
vid, v = tree.GetNearestVertex(goal_config)
d = self.planning_env.ComputeDistance(v, goal_config)
if (iter%10 == 0):
print(iter, ' Closest dist to goal :', d)
if ((d is not None) and (d < epsilon)):
tree.AddEdge(vid, tree.AddVertex(goal_config))
if self.planning_env.visualize:
self.planning_env.PlotEdge(v, goal_config)
print ("Goal Found !!!")
break
v_rand = self.planning_env.GenerateRandomConfiguration()
v_near_id, v_near = tree.GetNearestVertex(v_rand)
v_new = self.planning_env.Extend(v_near, v_rand)
if v_new is not None:
v_new_id = tree.AddVertex(v_new)
tree.AddEdge(v_near_id, v_new_id)
if self.planning_env.visualize:
self.planning_env.PlotEdge(v_near, v_new)
# If goal found, evaluate path
if self.planning_env.ComputeDistance(tree.vertices[-1], goal_config) == 0:
plan.append(goal_config)
id = len(tree.vertices)-1
while (id != tree.GetRootId()):
plan.append(tree.vertices[tree.edges[id]])
id = tree.edges[id]
plan = plan[::-1]
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
if self.planning_env.visualize:
[self.planning_env.PlotEdge(plan[i-1], plan[i]) for i in range(1,len(plan))]
return plan
def Plan(self, start_config, goal_config, epsilon=0.001):
tree = RRTTree(self.planning_env, start_config)
plan = []
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
# TODO: Here you will implement the rrt planner
# The return path should be an array
# of dimension k x n where k is the number of waypoints
# and n is the dimension of the robots configuration space
plan.append(start_config)
while True:
next_config = self.planning_env.GenerateRandomConfiguration()
#next_congfig = self.planning_env.Extend(plan[step-1], next_config)
launch_vertex_id, launch_vertex = tree.GetNearestVertex(
next_config)
next_config = self.planning_env.Extend(launch_vertex, next_config)
if (next_config == None):
continue
else:
next_id = tree.AddVertex(next_config)
tree.AddEdge(launch_vertex_id, next_id)
self.planning_env.PlotEdge(launch_vertex, next_config)
if (self.planning_env.ComputeDistance(goal_config, next_config) <
0.5):
final_config = self.planning_env.Extend(
next_config, goal_config)
if (final_config == None):
continue
else:
#plan.append(next_config)
sid = tree.AddVertex(final_config)
tree.AddEdge(next_id, sid)
path = []
while (sid <> 0):
path.append(sid)
sid = tree.edges.get(sid)
#print(sid)
break
path.reverse()
print(path)
for step in path:
plan.append(tree.vertices[step])
print(plan[-1])
return plan
def Plan(self, start_config, goal_config, epsilon=1):
tree = RRTTree(self.planning_env, start_config)
plan = []
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
# TODO: Here you will implement the rrt planner
# The return path should be an array
# of dimension k x n where k is the number of waypoints
# and n is the dimension of the robots configuration space
# forward pass
while (True):
# generate point
new_p = self.planning_env.GenerateRandomConfiguration()
# get id and distance of a node closest
(v_id, n_vertex) = tree.GetNearestVertex(new_p)
# see if the closet point to new point can connect
potential = self.planning_env.Extend(n_vertex, new_p)
# check if you can connect
if (potential != None):
# check distance
dist = self.planning_env.ComputeDistance(
potential, goal_config)
# add to tree class
temp = tree.AddVertex(potential)
tree.AddEdge(v_id, temp)
self.planning_env.PlotEdge(n_vertex, potential)
if (self.planning_env.ComputeDistance(potential, goal_config) <=
epsilon):
break
# back prop
plan.append(goal_config)
get_keys = tree.edges.keys()
tree_key = get_keys[len(tree.edges) - 1]
while (True):
plan.append(tree.vertices[tree_key])
tree_key = tree.edges.get(tree_key)
if (tree_key == 0):
break
# append starting point and reverse
plan.append(start_config)
plan.reverse()
return plan
def Plan(self, start_config, goal_config, epsilon = 0.001):
tree = RRTTree(self.planning_env, start_config)
plan = []
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
# TODO: Here you will implement the rrt planner
# The return path should be an array
# of dimension k x n where k is the number of waypoints
# and n is the dimension of the robots configuration space
dist = float('inf')
while dist > epsilon:
# Bias Sampling with P of sampling towards the Goal = 0.1
p = random.random()
if p < 0.1:
q_r = goal_config
else:
q_r = self.planning_env.GenerateRandomConfiguration()
# Get the Vertex closest to q_r
sid, q_n = tree.GetNearestVertex(q_r)
# Using linear interpolation to get the furthest vertex without collision
q_c = self.planning_env.Extend(q_n, q_r)
# Add vertex to the tree
eid = tree.AddVertex(q_c)
# Add an edge on the tree
tree.AddEdge(sid, eid)
# Check how close we are to the goal
dist = self.planning_env.ComputeDistance(q_c, goal_config)
# print q_n
# print q_c
if self.visualize:
self.planning_env.PlotEdge(q_n, q_c)
vid = eid
while (vid != tree.GetRootId()):
vid = tree.edges[vid]
plan = [tree.vertices[vid]] + plan
plan.append(goal_config)
# print len(plan)
return plan
def Plan(self, start_config, goal_config, epsilon=0.001):
tree = RRTTree(self.planning_env, start_config)
plan = []
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
# TODO: Here you will implement the rrt planner
# The return path should be an array
# of dimension k x n where k is the number of waypoints
# and n is the dimension of the robots configuration space
#plan.append(start_config)
new_config = start_config
distance = 100
while distance > epsilon:
r = random()
if r > self.planning_env.p:
target_config = self.planning_env.GenerateRandomConfiguration()
else:
target_config = goal_config
current_id, current_config = tree.GetNearestVertex(target_config)
new_config = self.planning_env.Extend(current_config,
target_config)
if new_config == None:
pass
else:
new_id = tree.AddVertex(new_config)
tree.AddEdge(current_id, new_id)
#self.planning_env.PlotEdge(tree.vertices[current_id], tree.vertices[new_id])
distance = self.planning_env.ComputeDistance(
new_config, goal_config)
plan = []
while new_id != 0:
new_id = tree.edges[new_id]
prev_node = tree.vertices[new_id]
plan.insert(0, prev_node)
plan.append(goal_config)
return plan
def Plan(self, start_config, goal_config, epsilon=.001):
start_time = time.time()
tree = RRTTree(self.planning_env, start_config)
plan = []
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
# TODO: Here you will implement the rrt planner
# The return path should be an array
# of dimension k x n where k is the number of waypoints
# and n is the dimension of the robots configuration space
plan.append(start_config)
plan.append(goal_config)
startNodeID = self.planning_env.discrete_env.ConfigurationToNodeId(
start_config)
goalNodeID = self.planning_env.discrete_env.ConfigurationToNodeId(
goal_config)
hCost = {}
hCost[startNodeID] = 0
treeOptCost = self.planning_env.ComputeHeuristicCost(
startNodeID, goalNodeID)
treeMaxCost = 0
currConfig = start_config
currNodeID = startNodeID
currID = tree.GetRootId()
print "startConfig = [%.2f, %.2f]" % (start_config[0], start_config[1])
print "goalConfig = [%.2f, %.2f]" % (goal_config[0], goal_config[1])
#while (self.planning_env.Extend(currConfig, goal_config) == None):
while (self.planning_env.ComputeDistance(currNodeID, goalNodeID) >
epsilon):
if (random.random() < .9):
while True:
newCurrConfig = self.planning_env.GenerateRandomConfiguration(
)
[nearID, nearConfig] = tree.GetNearestVertex(newCurrConfig)
nearNodeID = self.planning_env.discrete_env.ConfigurationToNodeId(
nearConfig)
nearCost = hCost[
nearNodeID] + self.planning_env.ComputeHeuristicCost(
nearNodeID, goalNodeID)
mQuality = (1 - ((nearCost - treeOptCost) /
(treeMaxCost - treeOptCost)))
mQuality = max(mQuality, 0.2)
r = random.random()
print "mQuality = %.2f and r = %.2f" % (mQuality, r)
if (r < mQuality):
break
else:
newCurrConfig = goal_config
[nearID, nearConfig] = tree.GetNearestVertex(newCurrConfig)
print "newCurrConfig = [%.2f, %.2f]" % (newCurrConfig[0],
newCurrConfig[1])
print "nearID = %d, nearConfig = [%.2f, %.2f]" % (
nearID, nearConfig[0], nearConfig[1])
extension = self.planning_env.Extend(nearConfig, newCurrConfig)
print extension
if (extension != None):
currConfig = extension
currID = tree.AddVertex(currConfig)
tree.AddEdge(nearID, currID)
nearNodeID = self.planning_env.discrete_env.ConfigurationToNodeId(
nearConfig)
extensionNodeID = self.planning_env.discrete_env.ConfigurationToNodeId(
extension)
currNodeID = extensionNodeID
hCost[extensionNodeID] = hCost[
nearNodeID] + self.planning_env.ComputeHeuristicCost(
nearNodeID, extensionNodeID)
extensionCost = hCost[
extensionNodeID] + self.planning_env.ComputeHeuristicCost(
extensionNodeID, goalNodeID)
treeMaxCost = max(extensionCost, treeMaxCost)
#plan.append(currConfig)
print "currID = %d, currConfig = [%.2f, %.2f]" % (
currID, currConfig[0], currConfig[1])
self.planning_env.PlotEdge(nearConfig, currConfig)
goalID = tree.AddVertex(goal_config)
tree.AddEdge(currID, goalID)
self.planning_env.PlotEdge(currConfig, goal_config)
currConfig = goal_config
currID = goalID
while 1:
currID = tree.edges[currID]
currConfig = tree.vertices[currID]
if (currID == tree.GetRootId()):
break
else:
plan.insert(1, currConfig)
for config in plan:
print "config = [%.2f, %.2f]" % (config[0], config[1])
print("--- %s seconds ---" % (time.time() - start_time))
print("--- %s path length ---" % len(plan))
print("--- %s vertices ---" % len(tree.vertices))
return plan
def Plan(self, start_config, goal_config):
epsilon = 0.001
start_time = time.time()
tree = RRTTree(self.planning_env, start_config, goal_config)
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
start_time = time.time()
count = 0
start = [round(q, 2) for q in start_config]
goal = [round(f, 2) for f in goal_config]
num_turns = 1 / epsilon
prob = 10
constant = 10
#solution_set={}
n = 0
cost = {}
cost[0] = 0
ellipse = float("inf")
informed = 1
while informed == 1:
while count != num_turns:
count = count + 1
sumrand = 0
it = True
if prob > 3:
while (it == True):
rand = self.planning_env.generate_random_configuration(
)
sumrand = self.planning_env.compute_distance(
rand, start) + self.planning_env.compute_distance(
rand, goal)
if sumrand <= ellipse:
it = False
prob = prob - 1
else:
rand = goal
prob = 10
#c= input ("stop here")
Nid, neighbour = tree.GetNearestVertex(rand)
new_guy = self.planning_env.extend(neighbour, rand)
if self.planning_env.state_validity_checker(new_guy):
count = count - 1
continue
if self.planning_env.edge_validity_checker(new_guy, neighbour):
count = count - 1
continue
r = (
(math.log10(count) / count)**(1 / len(new_guy))) * constant
near = {}
for nearID, i in enumerate(tree.vertices, 0):
if self.planning_env.compute_distance(new_guy, i) < min(
r, 0.05):
near[nearID] = i
xmin = Nid
xmincon = neighbour
cmin = cost[Nid] + self.planning_env.compute_distance(
neighbour, new_guy)
for nearID, nodes in near.iteritems():
#print "nearID",nearID,nodes
cnew = cost[nearID] + self.planning_env.compute_distance(
nodes, new_guy)
if cnew < cmin:
if self.planning_env.edge_validity_checker(
nodes, new_guy) == False:
xmin = nearID
xmincon = nodes
cmin = cnew
New_id = tree.AddVertex(new_guy)
tree.AddEdge(xmin, New_id)
#self.planning_env.PlotEdge(xmincon,new_guy)
cost[New_id] = cmin
#print "reached here",xmin,New_id
for nearID, nodes in near.iteritems():
cnear = cost[nearID]
cnew = cost[New_id] + self.planning_env.compute_distance(
nodes, new_guy)
if cnew < cnear:
if self.planning_env.edge_validity_checker(
nodes, new_guy) == False:
del tree.edges[nearID]
tree.AddEdge(New_id, nearID)
#self.planning_env.PlotEdge(nodes,new_guy)
if new_guy == goal:
n = n + 1
#print "Goal Reached",n
#c= input("stop here")
#print "edges",tree.edges[New_id]
#print "vertices",tree.vertices
last = New_id
book = []
while last != 0:
book.append(tree.vertices[last])
last = tree.edges[last]
#print "last",last
#print "tree.vertices",tree.vertices[last]
book.append(tree.vertices[last])
# "book",book
#c= input("stop here")
x = len(book)
nicebook = [0] * x
for i in range(x):
nicebook[i] = book[x - i - 1]
#print "something is done"
if (time.time() - start_time) < 5:
ellipse = self.planning_env.calc_elipse(nicebook)
continue
#print "count",count
dist = 0
f = start
for i in nicebook:
dist = dist + self.planning_env.compute_distance(f, i)
f = i
print "time take=", start_time - time.time()
print "total path =", dist
print "nicebookend ", nicebook[len(nicebook) - 1]
print "goal", goal
return nicebook
if new_guy == goal:
continue
else:
break
print "count", count
print "Goal NOT foud"
plan = []
plan.append(start_config)
plan.append(goal_config)
return plan
def Plan(self, start_config, goal_config, epsilon = 0.001):
tree = RRTTree(self.planning_env, start_config,goal_config)
plan = []
start_time = time.time()
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
# TODO: Here you will implement the hrrt planner
# The return path should be an array
# of dimension k x n where k is the number of waypoints
# and n is the dimension of the robots configuration space
count =0
start = [round(q,2) for q in start_config]
goal= [round(f,2) for f in goal_config]
num_turns= 1/epsilon
goal_prob=10
prob=0.3
optcost = self.planning_env.compute_distance(start,goal)
#print "optcost",optcost
while count != num_turns:
count= count+1
#print "count",count
if goal_prob>3:
if count>2:
r=0.5
m=0
while r>m :
r= random.random()
rand= self.planning_env.generate_random_configuration()
Nid,neighbour= tree.GetNearestVertex(rand)
#print "tree.fvalue[Nid]",tree.fvalue[Nid]
#print "max fvalue",max(tree.fvalue)
m= 1- ((tree.fvalue[Nid]-optcost)/(max(tree.fvalue)-optcost))
#print "m",m
m= min(m,prob)
#print "r",r
#print "out of this loop"
#c= input ("enter guy")
else:
rand= self.planning_env.generate_random_configuration()
Nid,neighbour= tree.GetNearestVertex(rand)
#print "just came of the else"
goal_prob= goal_prob-1
else:
rand= goal
Nid,neighbour= tree.GetNearestVertex(rand)
goal_prob=10
new_guy= self.planning_env.extend(neighbour,rand)
if self.planning_env.state_validity_checker(new_guy):
count= count-1
continue
New_id= tree.AddVertex(new_guy)
tree.AddCost(New_id,Nid)
#print "neighbour",neighbour,"new_guy",new_guy
tree.AddEdge(Nid,New_id)
if self.planning_env.dimension==2:
self.planning_env.PlotEdge(neighbour,new_guy)
#print "new_guy",new_guy
if new_guy==goal:
print "Goal Reached"
last=New_id
book=[]
while last!=0:
book.append(tree.vertices[last])
last= tree.edges[last]
book.append(tree.vertices[last])
x= len(book)
nicebook= [0]*x
for i in range(x):
nicebook[i]= book[x-i-1]
dist= 0
f=start
for i in nicebook:
dist = dist + self.planning_env.compute_distance(f,i)
f = i
print "total path =",dist
print("--- %s seconds ---" % (time.time() - start_time))
print "count",count
return nicebook
#c= input ("enter guy")
#print "cost",tree.cost
print "Goal NOT foud"
return 0
class RRTStarPlanner(object):
def __init__(self, planning_env, bias = 0.05, eta = 1.0, max_iter = 10000):
self.env = planning_env # Map Environment
self.tree = RRTTree(self.env)
self.bias = bias # Goal Bias
self.max_iter = max_iter # Max Iterations
self.eta = eta # Distance to extend
self.childs={}
def Plan(self, start_config, goal_config, rad=50):
# TODO: YOUR IMPLEMENTATION HERE
start_config= start_config.ravel()
goal_config= goal_config.ravel()
plan_time = time.time()
plan = []
# Start with adding the start configuration to the tree.
self.tree.AddVertex(start_config)
goal=False
for i in range(self.max_iter):
if (i%1000==0):
print(i)
r= self.sample(goal_config)
vid,vertex = self.tree.GetNearestVertex(r)
if (r.ravel()==vertex).all():
continue
new= self.extend(vertex,r)
# RRtstar algo
if (self.env.edge_validity_checker(vertex.reshape((2,1)),new.reshape((2,1)))):
# if obstacle free
X_nearID,X_near= self.tree.GetNNInRad(new,rad)
x_min= vertex
c_min=self.tree.costs[vid]+self.env.compute_distance(vertex, new)
min_ID= vid
for close_neigh,cID in zip(X_near,X_nearID):
if (self.env.edge_validity_checker(close_neigh.reshape((2,1)),new.reshape((2,1)))):
new_cost= self.tree.costs[cID]+self.env.compute_distance(close_neigh, new)
if (new_cost<c_min):
c_min= new_cost
x_min= close_neigh
min_ID= cID
nid= self.tree.AddVertex(new,c_min)
self.tree.AddEdge(min_ID,nid)
self.add_parents(min_ID,nid)
#rewiring the tree
for close_neigh,cID in zip(X_near,X_nearID):
if (self.env.edge_validity_checker(close_neigh.reshape((2,1)),new.reshape((2,1)))):
new_cost= c_min+self.env.compute_distance(close_neigh, new)
if (new_cost<self.tree.costs[cID]):
# update_edge
prev_parent= self.tree.edges[cID]
self.childs[prev_parent].remove(cID)
self.tree.AddEdge(nid,cID)
self.add_parents(nid,cID)
self.update_cost_tree(cID,self.tree.costs[cID]-new_cost)
else:
continue
if(new==goal_config.ravel()).all():
print('goal_reached')
goal= True
idx=nid
while (idx!=0):
plan.append(self.tree.vertices[idx])
idx= self.tree.edges[idx]
plan.append(self.tree.vertices[idx])
break
plan=np.asarray(plan)
plan= plan[::-1]
cost = new_cost
plan_time = time.time() - plan_time
print("Cost: %f" % cost)
print("Planning Time: %ds" % plan_time)
return plan.T
def extend(self, x_near, x_rand):
# TODO: YOUR IMPLEMENTATION HERE
start = x_near.ravel()
end = x_rand.ravel()
v= end-start
u= v/(np.sqrt(np.sum(v**2)))
d= self.env.compute_distance(start,end)*self.eta
point = start+ u*d
for i in range(2):
point[i]= round(point[i],2)
return point
def update_cost_tree(self, ID,cost_diff):
queue=[]
queue.append(ID)
while(len(queue)!=0):
new_ID= queue.pop()
self.tree.costs[new_ID]-=cost_diff
if (new_ID in self.childs.keys()):
for i in self.childs[new_ID]:
queue.append(i)
def add_parents(self,parent,child):
if (parent not in self.childs.keys()):
self.childs[parent]=[]
self.childs[parent].append(child)
# def calc_cost(self,ID):#to calculate cost from start node every time because costs are chainging when rewiring the tree
# total_cost=0
# prev_ID= ID
# while(prev_ID!=0):
# next_ID= self.tree.edges[prev_ID]
# total_cost+=self.env.compute_distance(self.tree.vertices[prev_ID],self.tree.vertices[next_ID] )
# prev_ID= next_ID
# return total_cost
def sample(self, goal):
# Sample random point from map
if np.random.uniform() < self.bias:
return goal
return self.env.sample()
class RRTStarPlanner(object):
def __init__(self, planning_env):
self.planning_env = planning_env
self.tree = RRTTree(self.planning_env)
self.step_size = 5.0
self.one_step = True
self.cost = {}
self.neighbers = 3
self.p_threshold = 0.05
def Plan(self, start_config, goal_config, epsilon=0.001):
# Initialize an empty plan.
plan = []
# Start with adding the start configuration to the tree.
start = time.time()
self.tree.AddVertex(start_config)
plan.append(start_config)
self.cost[0] = 0
for i in range(8000):
while True:
if random.random() <= self.p_threshold:
x_rand = goal_config
else:
x_rand = [
random.randint(0, self.planning_env.xlimit[1]),
random.randint(0, self.planning_env.ylimit[1])
]
if self.planning_env.state_validity_checker(x_rand):
break
x_near_id, x_near_vertex, vdist = self.tree.GetNearestVertex(
x_rand)
x_new = self.extend(x_near_vertex, x_rand)
if (not len(x_new)) or (
not self.planning_env.state_validity_checker(x_new)):
continue
if len(self.tree.vertices) > self.neighbers:
# Change father
knnIDs, x_new_neighbers_vertices, knnDists = self.tree.GetKNN(
x_new, self.neighbers)
total_dis = []
for i in range(self.neighbers):
total_dis.append(self.cost[knnIDs[i]] + knnDists[i])
new_father_id = knnIDs[total_dis.index(min(total_dis))]
x_new_id = self.tree.AddVertex(x_new)
self.cost[x_new_id] = round(min(total_dis), 2)
# rewire
for i in range(self.neighbers):
if self.cost[x_new_id] + knnDists[i] < self.cost[
knnIDs[i]]:
self.tree.AddEdge(x_new_id, knnIDs[i])
self.cost[knnIDs[i]] = self.cost[x_new_id] + round(
knnDists[i], 2)
else:
new_father_id = x_near_id
x_new_id = self.tree.AddVertex(x_new)
self.cost[x_new_id] = self.cost[new_father_id] + round(
vdist, 2)
self.tree.AddEdge(new_father_id, x_new_id)
if self.planning_env.compute_distance(x_new,
goal_config) < epsilon:
s_id = x_new_id
temp_list = [x_new]
while self.tree.edges[s_id] != self.tree.GetRootID():
temp_list.append(self.tree.vertices[s_id])
s_id = self.tree.edges[s_id]
plan += temp_list[::-1]
break
plan.append(goal_config)
end = time.time()
print('The time is', str(end - start))
print('The cost is ', self.cost[len(self.tree.vertices) - 2])
return numpy.array(plan)
def extend(self, x_near, x_rand):
x_new = []
dis = self.planning_env.compute_distance(x_near, x_rand)
if dis == 0:
return []
x_near = numpy.array(x_near, dtype=numpy.float32)
x_rand = numpy.array(x_rand, dtype=numpy.float32)
delta_x_new = x_rand - x_near
if self.one_step:
step_size = 1.0
delta_x = delta_x_new[0] * (step_size / dis)
delta_y = delta_x_new[1] * (step_size / dis)
for i in range(1, int(dis / step_size)):
temp = [x_near[0] + delta_x * i, x_near[1] + delta_y * i]
if not self.planning_env.state_validity_checker(temp):
return []
x_new = x_rand
else:
if dis >= self.step_size:
x_new.append(x_near[0] + delta_x_new[0] *
(self.step_size / dis))
x_new.append(x_near[1] + delta_x_new[1] *
(self.step_size / dis))
else:
x_new = x_rand
return x_new
def ShortenPath(self, path):
start_point = 0
end_point = 1
new_path = [path[start_point]]
while end_point < len(path):
if self.cut(start_point, end_point, path):
end_point += 1
else:
new_path.append(path[end_point])
start_point = end_point
end_point += 1
return new_path
def cut(self, start_point, end_point, path):
dis = self.planning_env.compute_distance(path[start_point],
path[end_point])
x_start = numpy.array(path[start_point], dtype=numpy.float32)
x_end = numpy.array(path[end_point], dtype=numpy.float32)
delta_x_new = x_start - x_end
step_size = 0.1
delta_x = delta_x_new[0] * (step_size / dis)
delta_y = delta_x_new[1] * (step_size / dis)
for i in range(1, int(dis / step_size)):
temp = [x_start[0] + delta_x * i, x_start[1] + delta_y * i]
if not self.planning_env.state_validity_checker(temp):
return False
return True
def Plan(self, start_config, goal_config, epsilon=0.001):
tree = RRTTree(self.planning_env, start_config)
plan = []
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
# TODO: Here you will implement the rrt planner
# The return path should be an array
# of dimension k x n where k is the number of waypoints
# and n is the dimension of the robots configuration space
threshold = 0.3
extendedConfig = self.planning_env.GenerateRandomConfiguration()
start_id = self.planning_env.discrete_env.ConfigurationToNodeId(
start_config)
goal_id = self.planning_env.discrete_env.ConfigurationToNodeId(
goal_config)
self.g_scores = dict()
self.f_scores = dict()
self.g_scores[start_id] = 0
self.f_scores[start_id] = self.planning_env.ComputeHeuristicCost(
start_id, goal_id)
m_quality = 0
prob_floor = 0.4
C_opt = self.planning_env.ComputeDistance(start_id, goal_id)
def compDist(start_config, end_config):
return numpy.linalg.norm(
numpy.array(start_config) - numpy.array(end_config), 2)
while compDist(extendedConfig, goal_config) > threshold:
m_quality = 0.0
r = random.random()
while (r > m_quality):
prob = random.random()
if (prob < 0.2):
config = numpy.copy(goal_config)
xnnIDs, xnnVertices = tree.GetKNearestVertex(
(config), len(tree.vertices))
xnnID, xnnVertice = tree.GetNearestVertex((config))
m_quality = 1
else:
config = self.planning_env.GenerateRandomConfiguration()
xnnIDs, xnnVertices = tree.GetKNearestVertex(
(config), len(tree.vertices))
xnnID, xnnVertice = tree.GetNearestVertex((config))
C_vertex = self.planning_env.ComputeHeuristicCost(
self.planning_env.discrete_env.ConfigurationToNodeId(
config), goal_id)
C_max = max(self.f_scores.values())
q = (1 - ((C_vertex - C_opt) / float(C_max - C_opt)))
print "m_quality ", q
m_quality = min(
prob_floor,
abs(1 - ((C_vertex - C_opt) / float(C_max - C_opt))))
r = random.random()
print "m_quality ", m_quality
for i in range(len(tree.vertices)):
path = self.planning_env.Extend(xnnVertices[i], config)
if path != None:
xnnID = xnnIDs[i]
xnnVertice = xnnVertices[i]
break
if (path != None and len(path) != 0):
extendedConfig = path[-1, :]
vid = tree.AddVertex(extendedConfig)
if vid != xnnID:
tree.AddEdge(xnnID, vid)
g_score = self.planning_env.ComputeDistance(vid, xnnID)
self.g_scores[vid] = g_score + self.g_scores[xnnID]
self.f_scores[
vid] = g_score + self.planning_env.ComputeHeuristicCost(
vid, goal_id)
if self.visualize:
self.planning_env.PlotEdge(tree.vertices[xnnID],
tree.vertices[vid])
plan.append(goal_config)
curr_id = goal_id
while (curr_id != start_id):
print "current id ", curr_id
plan.append(tree.vertices[curr_id]) # Add the new vertex to plan
curr_id = tree.edges[
curr_id] # Get the vertex opposite the edge of the current id
plan.append(start_config)
plan = plan[::-1]
print "Number of vertices in the tree:", len(tree.vertices)
return plan, len(tree.vertices)
class RRTPlanner(object):
def __init__(self, planning_env, bias=0.05, eta=1.0, max_iter=100000):
self.env = planning_env # Map Environment
self.tree = RRTTree(self.env)
self.bias = bias # Goal Bias
self.max_iter = max_iter # Max Iterations
self.eta = eta # Distance to extend
def Plan(self, start_config, goal_config):
# TODO: YOUR IMPLEMENTATION HERE
plan_time = time.time()
# Start with adding the start configuration to the tree.
self.tree.AddVertex(start_config)
plan = []
plan.append(start_config)
for i in range(self.max_iter):
x_rand = self.sample(goal_config)
x_near_id, x_near = self.tree.GetNearestVertex(x_rand)
# x_near = self.tree.vertices[x_near_id]
x_new = self.extend(x_near, x_rand)
x_new = np.array(x_new)
if (len(x_new)
== 0) or (not self.env.state_validity_checker(x_new)):
continue
x_new_id = self.tree.AddVertex(x_new)
self.tree.AddEdge(x_near_id, x_new_id)
if self.env.compute_distance(x_new, goal_config) < 0.0001:
end_id = x_new_id
traj = [x_new]
while self.tree.edges[end_id] != self.tree.GetRootID():
traj.append(self.tree.vertices[end_id])
end_id = self.tree.edges[end_id]
plan += traj[::-1]
break
plan.append(goal_config)
cost = 0
for i in range(len(plan) - 1):
cost += ((plan[i + 1][0] - plan[i][0])**2 +
(plan[i + 1][1] - plan[i][1])**2)**0.5
plan_time = time.time() - plan_time
print("Cost: %f" % cost)
print("Planning Time: %ds" % plan_time)
return np.concatenate(plan, axis=1)
def extend(self, x_near, x_rand):
# TODO: YOUR IMPLEMENTATION HERE
# x_new = []
dist = self.env.compute_distance(x_near, x_rand)
if dist == 0:
return []
if not self.env.edge_validity_checker(x_near, x_rand):
return []
d_new = x_rand - x_near
d_x = d_new[0, 0] * self.eta
d_y = d_new[1, 0] * self.eta
if self.eta <= dist:
x_new = np.zeros(x_near.shape)
x_new[0, 0] = x_near[0, 0] + d_x
x_new[1, 0] = x_near[1, 0] + d_y
return x_new
else:
return x_rand
def sample(self, goal):
# Sample random point from map
if np.random.uniform() < self.bias:
return goal
return self.env.sample()
class RRTPlanner(object):
def __init__(self, planning_env, bias = 0.05):
self.planning_env = planning_env
self.tree = RRTTree(self.planning_env)
self.bias = bias # bias of sampling to pick the goal
def Plan(self, start_config, goal_config, epsilon = -1):
# Initialize an empty plan.
plan = []
c_space = list(zip(numpy.where(self.planning_env.map==0)[0],
numpy.where(self.planning_env.map==0)[1]))
# Start with adding the start configuration to the tree.
self.tree.AddVertex(start_config)
plan.append(start_config)
# TODO (student): Implement your planner here.
connected_to_goal = False
while not connected_to_goal:
x_rand = self.sample_random_state(c_space,goal_config)
nn_id, nn = self.tree.GetNearestVertex(x_rand)
x_new = self.extend(x_rand, nn, epsilon)
if self.planning_env.edge_validity_checker(nn, x_new, discrete=False):
x_new_id = self.tree.AddVertex(x_new)
self.tree.AddEdge(nn_id, x_new_id)
plot_x = [nn[0], x_new[0]]
plot_y = [nn[1], x_new[1]]
plt.plot(plot_y, plot_x,'-or',markersize=1)
plan.append(x_new)
if x_new == goal_config:
connected_to_goal = True
return numpy.array(plan)
def sample_random_state(self, c_space, goal):
sample_from = numpy.random.random()
if sample_from > self.bias: #sample from the entire space
rnd_idx = numpy.random.randint(1, len(c_space))
v = list(c_space[rnd_idx])
while not self.planning_env.state_validity_checker(v):
rnd_idx = numpy.random.randint(1, len(c_space))
v = list(c_space[rnd_idx])
return v
else: # sample only from x_goal. in our case just return the single goal
return list(goal)
def extend(self, x_rand, x_near, epsilon):
# TODO (student): Implement an extend logic.
if epsilon < 0: #E1 nearest neighbor tries to extend all the way till the sampled point
return x_rand
# E2 nearest neighbor tries to extend to the sampled point by a step size epsilon
# create a unit vector from nearest neighbor to x_rand
vec = numpy.array([x_rand[0] - x_near[0], x_rand[1] - x_near[1]])
if numpy.linalg.norm(vec) == 0:
unit_vec = numpy.array([0,0])
else:
unit_vec = vec / numpy.linalg.norm(vec)
# go epsilon in diretion of vec
x_new = epsilon*unit_vec + numpy.array(x_near)
return x_new.tolist()
def ShortenPath(self, path):
# TODO (student): Postprocessing of the plan.
current = self.tree.vertices[-1]
current_id = len(self.tree.vertices) -1
if current != self.planning_env.goal:
print ("Path was not found!")
short_path = [current]
while current != path[0].tolist():
for i,prev in enumerate(short_path[:-2]):
if self.planning_env.edge_validity_checker(current, prev, discrete=False):
short_path = short_path[:i+1] + [short_path[-1]]
current_id = self.tree.edges[current_id]
current = self.tree.vertices[current_id]
short_path.append(current)
short_path.reverse()
return numpy.array(short_path)
class RRTPlannerNonholonomic(object):
def __init__(self,
planning_env,
bias=0.05,
max_iter=3000,
num_control_samples=25):
self.env = planning_env # Car Environment
self.tree = RRTTree(self.env)
self.bias = bias # Goal Bias
self.max_iter = max_iter # Max Iterations
self.num_control_samples = 25 # Number of controls to sample
def Plan(self, start_config, goal_config):
# TODO: YOUR IMPLEMENTATION HERE
plan_time = time.time()
# Start with adding the start configuration to the tree.
self.tree.AddVertex(start_config)
for it in range(self.max_iter):
# generate random node
if it % 200 == 0:
print(it)
if np.random.random() < self.bias:
x_rand = goal_config
else:
x_rand = self.env.sample()
# find nearest node
vid_near, x_near = self.tree.GetNearestVertex(x_rand)
# extend
x_extend, rollout_extend, action_extend = self.extend(
x_near, x_rand)
if x_extend is None:
continue
# add extend node
cost = self.tree.costs[vid_near]
vid_extend = self.tree.AddVertex(
x_extend, cost + self.env.compute_distance(x_near, x_extend))
self.tree.AddEdge(vid_near, vid_extend, action_extend,
rollout_extend)
# check goal
if self.env.goal_criterion(x_extend, goal_config):
break
# if fail
plan = []
if it == self.max_iter - 1:
print("fail")
plan.append(start_config)
plan.append(goal_config)
return np.concatenate(plan, axis=1), None, None
vid = vid_extend
conf = goal_config
plan.append(conf)
rollouts = []
actions = []
cost = self.tree.costs[vid]
while not (conf[0][0] == start_config[0][0]
and conf[1][0] == start_config[1][0]):
(vid, action, rollout) = self.tree.edges[vid]
actions.append(
action.reshape((2, -1)).repeat([len(rollout[0]) - 1], axis=1))
rollouts.append(rollout[:, 1:])
conf = self.tree.vertices[vid]
plan.append(conf)
plan.reverse()
actions.reverse()
rollouts.append(start_config)
rollouts.reverse()
plan_time = time.time() - plan_time
print("Cost: %f" % cost)
print("Planning Time: %ds" % plan_time)
return np.concatenate(plan, axis=1), np.concatenate(
actions, axis=1), np.concatenate(rollouts, axis=1)
def extend(self, x_near, x_rand):
""" Extend method for non-holonomic RRT
Generate n control samples, with n = self.num_control_samples
Simulate trajectories with these control samples
Compute the closest closest trajectory and return the resulting state (and cost)
"""
# TODO: YOUR IMPLEMENTATION HERE
x_extend = None
rollout_extend = None
action_extend = None
dist = self.env.compute_distance(x_near, x_rand)
for i in range(self.num_control_samples):
linear_vel, steer_angle = self.env.sample_action()
x_next, rollout = self.env.simulate_car(x_near, x_rand, linear_vel,
steer_angle)
if x_next is None:
continue
d = self.env.compute_distance(x_next, x_rand)
if d < dist:
dist = d
x_extend = x_next
rollout_extend = rollout
action_extend = np.array([linear_vel, steer_angle])
return x_extend, rollout_extend, action_extend
def sample(self, goal):
# Sample random point from map
if np.random.uniform() < self.bias:
return goal
return self.env.sample()
def Plan(self, start_config, goal_config, heuristic, epsilon):
#Default to regular RRT
tree = RRTTree(self.planning_env, start_config)
plan = []
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
# TODO: Here you will implement the rrt planner
# The return path should be an array
# of dimension k x n where k is the number of waypoints
# and n is the dimension of the robots configuration space
plan_temp = []
plan_temp.append(start_config)
# tree.AddVertex(start_config)
plan_parent = []
plan_parent.append(start_config);
# epsilon = .01
print "start: ", start_config, " goal: ", goal_config
count = 0;
self.env = self.planning_env.robot.GetEnv()
robot_name = self.env.GetBodies()[0].GetName()
#heuristic stuff
# initialize first vertex cost (start id)
# get node ids for goal, start
startId = self.planning_env.discrete_env.ConfigurationToNodeId(start_config)
goalId = self.planning_env.discrete_env.ConfigurationToNodeId(goal_config)
startHeuristic = self.planning_env.ComputeHeuristicCost(startId, goalId)
vertexCosts = {0 : startHeuristic} # compute optimal heuristic cost constant
optCost = self.planning_env.ComputeHeuristicCost(startId, goalId)
# initialize maximum cost
maxCost= 0.0
# initialize mFloor
mFloor = 0.6
# with self.env:
# with self.planning_env.robot.CreateRobotStateSaver():
while(1):
# import pdb; pdb.set_trace()
if heuristic:
r = 1
mQuality = 0
vid1 = 1
while r > mQuality:
if count%10 == 0:
sample_config = goal_config
else:
sample_config = self.planning_env.GenerateRandomConfiguration()
vid1, nearest_vertex = tree.GetNearestVertex(sample_config) # Finding the nearest vertex
vertexNodeId = self.planning_env.discrete_env.ConfigurationToNodeId(nearest_vertex)
vertexCost = self.computeVertexCost(vid1, vertexCosts, tree)# + self.planning_env.ComputeHeuristicCost(vertexNodeId, goalId)
# vertexCosts[vid1] = vertexCost
mQuality = 1.0 - numpy.divide((vertexCost-optCost), (maxCost-optCost))
mQuality = min(mQuality, mFloor)
r = numpy.random.random(1)[0]
# import pdb; pdb.set_trace()
if vid1 == 0:
break
# import pdb; pdb.set_trace()
else:
if count%10 == 0:
sample_config = goal_config
else:
sample_config = self.planning_env.GenerateRandomConfiguration()
vid1, nearest_vertex = tree.GetNearestVertex(sample_config) # Finding the nearest vertex
sample_extend_config = self.planning_env.Extend(nearest_vertex,sample_config, epsilon) # Checking for collision
if sample_extend_config == None:
count = count + 1
continue
plan_parent.append(nearest_vertex)
plan_temp.append(sample_extend_config)
vid2 = tree.AddVertex(sample_extend_config)
tree.AddEdge(vid1,vid2)
if heuristic:
# import pdb; pdb.set_trace()
prevId = tree.edges[vid2]
vertexNodeId2 = self.planning_env.discrete_env.ConfigurationToNodeId(sample_extend_config)
newVertexCost = vertexCosts[prevId] + self.planning_env.ComputeDistance(prevId, vertexNodeId2) + 3*self.planning_env.ComputeHeuristicCost(vertexNodeId2, goalId)
vertexCosts[vid2] = newVertexCost
if newVertexCost > maxCost:
maxCost = newVertexCost
if (robot_name != 'Herb2'): # Visualize only for PR2
self.planning_env.PlotEdge(nearest_vertex,plan_temp[-1])
if self.planning_env.HRRTComputeDistance(plan_temp[-1],goal_config) < epsilon: # Break Condition
break
count = count + 1
plan = []
plan.append(goal_config)
plan.append(tree.vertices[-1])
lastid = vid2
# import pdb; pdb.set_trace()
while(1):
nextid = tree.edges[lastid]
# nextid = tree.edges.keys()[tree.edges.values().index(lastid)]
plan.append(tree.vertices[nextid])
lastid = nextid
if numpy.array_equal(plan[-1],start_config):
break
plan = plan[::-1]
dist = 0.0
for i in xrange(0,len(plan) - 1):
dist = dist + numpy.linalg.norm(plan[i + 1] - plan[i])
if (robot_name != 'Herb2'): # Visualize only for PR2
self.planning_env.PlotEdge(plan[i], plan[i+1], 'r')
print "Path Length: ", dist
print "Number of vertices: ", len(plan)
return plan
class RRTPlanner(object):
def __init__(self, planning_env, visualize):
self.planning_env = planning_env
self.visualize = visualize
self.tree = []
self.path = []
self.result = []
self.arrived = False
def Plan(self, start_config, goal_config, epsilon=1.0):
self.tree = RRTTree(self.planning_env, start_config)
plan = []
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
# TODO: Here you will implement the rrt planner
# The return path should be an array
# of dimension k x n where k is the number of waypoints
# and n is the dimension of the robots configuration space
goal_tf = numpy.eye(4, dtype=float)
goal_tf[0, 3] = goal_config[0]
goal_tf[1, 3] = goal_config[1]
start_tf = numpy.eye(4, dtype=float)
start_tf[0, 3] = start_config[0]
start_tf[1, 3] = start_config[1]
# plan.append(start_config)
# plan.append(goal_config)
self.tree.vertices = []
# print("initial tree vertices is {} ".format(tree.vertices))
self.tree.AddVertex(start_tf)
# print("current tree vertices is {} ".format(tree.vertices))
count = 0
while (True):
count += 1
# Generate random point
if (count % 10 is not 0):
qr = self.planning_env.GenerateRandomConfiguration()
else:
qr = goal_tf
# Find the nearest point to the random point
# qi: nearest point
if (numpy.shape(qr) != (4, 4)):
qr = self.planning_env.GenerateRandomConfiguration()
qi_id, qi = self.tree.GetNearestVertex(qr)
else:
qi_id, qi = self.tree.GetNearestVertex(qr)
# Extend the new point from the nearest point toward the random point for one step distance
qc = self.planning_env.Extend(qi, qr)
qc_id = self.tree.AddVertex(qc)
self.tree.AddEdge(qi_id, qc_id)
# when reach the goal, break the while loop
if (self.planning_env.ComputeDistance(qc, goal_tf) < epsilon):
print("Reach the goal is {} ".format(qc_id))
break
# use DFS to find the path from the start_config to the goal_config
# print("Print all paths is {} ".format(self.tree.vertices))
visited = [False] * len(self.tree.vertices)
# self.path = self.printAllPaths(1,qc_id)
path = []
level = 0
self.printAllPathsUtil(0, qc_id, visited, path, level)
print("Not self path is {}".format(self.path))
idx = 0
while (self.path[idx] is not qc_id):
self.result.append(self.path[idx])
idx += 1
self.result.append(qc_id)
print("The self result is {}".format(self.result))
for i in self.result:
v = self.tree.vertices[i]
plan.append((v[0, 3], v[1, 3]))
print("Final plan is {}".format(plan))
print("Goal Config is {} ".format(goal_config))
return plan
# def printAllPaths(self,s,d):
# # Mark all the vertices as not visited
# visited = [False] * len(self.tree.vertices)
# self.path = []
# self.path = self.printAllPathsUtil(s,d,visited,self.path)
# print("Final path is {}".format(self.path))
# return
def printAllPathsUtil(self, u, d, visited, path, level):
level += 1
visited[u] = True
path.append(u)
if u == d:
print("---------------------------------------------")
print("This is path {}".format(path))
# self.result.append(path)
self.path = path
else:
for i in self.tree.edges[u]:
if visited[i] == False:
print("level %d, val %d " % (level, i))
self.printAllPathsUtil(i, d, visited, path, level)
def Plan(self, start_config, goal_config):
self.planning_env.InitializePlot(goal_config)
start_time = time.time()
plan = []
# TODO: Here you will implement the breadth first planner
# The return path should be a numpy array
# of dimension k x n where k is the number of waypoints
# and n is the dimension of the robots configuration space
plan.append(start_config)
plan.append(goal_config)
start_id = self.planning_env.discrete_env.ConfigurationToNodeId(
start_config)
goal_id = self.planning_env.discrete_env.ConfigurationToNodeId(
goal_config)
seen = [start_id]
queue = Queue()
queue.put(start_id)
tree = RRTTree(self.planning_env, start_config)
nodeIdToTreeIdDict = {}
nodeIdToTreeIdDict[start_id] = 0
while not queue.empty():
curr_id = queue.get()
curr_tree_id = nodeIdToTreeIdDict[curr_id]
for node_id in self.planning_env.GetSuccessors(curr_id):
if not node_id in seen:
seen.append(node_id)
node_tree_id = tree.AddVertex(
self.planning_env.discrete_env.NodeIdToConfiguration(
node_id))
nodeIdToTreeIdDict[node_id] = node_tree_id
tree.AddEdge(curr_tree_id, node_tree_id)
#self.planning_env.PlotEdge(self.planning_env.discrete_env.NodeIdToConfiguration(curr_id),self.planning_env.discrete_env.NodeIdToConfiguration(node_id) );
queue.put(node_id)
if node_id == goal_id:
while True:
old_config = tree.vertices[node_tree_id]
node_tree_id = tree.edges[node_tree_id]
node_config = tree.vertices[node_tree_id]
self.planning_env.PlotEdge(old_config, node_config)
if (node_tree_id == tree.GetRootId()):
plan_length = self.Plan_Length(plan)
print("--- %s seconds ---" %
(time.time() - start_time))
print("--- %s plan length ---" % plan_length)
print("--- %s vertices ---" %
len(tree.vertices))
return plan
else:
plan.insert(1, node_config)
return plan
class RRTPlanner(object):
def __init__(self, planning_env, visualize):
self.planning_env = planning_env
self.visualize = visualize
self.tree = []
self.path = []
self.result = []
self.arrived = False
def Plan(self, start_config, goal_config, epsilon = 2.0):
self.tree = RRTTree(self.planning_env, start_config)
plan = []
start_time = time.time()
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
# TODO: Here you will implement the rrt planner
# The return path should be an array
# of dimension k x n where k is the number of waypoints
# and n is the dimension of the robots configuration space
# simple:
# goal_tf = numpy.eye(4,dtype = float)
# goal_tf[0,3] = goal_config[0]
# goal_tf[1,3] = goal_config[1]
#
# start_tf = numpy.eye(4,dtype = float)
# start_tf[0,3] = start_config[0]
# start_tf[1,3] = start_config[1]
self.tree.vertices = []
# simple:
# self.tree.AddVertex(start_tf)
self.tree.AddVertex(start_config)
print("tree vertex:{}".format(start_config))
count = 0
while (True):
count += 1
# Generate random point
if(count % 10 is not 0):
qr = self.planning_env.GenerateRandomConfiguration()
else:
# simple:
# qr = goal_tf
qr = goal_config
# Find the nearest point to the random point
# qi: nearest point
#simple
# if(numpy.shape(qr) != (4, 4)):
if (numpy.shape(qr) != (1, 7)):
qr = self.planning_env.GenerateRandomConfiguration()
qi_id, qi = self.tree.GetNearestVertex(qr)
else:
qi_id, qi = self.tree.GetNearestVertex(qr)
qc = self.planning_env.Extend(qi,qr)
qc_id = self.tree.AddVertex(qc)
self.tree.AddEdge(qi_id, qc_id)
# when reach the goal, break the while loop
# if(self.planning_env.ComputeDistance(qc, goal_tf) < epsilon):
print("compute distance:{}".format(self.planning_env.ComputeDistance(qc, goal_config)))
if(self.planning_env.ComputeDistance(qc, goal_config) < epsilon):
print("Reach the goal is {} ".format(qc_id))
break
path = []
level = 0
visited = [False] * len(self.tree.vertices)
# use DFS to find the path from the start_config to the goal_config
# print("Print all paths is {} ".format(self.tree.vertices))
self.printAllPathsUtil(0, qc_id,visited,path,level)
# Hacky way to truncate the extra path return by the DFS
idx = 0
while(self.path[idx] is not qc_id):
self.result.append(self.path[idx])
idx += 1
self.result.append(qc_id)
# Look up the vertices given the vertices' id
for i in self.result:
v = self.tree.vertices[i]
#simple:
# plan.append((v[0,3],v[1,3]))
plan.append(v)
step_dist = self.planning_env.step
all_step_dist = numpy.sum(len(plan) * step_dist)
elapsed_time = time.time() - start_time
vertices = len(self.tree.vertices)
print("RRT: All step dist: {}, elapsed time is {} and vertices number {} ".format(all_step_dist,
elapsed_time,
vertices))
# return N*Vertice_dimension ndarray path
return plan
def printAllPathsUtil(self, u, d, visited, path,level):
level += 1
visited[u] = True
path.append(u)
if u == d:
print("---------------------------------------------")
print("This is path {}".format(path))
# self.result.append(path)
self.path = path
else:
for i in self.tree.edges[u]:
if visited[i] == False:
print("level %d, val %d "%(level,i))
self.printAllPathsUtil(i,d,visited,path,level)
def Plan(self, start_config, goal_config, epsilon = 0.001):
tree = RRTTree(self.planning_env, start_config)
plan = []
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
# TODO: Here you will implement the rrt planner
# The return path should be an array
# of dimension k x n where k is the number of waypoints
# and n is the dimension of the robots configuration space
p_min = 0.1
C_max = 0
while(1):
#import IPython
#IPython.embed()
#drop one valid random point that only connect with start tree
p = 0
while numpy.random.uniform(0,1) > p:
point_drop = self.planning_env.GenerateRandomConfiguration()
index, near_point = tree.GetNearestVertex(point_drop)
# print near_point
curr_path = self.find_path(tree,0,index)
curr_cost = self.planning_env.ComputePathLength(curr_path)
curr_h = self.planning_env.ComputeHeuristicC(near_point,goal_config)
C_q = curr_cost + 5*curr_h
C_max = max(C_q, C_max)
C_opt = self.planning_env.ComputeDistanceC(start_config, goal_config)
# print C_max, C_opt
try:
mq = 1 - float(C_q - C_opt)/float(C_max - C_opt)
except:
mq = 0
p = max(mq, p_min)
point_chosen_from_s = self.planning_env.Extend(tree.vertices[index],point_drop)
point_chosen_from_g = self.planning_env.Extend(goal_config,point_drop)
#Check whether the point_drop can be directly used for connecting both
if(point_chosen_from_s == None or point_chosen_from_g == None): continue
dist_s = self.planning_env.ComputeDistanceC(point_chosen_from_s, point_drop)
dist_g = self.planning_env.ComputeDistanceC(point_chosen_from_g,point_drop)
if (dist_s <= epsilon and dist_g <= epsilon):
point_addon_s_final = tree.AddVertex(point_chosen_from_s)
tree.AddEdge(index,point_addon_s_final)
point_addon_g_final = tree.AddVertex(point_chosen_from_g)
tree.AddEdge(point_addon_s_final,point_addon_g_final)
#self.planning_env.PlotEdge(near_point,point_chosen_from_s)
#self.planning_env.PlotEdge(point_chosen_from_g,goal_config)
break
#If point_drop can only connect to start tree but not connect to goal point
elif (dist_s <= epsilon and dist_g > epsilon):
point_addon_s = tree.AddVertex(point_chosen_from_s)
tree.AddEdge(index,point_addon_s)
#self.planning_env.PlotEdge(near_point,point_chosen_from_s)
#If point_drop can connect either the start tree or the goal point
elif (dist_s > epsilon and dist_g > epsilon):
point_addon_s = tree.AddVertex(point_chosen_from_s)
tree.AddEdge(index,point_addon_s)
#self.planning_env.PlotEdge(near_point,point_chosen_from_s)
# total_time = time.time() - start_time;
# Find the path
path_start_tree = self.find_path(tree, 0, point_addon_g_final)
path_start_tree.reverse()
for i in path_start_tree:
plan.append(i)
plan.append(goal_config)
dist_plan = self.planning_env.ComputePathLength(plan)
print "total plan distance"
print dist_plan
print "total vertices in tree "
print len(tree.vertices)
# end of implement
# print "total plan time = "
# print total_time
# print " "
return plan
class RRTPlannerNonholonomic(object):
def __init__(self,
planning_env,
bias=0.05,
max_iter=10000,
num_control_samples=25):
self.env = planning_env # Car Environment
self.tree = RRTTree(self.env)
self.bias = bias # Goal Bias
self.max_iter = max_iter # Max Iterations
self.num_control_samples = 25 # Number of controls to sample
def Plan(self, start_config, goal_config):
# TODO: YOUR IMPLEMENTATION HERE
start_config = start_config.ravel()
goal_config = goal_config.ravel()
plan_time = time.time()
plan = []
# Start with adding the start configuration to the tree.
start_config = start_config.reshape((3, 1))
goal_config = goal_config.reshape((3, 1))
self.tree.AddVertex(start_config)
for i in range(self.max_iter):
if (i % 1000 == 0):
print(i)
r = self.sample(goal_config).ravel()
r = r.reshape((3, 1))
vid, vertex = self.tree.GetNearestVertex(r.reshape((3, 1)))
#vertex= vertex.reshape((3,1))
if (r.ravel() == vertex).all():
continue
new, exec_time = self.extend(vertex, r)
if (exec_time is None):
continue
# if (not self.env.edge_validity_checker(vertex.reshape((3,1)),new.reshape((3,1)))):
# continue
#print('exec_time',exec_time)
new_cost = self.tree.costs[vid] + exec_time
nid = self.tree.AddVertex(new, new_cost)
self.tree.AddEdge(vid, nid)
#print('new', new,'goal', goal_config)
if (self.env.goal_criterion(new, goal_config)):
print('goal_reached')
idx = nid
while (idx != 0):
plan.append(self.tree.vertices[idx])
idx = self.tree.edges[idx]
plan.append(self.tree.vertices[idx])
break
plan = np.asarray(plan)
plan = plan[::-1]
cost = new_cost
plan_time = time.time() - plan_time
print("Cost: %f" % cost)
print("Planning Time: %ds" % plan_time)
plan = plan.T
plan = plan.reshape((3, -1))
return plan
def extend(self, x_near, x_rand):
""" Extend method for non-holonomic RRT
Generate n control samples, with n = self.num_control_samples
Simulate trajectories with these control samples
Compute the closest closest trajectory and return the resulting state (and cost)
"""
# TODO: YOUR IMPLEMENTATION HERE
min_dist = float('inf')
min_cost = None
for i in range(self.num_control_samples):
vel, angle = self.env.sample_action()
point, dt = self.env.simulate_car(x_near, x_rand, vel, angle)
if point is not None and self.env.compute_distance(
point, x_rand) < min_dist:
x_near = point
min_cost = dt
x_near = x_near.reshape((3, 1))
for i in range(3):
x_near[i] = round(x_near[i][0], 2)
return x_near, min_cost
def sample(self, goal):
# Sample random point from map
if np.random.uniform() < self.bias:
return goal
return self.env.sample()
def compute_distance(self, start_config, end_config):
th1 = start_config[2, :]
th2 = end_config[2, :]
ang_diff = np.abs(th1 - th2) * 180 / np.pi
dsit = np.sqrt((start_config[0, :] - end_config[0, :])**2 +
(start_config[1, :] - end_config[1, :])**2 +
ang_diff**2)
return dsit
class RRTPlanner(object):
def __init__(self, planning_env, bias=0.05, eta=1.0, max_iter=10000):
self.env = planning_env # Map Environment
self.tree = RRTTree(self.env)
self.bias = bias # Goal Bias
self.max_iter = max_iter # Max Iterations
self.eta = eta # Distance to extend
def Plan(self, start_config, goal_config):
# TODO: YOUR IMPLEMENTATION HERE
start_config = start_config.ravel()
goal_config = goal_config.ravel()
plan_time = time.time()
plan = []
# Start with adding the start configuration to the tree.
self.tree.AddVertex(start_config)
for i in range(self.max_iter):
r = self.sample(goal_config)
vid, vertex = self.tree.GetNearestVertex(r)
if (r.ravel() == vertex).all():
continue
new = self.extend(vertex, r)
if (not self.env.edge_validity_checker(vertex.reshape(
(2, 1)), new.reshape((2, 1)))):
continue
#print(news)
new_cost = self.tree.costs[vid] + self.env.compute_distance(
vertex, new)
nid = self.tree.AddVertex(new, new_cost)
self.tree.AddEdge(vid, nid)
if (new == goal_config.ravel()).all():
print('goal_reached')
idx = nid
while (idx != 0):
plan.append(self.tree.vertices[idx])
idx = self.tree.edges[idx]
plan.append(self.tree.vertices[idx])
break
plan = np.asarray(plan)
plan = plan[::-1]
cost = new_cost
plan_time = time.time() - plan_time
print("Cost: %f" % cost)
print("Planning Time: %ds" % plan_time)
return plan.T
def extend(self, x_near, x_rand):
# TODO: YOUR IMPLEMENTATION HERE
start = x_near.ravel()
end = x_rand.ravel()
v = end - start
u = v / (np.sqrt(np.sum(v**2)))
d = self.env.compute_distance(start, end) * self.eta
point = start + u * d
for i in range(2):
point[i] = round(point[i], 2)
return point
def sample(self, goal):
# Sample random point from map
if np.random.uniform() < self.bias:
return goal
return self.env.sample()
class BITStarPlanner(object):
'''
Object initializer function
'''
def __init__(self, planning_env, visualize):
self.planning_env = planning_env
self.visualize = visualize
self.vertex_queue = [] # self.vertex_queue = node_id
self.edge_queue = [] # self.edge_queue = (sid, eid)
self.samples = dict() # self.edge_queue[node_id] = config
self.g_scores = dict() # self.g_scores[node_id] = g_score
self.f_scores = dict() # self.f_scores[node_id] = f_score
self.r = float("inf") # radius
self.v_old = [] # old vertices
self.nodes = dict()
'''
Main Implementation for getting a plan
'''
def Plan(self, start_config, goal_config, epsilon=0.001):
# Initialize plan
plan = []
self.start_config = start_config
self.goal_config = goal_config
self.start_id = self.planning_env.discrete_env.ConfigurationToNodeId(
start_config)
self.goal_id = self.planning_env.discrete_env.ConfigurationToNodeId(
goal_config)
self.tree = RRTTree(self.planning_env, start_config) # initialize tree
# Initialize plot
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
# Add the goal to the samples
self.samples[self.goal_id] = self.goal_config
self.g_scores[self.goal_id] = float("inf")
self.f_scores[self.goal_id] = 0
# Add the start id to the tree
self.tree.AddVertex(self.start_config)
self.g_scores[self.start_id] = 0
self.f_scores[self.start_id] = self.planning_env.ComputeHeuristicCost(
self.start_id, self.goal_id)
# Specifies the number of iterations
iterations = 0
max_iter = 100
print "Start ID: ", self.start_id
print "Goal ID: ", self.goal_id
self.samples.update(self.Sample(m=200))
self.r = 1.0
# run until done
found_goal = False
while (iterations < max_iter):
# Add the start of a new batch
if len(self.vertex_queue) == 0 and len(self.edge_queue) == 0:
print "Batch: ", iterations
# Prune the tree
#self.Prune(self.g_scores[self.goal_id])
if iterations != 0:
self.samples.update(
self.Sample(m=50, c_max=self.g_scores[self.goal_id]))
#self.samples[self.goal_id] = self.goal_config
self.r = 2.0
# Make the old vertices the new vertices
self.v_old += self.tree.vertices.keys()
# Add the vertices to the vertex queue
for node_id in self.tree.vertices.keys():
if node_id not in self.vertex_queue:
self.vertex_queue.append(node_id)
# Expand the best vertices until an edge is better than the vertex
while (self.BestVertexQueueValue() <= self.BestEdgeQueueValue()):
self.ExpandVertex(self.BestInVertexQueue())
# Add the best edge to the tree
best_edge = self.BestInEdgeQueue()
self.edge_queue.remove(best_edge)
# See if it can improve the solution
estimated_cost_of_vertex = self.g_scores[
best_edge[0]] + self.planning_env.ComputeDistance(
best_edge[0],
best_edge[1]) + self.planning_env.ComputeHeuristicCost(
best_edge[1], self.goal_id)
estimated_cost_of_edge = self.planning_env.ComputeDistance(
self.start_id,
best_edge[0]) + self.planning_env.ComputeDistance(
best_edge[0],
best_edge[1]) + self.planning_env.ComputeHeuristicCost(
best_edge[1], self.goal_id)
actual_cost_of_edge = self.g_scores[
best_edge[0]] + self.planning_env.ComputeDistance(
best_edge[0], best_edge[1])
if (estimated_cost_of_vertex < self.g_scores[self.goal_id]):
if (estimated_cost_of_edge < self.g_scores[self.goal_id]):
if (actual_cost_of_edge < self.g_scores[self.goal_id]):
# Connect
first_config = self.planning_env.discrete_env.NodeIdToConfiguration(
best_edge[0])
next_config = self.planning_env.discrete_env.NodeIdToConfiguration(
best_edge[1])
path = self.con(first_config, next_config)
last_edge = self.planning_env.discrete_env.ConfigurationToNodeId(
next_config)
if path == None or len(path) == 0: # no path
continue
next_config = path[len(path) - 1, :]
last_config_in_path_id = self.planning_env.discrete_env.ConfigurationToNodeId(
next_config)
best_edge = (best_edge[0], last_config_in_path_id)
if(best_edge[1] in self.tree.vertices.keys()):
'''
for vertex in self.tree.vertices[best_edge[1]]:
self.tree.vertices[vertex].remove(best_edge[1])
if (vertex, best_edge[1]) in self.tree.edges:
self.tree.edges.remove((vertex, best_edge[1]))
if (best_edge[1], vertex) in self.tree.edges:
self.tree.edges.remove((best_edge[1], vertex))
del self.tree.vertices[best_edge[1]][:]
self.tree.edges.remove()
self.UpdateGraph()
'''
else:
try:
del self.samples[best_edge[1]]
except (KeyError):
pass
eid = self.tree.AddVertex(next_config)
self.vertex_queue.append(eid)
if eid == self.goal_id or best_edge[
0] == self.goal_id or best_edge[
1] == self.goal_id:
print "Found goal!"
found_goal = True
#if eid not in self.tree.vertices[best_edge[0]] or best_edge[0] not in self.tree.vertices[eid]:
self.tree.AddEdge(best_edge[0], best_edge[1])
g_score = self.planning_env.ComputeDistance(
best_edge[0], best_edge[1])
self.g_scores[best_edge[1]] = g_score + self.g_scores[
best_edge[0]]
self.f_scores[best_edge[
1]] = g_score + self.planning_env.ComputeHeuristicCost(
best_edge[1], self.goal_id)
self.UpdateGraph()
if self.visualize:
self.planning_env.PlotEdge(first_config,
next_config)
for edge in self.edge_queue:
if edge[0] == best_edge[1]:
if self.g_scores[edge[
0]] + self.planning_env.ComputeDistance(
edge[0], best_edge[1]
) >= self.g_scores[self.goal_id]:
if (edge[0],
best_edge[1]) in self.edge_queue:
self.edge_queue.remove(
(edge[0], best_edge[1]))
if (edge[1] == best_edge[1]):
if (self.g_scores[edge[1]] +
self.planning_env.ComputeDistance(
edge[1], best_edge[1]) >=
self.g_scores[self.goal_id]):
if (last_edge,
best_edge[1]) in self.edge_queue:
self.edge_queue.remove(
(last_edge, best_edge[1]))
else:
print "Nothing good"
self.edge_queue = []
self.vertex_queue = []
iterations += 1
print "Iteration: ", iterations
print "Find the plan"
# Return a plan
plan.append(self.goal_config)
curr_id = self.goal_id
while (curr_id != self.start_id):
print "Current ID: ", curr_id
#self.tree.vertices[curr_id].remove(next_id)
#curr_id = next_id
plan.append(
self.planning_env.discrete_env.NodeIdToConfiguration(curr_id))
curr_id = self.nodes[curr_id]
# Whenever the current id is the start id, append start id
plan.append(self.start_config)
plan = plan[::-1] # reverse
return numpy.array(plan), len(self.tree.vertices)
'''
Function to expand a vertex
'''
def ExpandVertex(self, vid):
# Remove vertex from vertex queue
self.vertex_queue.remove(vid)
# Get the current configure from the vertex
curr_config = numpy.array(
self.planning_env.discrete_env.NodeIdToConfiguration(vid))
# Get a nearest value in vertex for every one in samples where difference is less than the radius
possible_neighbors = [
] # possible sampled configs that are within radius
#print "Samples to expand: ", self.samples
for sample_id, sample_config in self.samples.iteritems():
sample_config = numpy.array(sample_config)
if (numpy.linalg.norm(sample_config - curr_config, 2) <= self.r
and sample_id != vid):
possible_neighbors.append((sample_id, sample_config))
# Add an edge to the edge queue if the path might improve the solution
for neighbor in possible_neighbors:
sample_id = neighbor[0]
sample_config = neighbor[1]
estimated_f_score = self.planning_env.ComputeDistance(
self.start_id, vid) + self.planning_env.ComputeDistance(
vid, sample_id) + self.planning_env.ComputeHeuristicCost(
sample_id, self.goal_id)
if estimated_f_score < self.g_scores[self.goal_id]:
self.edge_queue.append((vid, sample_id))
# Add the vertex to the edge queue
if vid not in self.v_old:
possible_neighbors = []
for v, edges in self.tree.vertices.iteritems():
if v != vid and (v, vid) not in self.edge_queue and (
vid, v) not in self.edge_queue:
v_config = numpy.array(
self.planning_env.discrete_env.NodeIdToConfiguration(
v))
if (numpy.linalg.norm(v_config - curr_config, 2) <= self.r
and v != vid):
possible_neighbors.append((vid, v_config))
# Add an edge to the edge queue if the path might improve the solution
for neighbor in possible_neighbors:
sample_id = neighbor[0]
sample_config = neighbor[1]
estimated_f_score = self.planning_env.ComputeDistance(
self.start_id, vid) + self.planning_env.ComputeDistance(
vid,
sample_id) + self.planning_env.ComputeHeuristicCost(
sample_id, self.goal_id)
if estimated_f_score < self.g_scores[self.goal_id] and (
self.g_scores[vid] + self.planning_env.ComputeDistance(
vid, sample_id)) < self.g_scores[sample_id]:
self.edge_queue.append((vid, sample_id))
'''
Function to prune the tree
'''
def Prune(self, c):
print "Puning!"
# Remove samples whose estmated cost to goal is > c
self.samples = {
node_id: config
for node_id, config in self.samples.iteritems()
if self.planning_env.ComputeDistance(self.start_id, node_id) +
self.planning_env.ComputeHeuristicCost(node_id, self.goal_id) <= c
}
# Remove vertices whose estimated cost to goal is > c
vertices_to_delete = []
for vertex, edges in self.tree.vertices.iteritems():
if self.f_scores[vertex] > c or self.f_scores[vertex] == float(
"inf"):
# Delete the vertex and all of its edges
for edge in edges:
self.tree.vertices[edge].remove(vertex)
self.tree.vertices[vertex].remove(edge)
if (edge, vertex) in self.tree.edges:
self.tree.edges.remove((edge, vertex))
if (vertex, edge) in self.tree.edges:
self.tree.edges.remove((vertex, edge))
vertices_to_delete.append(vertex)
for vertex in vertices_to_delete:
del self.tree.vertices[vertex]
self.UpdateGraph()
# Remove edge if either vertex connected to its estimated cost to goal is > c
for nid in self.tree.edges:
if self.f_scores[nid[0]] > c or self.f_scores[nid[1]] > c:
if nid[1] in self.tree.vertices[nid[0]]:
self.tree.vertices[nid[0]].remove(nid[1])
if nid[0] in self.tree.vertices[nid[1]]:
self.tree.vertices[nid[1]].remove(nid[0])
self.tree.edges.remove((nid[0], nid[1]))
# Add vertices to samples if its g_score is infinity
'''
new_samples = {node_id:config for node_id, config in self.tree.vertices.iteritems() if self.g_scores[node_id] == float("inf")}
for node_id, config in new_samples:
if node_id not in self.samples.keys():
self.samples[node_id] = config
'''
self.ReturnDisconnected()
self.UpdateGraph()
'''
Function to extend between two configurations
'''
def ext(self, tree, random_config):
# Get the nearest configuration to this
sid, nearest_config = tree.GetNearestVertex(random_config)
# Get the interpolation between the two
path = self.planning_env.Extend(nearest_config, random_config)
# Return only the first two parts in the path
if (path == None):
return path, sid, nearest_config
else:
return path[0:2, :], sid, nearest_config
'''
Function to connnect two configurations
'''
def con(self, start_config, end_config):
# Return the whole path to the end
return self.planning_env.Extend(start_config, end_config)
'''
Function to get the new radius of the r-disk
'''
def radius(self, q):
eta = 2.0 # tuning parameter
dimension = len(
self.planning_env.lower_limits) + 0.0 # dimension of problem
space_measure = self.planning_env.space_measure # volume of the space
unit_ball_measure = self.planning_env.unit_ball_measure # volume of the dimension of the unit ball
min_radius = eta * 2.0 * pow(
(1.0 + 1.0 / dimension) *
(space_measure / unit_ball_measure), 1.0 / dimension)
return min_radius * pow(numpy.log(q) / q, 1 / dimension)
def GetNearestSample(self, config):
dists = dict()
for index in self.samples.keys():
if index == self.planning_env.discrete_env.ConfigurationToNodeId(
self.start_config):
dists[index] = 999
pass
dists[index] = self.planning_env.ComputeDistance(
self.planning_env.discrete_env.ConfigurationToNodeId(config),
index)
# vid, vdist = min(dists.items(), key=operator.itemgetter(0))
sample_id = min(dists, key=dists.get)
return sample_id, self.samples[sample_id]
def Sample(self, m, c_max=float("inf")):
new_samples = dict()
if c_max < float("inf"):
c_min = self.planning_env.ComputeDistance(self.start_config,
self.goal_config)
x_center = (self.start_config + self.goal_config) / 2
# Get a random sample form the unit ball
X_ball = self.SampleUnitNBall(m)
# scale the unit ball
scale = self.planning_env.GetEllipsoidScale(c_max, c_min)
points_scale = numpy.dot(X_ball, scale)
# Translate them to the center
points_trans = points_scale + x_center
# generate the dictionary
for point in points_trans:
node_id = self.planning_env.discrete_env.ConfigurationToNodeId(
numpy.array(point))
new_samples[node_id] = numpy.array(point)
else:
# Initially just uniformly sample
for i in xrange(0, m + 1):
random_config = self.planning_env.GenerateRandomConfiguration()
random_id = self.planning_env.discrete_env.ConfigurationToNodeId(
random_config)
new_samples[random_id] = random_config
return new_samples
def SampleUnitNBall(self, m):
points = numpy.random.uniform(-1, 1,
[m * 2, self.planning_env.dimension])
points = list(points)
points = [point for point in points if numpy.linalg.norm(point, 2) < 1]
points = numpy.array(points)
points = list(points)
points = [
point for point in points if self.planning_env.ValidConfig(point)
]
points = numpy.array(points)
print "Shape of points: ", numpy.shape(points)
return points[0:m, :]
def BestVertexQueueValue(self):
# Return the best value in the Queue by score g_tau[v] + h[v]
if (len(self.vertex_queue) == 0): # Edge Case
return float("inf")
#print "Vertex Queue before: ", self.vertex_queue
values = [
self.g_scores[v] +
self.planning_env.ComputeHeuristicCost(v, self.goal_id)
for v in self.vertex_queue
]
values.sort()
return values[0]
def BestEdgeQueueValue(self):
if (len(self.edge_queue) == 0): # Edge case
return float("inf")
# Return the best value in the queue by score g_tau[v] + c(v,x) + h(x)
values = [
self.g_scores[e[0]] +
self.planning_env.ComputeDistance(e[0], e[1]) +
self.planning_env.ComputeHeuristicCost(e[1], self.goal_id)
for e in self.edge_queue
]
values.sort(reverse=True)
return values[0]
def BestInEdgeQueue(self):
# Return the best value in the edge queue
e_and_values = [
(e[0], e[1], self.g_scores[e[0]] +
self.planning_env.ComputeDistance(e[0], e[1]) +
self.planning_env.ComputeHeuristicCost(e[1], self.goal_id))
for e in self.edge_queue
]
e_and_values = sorted(e_and_values, key=lambda x: x[2])
return (e_and_values[0][0], e_and_values[0][1])
def BestInVertexQueue(self):
# Return the besst value in the vertex queue
v_and_values = [
(v, self.g_scores[v] +
self.planning_env.ComputeHeuristicCost(v, self.goal_id))
for v in self.vertex_queue
]
v_and_values = sorted(v_and_values, key=lambda x: x[1])
return v_and_values[0][0]
def UpdateGraph(self):
# Initialize lists
closed_set = []
open_set = []
g_scores = dict()
f_scores = dict()
current_id = self.start_id
open_set.append(self.start_id)
# initialize flags and counters
found_goal = False
while len(open_set) != 0:
# Get the element with the lowest f_score
curr_id = min(open_set, key=lambda x: self.f_scores[x])
# Remove element from open set
open_set.remove(curr_id)
# Check to see if you are at goal
if (curr_id == self.goal_id):
#print "Found goal"
self.nodes[self.goal_id]
found_goal = True
break
# Add node to closed set
if (curr_id not in closed_set):
closed_set.append(curr_id)
# Find a non-visited successor to the current_id
successors = self.tree.vertices[curr_id]
for successor in successors:
if (successor in closed_set):
continue
else:
# Calculate the tentative g score
successor_config = self.planning_env.discrete_env.NodeIdToConfiguration(
successor)
g_score = self.g_scores[
curr_id] + self.planning_env.ComputeDistance(
curr_id, successor)
if successor not in open_set:
# Add to open set
open_set.append(successor)
elif g_score >= self.g_scores[successor]:
continue
# Update g and f scores
self.g_scores[successor] = g_score
self.f_scores[
successor] = g_score + self.planning_env.ComputeHeuristicCost(
successor, self.goal_id)
# Store the parent and child
self.nodes[successor] = curr_id
def UpdateGraphPrint(self):
# Initialize lists
closed_set = []
open_set = []
current_id = selfstart_id
open_set.append(self.start_id)
# Initialize plot
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(self.goal_config)
# initialize flags and counters
found_goal = False
while len(open_set) != 0:
# Get the element with the lowest f_score
minn = float("inf")
min_node = None
min_idx = 0
for i in xrange(0, len(open_set)):
try:
f_score = self.f_scores[open_set[i]]
except (KeyError):
pass
if f_score < minn:
minn = f_score
min_node = open_set[i]
min_idx = i
curr_id = min_node
# Remove element from open set
open_set.pop(min_idx)
# Check to see if you are at goal
if (curr_id == self.goal_id):
found_goal = True
break
# Add node to closed set
if (curr_id not in closed_set):
closed_set.append(curr_id)
# Find a non-visited successor to the current_id
successors = self.tree.vertices[curr_id]
for successor in successors:
if (successor in closed_set):
continue
else:
# Calculate the tentative g score
successor_config = self.planning_env.discrete_env.NodeIdToConfiguration(
successor)
g_score = self.g_scores[
curr_id] + self.planning_env.ComputeDistance(
curr_id, successor)
if successor not in open_set:
# Add to open set
open_set.append(successor)
elif g_score >= self.g_scores[successor]:
continue
# Update g and f scores
self.g_scores[successor] = g_score
self.f_scores[
successor] = g_score + self.planning_env.ComputeHeuristicCost(
successor, self.goal_id)
# Store the parent and child
self.nodes[successor] = curr_id
if self.visualize: # Plot the edge
pred_config = self.planning_env.discrete_env.NodeIdToConfiguration(
curr_id)
succ_config = self.planning_env.discrete_env.NodeIdToConfiguration(
successor)
self.planning_env.PlotEdge(pred_config, succ_config)
def ReturnDisconnected(self):
# Open queue
queue = []
queue.append(self.start_id)
visited = [] # visited nodes
current_id = self.start_id
found_goal = False
while len(queue) != 0:
# Get the head of the queue
current_id = queue.pop(0)
successors = self.tree.vertices[current_id]
# Find a non-visited successor to the current_id
for successor in successors:
if (successor not in visited):
visited += [successor]
queue += [successor]
for vertex, edges in self.tree.vertices.iteritems():
if vertex not in visited:
for edge in edges:
self.tree.vertices[edge].remove(vertex)
if (edge, vertex) in self.tree.edges:
self.tree.edges.remove((edge, vertex))
if (vertex, edge) in self.tree.edges:
self.tree.edges.remove((edge, vertex))
del self.tree.vertices[vertex]
self.samples[
vertex] = self.planning_env.discrete_env.NodeIdToConfiguration(
vertex)
def Plan(self, start_config, goal_config, epsilon=0.001):
# count for executing timing
import time
t0 = time.clock()
tree = RRTTree(self.planning_env, start_config)
plan = []
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
# TODO: Here you will implement the rrt planner
# The return path should be an array
# of dimension k x n where k is the number of waypoints
# and n is the dimension of the robots configuration space
# start the Forward RRT algorithm
# set up the number of iterations we want to try before we give up
num_iter = 1000
# set up the probability to generate goal config
prob_goal = 0.2
# if it suceeeds to connect the goal, make isFail = False
isFail = True
import random
import time
num_vertex = 0
for i in xrange(num_iter):
# generate random configuration, config_q
prob = random.random()
if prob < prob_goal:
config_q = numpy.copy(goal_config)
else:
config_q = self.planning_env.GenerateRandomConfiguration()
# get the nearest neighbor from the Tree, based on config_q
mid, mdist = tree.GetNearestVertex(config_q)
# get the nearest neighbor, config_m
config_m = tree.vertices[mid]
# append config_n to the tree if config_n is addable
# where config_n is the extended node between config_m and config_q
# in our case, if it is extendable, config_n always equals to config_q
config_n = self.planning_env.Extend(config_m, config_q)
if numpy.array_equal(config_n, config_m):
continue
else:
num_vertex = num_vertex + 1
tree.AddVertex(config_n)
tree.AddEdge(mid,
len(tree.vertices) -
1) # id of config_n is the last one in vertices
# draw the expended edge
if len(config_m) == 2:
self.planning_env.PlotEdge(config_m, config_n)
# check if config_n equals to goal_config, if yes, break
if numpy.array_equal(config_n, goal_config):
isFail = False
goal_id = len(tree.vertices) - 1
break
# recursive append the waypoints to the tree based on the tree.edges information
# stop if we trace back to root, id = 0
# start here
print "number of vertices: %d" % (num_vertex)
if isFail:
print "path length: 0"
print "plan time: %f" % (time.clock() - t0)
return []
else:
current_id = goal_id
while current_id != 0:
plan.append(tree.vertices[current_id])
current_id = tree.edges[current_id]
plan.append(
tree.vertices[current_id]) # add start config to the plan
# reverse the order of plan
plan = plan[::-1]
path_length = 0
for i in xrange(len(plan) - 1):
path_length = path_length + self.planning_env.ComputeDistance(
plan[i], plan[i + 1])
print "path length: %f" % (path_length)
print "plan time: %f" % (time.clock() - t0)
return plan
class RRTPlanner(object):
def __init__(self, planning_env, visualize):
self.planning_env = planning_env
self.visualize = visualize
def ChooseTarget(self, goal_config):
goal_config = numpy.copy(goal_config)
p = numpy.random.uniform(0.0, 1.0)
goal_p = 0.5
config = self.planning_env.GenerateRandomConfiguration()
#print "P: " + str(p)
#print "G: " + str(goal_p)
# Randomly try goal
if p < goal_p:
#print "HERE"
return goal_config
else:
return config
def Plan(self, start_config, goal_config, epsilon=0.1):
start_config = numpy.copy(start_config)
goal_config = numpy.copy(goal_config)
self.tree = RRTTree(self.planning_env, start_config)
plan = []
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
# TODO: Here you will implement the rrt planner
# The return path should be an array
# of dimension k x n where k is the number of waypoints
# and n is the dimension of the robots configuration space
#plan.append(start_config)
#plan.append(goal_config)
self.tree.AddEdge(self.tree.GetRootId, self.tree.GetRootId)
q_nearest = start_config
while (self.planning_env.ComputeConfigDistance(q_nearest, goal_config)
> epsilon):
# Get Random Point to add to tree
q_target = self.ChooseTarget(goal_config)
#Find nearest neighbor to new point
vid, q_nearest = self.tree.GetNearestVertex(q_target)
#pdb.set_trace()
# Join goal to target if possible
q_extended = self.planning_env.Extend(q_nearest, q_target)
#q_connecting = self.planning_env.Extend(goal_config,q_target)
if q_extended is not None:
#print "QX: " + str(q_extended[0]) + " QY: " + str(q_extended[1])
if numpy.array_equal(q_nearest, q_extended) == False:
self.tree.AddVertex(q_extended)
self.tree.AddEdge(vid, len(self.tree.vertices) - 1)
#self.planning_env.PlotEdge(q_nearest,q_extended)
if numpy.array_equal(q_extended, goal_config):
goal_index = len(self.tree.vertices) - 1
break
current_index = goal_index
while current_index != 0:
plan.append(self.tree.vertices[current_index])
current_index = self.tree.edges[current_index]
plan.append(self.tree.vertices[current_index])
plan = plan[::-1]
#pdb.set_trace()
print "Tree Size: " + str(len(self.tree.vertices))
return plan
def Plan(self, start_config, goal_config, epsilon=0.01):
print('start arm plan')
tree = RRTTree(self.planning_env, start_config)
plan = []
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
# TODO: Here you will implement the rrt planner
# The return path should be an array
# of dimension k x n where k is the number of waypoints
# and n is the dimension of the robots configuration space
start_time = time.time()
self.cost[0] = 0
opt_cost = self.planning_env.ComputeDistance_continue(
start_config, goal_config)
#prevent divid by 0
max_cost = opt_cost + 0.3
pr = 0.01
mdist = 1
self.planning_env.SetGoalParameters(goal_config, pr)
while (1):
#import IPython
#IPython.embed()
#drop one valid random point that only connect with start tree
point_drop = self.planning_env.GenerateRandomConfiguration()
nearest_index, nearest_point = tree.GetNearestVertex(point_drop)
point_drop = gp.replace(nearest_point, point_drop,
mdist * (len(tree.vertices) / 200 + 1))
point_chosen_from_s = self.planning_env.Extend(
tree.vertices[nearest_index], point_drop)
point_chosen_from_g = self.planning_env.Extend(
goal_config, point_drop)
#Check whether the point_drop can be directly used for connecting both
if (point_chosen_from_s == None or point_chosen_from_g == None):
continue
dist_s = self.planning_env.ComputeDistance_continue(
point_chosen_from_s, point_drop)
dist_g = self.planning_env.ComputeDistance_continue(
point_chosen_from_g, point_drop)
#print "dist_s {} dist_g {}".format(dist_s, dist_g)
#find the path from start to end
if (dist_s <= epsilon and dist_g <= epsilon):
point_addon_s_final = tree.AddVertex(point_chosen_from_s)
tree.AddEdge(nearest_index, point_addon_s_final)
goal_id = tree.AddVertex(goal_config)
tree.AddEdge(point_addon_s_final, goal_id)
break
#If point_drop can only connect to start tree or connect to goal point
else:
c_path = self.cost[
nearest_index] + self.planning_env.ComputeDistance_continue(
point_chosen_from_s, nearest_point)
# current path cost + heuristic distance
c_vertex = c_path + self.planning_env.ComputeDistance_continue(
point_chosen_from_s, goal_config)
#print('h_cost_in_arm',self.planning_env.ComputeDistance_continue(point_chosen_from_s, goal_config))
m_q = 1 - (c_vertex - opt_cost) / (max_cost - opt_cost)
p = max(m_q, self.pmin)
#print "prob: {} c_vertex: {} vertex number: {}".format(p, c_vertex, len(tree.vertices))
r = numpy.random.random_sample()
if (r < p):
point_addon_s = tree.AddVertex(point_chosen_from_s)
tree.AddEdge(nearest_index, point_addon_s)
self.cost[point_addon_s] = c_path
max_cost = max(max_cost, c_vertex)
if (self.visualize):
self.planning_env.PlotEdge(nearest_point,
point_chosen_from_s)
# Find the path
total_time = time.time() - start_time
path_start_tree = self.find_path(tree, 0, goal_id)
path_start_tree.reverse()
for i in path_start_tree:
plan.append(i)
plan.append(goal_config)
dist_plan = self.planning_env.ComputePathLength(plan)
print "total plan distance"
print dist_plan
print "total vertices in tree = "
print len(tree.vertices)
print "total plan time = "
print total_time
return plan
plan.append(start_config)
plan.append(goal_config)
return plan
class RRTPlannerNonholonomic(object):
def __init__(self,
planning_env,
bias=0.05,
max_iter=10000,
num_control_samples=25):
self.env = planning_env # Car Environment
self.tree = RRTTree(self.env)
self.bias = bias # Goal Bias
self.max_iter = max_iter # Max Iterations
self.num_control_samples = 25 # Number of controls to sample
def Plan(self, start_config, goal_config):
# TODO: YOUR IMPLEMENTATION HERE
plan_time = time.time()
# Start with adding the start configuration to the tree.
self.tree.AddVertex(start_config)
plan = []
plan.append(start_config)
for i in range(self.max_iter):
# print(i)
x_rand = self.sample(goal_config)
if not self.env.state_validity_checker(x_rand):
continue
x_near_id, x_near = self.tree.GetNearestVertex(x_rand)
x_new, delta_t, action_t = self.extend(x_near=x_near,
x_rand=x_rand)
if x_new is None:
continue
x_new_id = self.tree.AddVertex(x_new)
self.tree.AddEdge(x_near_id, x_new_id)
if self.env.goal_criterion(x_new, goal_config):
end_id = x_near_id
traj = [x_new]
while self.tree.edges[end_id] != self.tree.GetRootID():
traj.append(self.tree.vertices[end_id])
end_id = self.tree.edges[end_id]
plan += traj[::-1]
break
plan.append(goal_config)
cost = 0
for i in range(len(plan) - 1):
cost += self.env.compute_distance(plan[i], plan[i + 1])
plan_time = time.time() - plan_time
print("Cost: %f" % cost)
print("Planning Time: %ds" % plan_time)
return np.concatenate(plan, axis=1)
def extend(self, x_near, x_rand):
""" Extend method for non-holonomic RRT
Generate n control samples, with n = self.num_control_samples
Simulate trajectories with these control samples
Compute the closest closest trajectory and return the resulting state (and cost)
"""
# TODO: YOUR IMPLEMENTATION HERE
dist = self.env.compute_distance(x_near, x_rand)
for i in range(self.num_control_samples):
linear_vel, steer_angle = self.env.sample_action()
x_new, delta_t = self.env.simulate_car(x_near, x_rand, linear_vel,
steer_angle)
if x_new is None:
continue
d = self.env.compute_distance(x_new, x_rand)
if d < dist:
action_t = np.array([linear_vel, steer_angle])
return x_new, delta_t, action_t
return None, None, None
def sample(self, goal):
# Sample random point from map
if np.random.uniform() < self.bias:
return goal
return self.env.sample()
class RRTPlanner(object):
def __init__(self, planning_env, visualize, width, height, robot_radius):
self.planning_env = planning_env
self.visualize = visualize
self.width = width
self.height = height
self.robot_radius = robot_radius
self.num_extended_fail = 0
self.num_extended_pass = 0
def Plan(self, env_config, start_config, goal_config):
self.env_config = env_config
self.start_config = start_config
self.goal_config = goal_config
GoalProb = 0.2
epsilon = self.robot_radius / 2
start_coord = start_config[0]
goal_coord = goal_config[0]
if self.visualize:
self.InitializePlot(start_coord)
self.tree = RRTTree(start_coord)
GoalFound = False
timeout_limit = 5.0
start_time = time.time()
run_time = time.time() - start_time
while not (GoalFound) and run_time < timeout_limit:
random_coord = self.GenerateRandomNode(GoalProb, goal_coord)
nearest_node = self.tree.GetNearestNode(random_coord)
CoordInCollision = self.planning_env.CheckPathCollision(
env_config, nearest_node, random_coord)
# If random coordinate is reachable, extend to it, else towards it
if not (CoordInCollision):
new_coord = random_coord
else:
new_coord = self.ExtendTowardsCoord(nearest_node, random_coord)
InCollision = self.planning_env.CheckPathCollision(
env_config, nearest_node, new_coord)
if InCollision:
self.num_extended_fail += 1
continue
else:
self.num_extended_pass += 1
self.tree.AddEdge(nearest_node, new_coord)
self.PlotEdge(nearest_node, new_coord)
DistToGoal = self.ComputeDistance(goal_coord, new_coord)
if goal_coord == new_coord:
GoalFound = True
elif (DistToGoal < epsilon):
self.tree.AddEdge(new_coord, goal_coord)
GoalFound = True
run_time = time.time() - start_time
#RRT either found the goal or timed out
# construct_time = time.time() - start_time
#Check if RRT completed
if GoalFound == False:
if_fail = 1
path3D = []
len_path = 0
construct_time = 0
else:
find_path_time = time.time()
if_fail = 0
path2D = [goal_coord]
while path2D[-1] != start_coord:
Parent = self.tree.NodeParent[path2D[-1]]
previous = Parent[0]
path2D.append(previous)
path2D.reverse()
path3D, len_path = self.planning_env.Construct3DPath(
path2D, start_config, goal_config)
construct_time = time.time() - find_path_time
num_nodes = len(self.tree.Nodes2D)
Vertices = self.tree.Nodes2D
Edges = self.tree.NodeParent
NodeStats = [num_nodes, self.num_extended_pass, self.num_extended_fail]
return Vertices, Edges, path3D, construct_time, NodeStats, len_path, if_fail
def GenerateRandomNode(self, GoalProb, goal_coord):
RandProb = numpy.random.random_sample()
if RandProb < GoalProb:
return goal_coord
else:
new_coord = self.GetRandCoord()
return new_coord
def GetPointAtDistOnLine(self, start, end, dist):
start_x = start[0]
start_y = start[1]
end_x = end[0]
end_y = end[1]
vector = (end_x - start_x, end_y - start_y)
vectorMagnitude = math.sqrt(
numpy.square(end_x - start_x) + numpy.square(end_y - start_y))
unitVector = (vector[0] / vectorMagnitude, vector[1] / vectorMagnitude)
delta_x = unitVector[0] * dist
delta_y = unitVector[1] * dist
new_coord_x = round(start[0] + delta_x, 2)
new_coord_y = round(start[1] + delta_y, 2)
new_coord = (new_coord_x, new_coord_y)
return new_coord
def GetRandCoord(self):
rand_x = round(numpy.random.random_sample() * self.width, 2)
rand_y = round(numpy.random.random_sample() * self.height, 2)
return (rand_x, rand_y)
def ExtendTowardsCoord(self, nearest_node, random_coord):
start_x = nearest_node[0]
start_y = nearest_node[1]
end_x = random_coord[0]
end_y = random_coord[1]
vector = (end_x - start_x, end_y - start_y)
vectorMagnitude = math.sqrt(
numpy.square(end_x - start_x) + numpy.square(end_y - start_y))
unitVector = (vector[0] / vectorMagnitude, vector[1] / vectorMagnitude)
delta_x = unitVector[0] * self.robot_radius
delta_y = unitVector[1] * self.robot_radius
new_coord_x = round(nearest_node[0] + delta_x, 2)
new_coord_y = round(nearest_node[1] + delta_y, 2)
new_coord = (new_coord_x, new_coord_y)
return new_coord
def ComputeDistance(self, node1, node2):
dist = numpy.sqrt(
numpy.square(node1[0] - node2[0]) +
numpy.square(node1[1] - node2[1]))
return dist
def InitializePlot(self, goal_config):
self.fig = pl.figure()
border = self.robot_radius / 2
lower_limits = [0 - border, 0 - border]
upper_limits = [self.width + border, self.height + border]
pl.xlim([lower_limits[0], upper_limits[0]])
pl.ylim([lower_limits[1], upper_limits[1]])
pl.plot(goal_config[0], goal_config[1], 'gx')
pl.ion()
pl.show()
def PlotEdge(self, start, end):
pl.plot([start[0], end[0]], [start[1], end[1]], 'k.-', linewidth=2.5)
pl.draw()
def Plan(self, start_config, goal_config, epsilon=0.001):
# TODO: Here you will implement the rrt planner
# The return path should be an array
# of dimension k x n where k is the number of waypoints
# and n is the dimension of the robots configuration space
self.planning_env.hRRT_SetGoalParameters(goal_config)
pGoal = 0.2
minNodeQuality = 0.3
print "Staring Heuristic RRT Planner"
start_time = time.clock()
tree = RRTTree(self.planning_env, start_config)
plan = []
# if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
# self.planning_env.InitializePlot(goal_config)
# showPath = True
# else:
# showPath = False
goalFound = False
maxIterations = 1000
numNodes = 0
for i in range(maxIterations):
# print "\nIteration " + str(i)
goalSampler = numpy.random.random()
if goalSampler < pGoal:
new_config = goal_config
# print "Goal Sampled."
# nearest_vertex = tree.GetNearestVertex(new_config)
# print "Goal Sampled. " + "Nearest vertex: " + str(nearest_vertex[1])
else:
new_config = self.planning_env.hRRT_GenerateRandomConfiguration(
)
if numNodes < 10: # Since there are not too many members in tree, can't get meaningful k-NN
nearest_vertex = tree.GetNearestVertex(new_config)
nodeQuality = tree.getNodeQuality(nearest_vertex[0])
# print "New random sample: " + str(new_config) + "\tNearest vertex: " + str(nearest_vertex[1]) + "\tNode quality: " + str(nodeQuality)
nearest_config = nearest_vertex[1]
nearest_vertex_id = nearest_vertex[0]
extended_config = self.planning_env.hRRT_Extend(
nearest_config, new_config)
else: # Get k nearest neighbours. Acts as jailbreaker in a case where hRRT always gets
# a nearest neigbour for the goal which can not be extended towards the goal.
k_nearest_vertices = tree.GetkNearestVertices(new_config)
to_delete = []
for v in range(len(k_nearest_vertices)):
nodeQuality = tree.getNodeQuality(k_nearest_vertices[v][0])
# print "New random sample: " + str(new_config) + "\tNearest vertex: " + str(k_nearest_vertices[v][1]) + "\tNode quality: " + str(nodeQuality)
if nodeQuality < minNodeQuality:
nodeQuality = minNodeQuality
nodeRejector = numpy.random.random()
if nodeQuality < nodeRejector:
# print "Node Rejected"
to_delete.append(v)
if to_delete != []:
to_delete = to_delete[::-1]
if to_delete[-1] == 0 or len(
to_delete) > 2: # Gets rid of bad nodes.
continue
for v in to_delete:
del k_nearest_vertices[v]
if k_nearest_vertices == []:
continue
for v in k_nearest_vertices:
nearest_config = v[1]
nearest_vertex_id = v[0]
# print str(nearest_config) + str(nearest_vertex_id)
extended_config = self.planning_env.hRRT_Extend(
nearest_config, new_config)
if extended_config != []:
break
if extended_config == []:
# print "Path not extended."
continue
numNodes = numNodes + 1
# print "New node to be added: " + str(extended_config)
tree.AddEdge(nearest_vertex_id, tree.AddVertex(extended_config))
# if showPath:
# self.planning_env.PlotEdge(nearest_config, extended_config)
testGoal = (extended_config == goal_config).all()
if testGoal == True:
goalFound = True
break
if goalFound:
print "Goal found.\n\tNumber of vertices: %d\n\tTime Elapsed: %0.3f seconds" % (
numNodes, time.clock() - start_time)
x = len(tree.edges.items())
while x != 0:
plan.append(tree.vertices[x])
x = tree.edges[x]
plan.append(start_config)
plan = plan[::-1]
plan.append(goal_config)
path_length = tree.getPathLength()
print "\tPath Length: %0.3f" % (path_length)
else:
print "Goal not found.\n\tVertices Explored: %d\n\tTime Elapsed: %0.3f seconds" % (
numNodes, time.clock() - start_time)
# plan.append(start_config)
# plan.append(goal_config)
plan = []
return plan