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 __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=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 __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, 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 = .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
class RRTPlanner(object):
def __init__(self, planning_env):
self.planning_env = planning_env
self.tree = RRTTree(self.planning_env)
def Plan(self, start_config, goal_config, epsilon=0.001):
# Initialize an empty plan.
plan = []
# Start with adding the start configuration to the tree.
self.tree.AddVertex(start_config)
# TODO (student): Implement your planner here.
plan.append(start_config)
plan.append(goal_config)
return numpy.array(plan)
def extend(self):
# TODO (student): Implement an extend logic.
pass
def ShortenPath(self, path):
# TODO (student): Postprocessing of the plan.
return path
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)
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 = 0.001):
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)
temp_plan = []
temp_start_config = copy.deepcopy(start_config)
temp_plan.append(temp_start_config)
back_trace = []
back_trace.append(0)
while 1:
if self.planning_env.Extend(temp_start_config, goal_config) is not None:
break
end_config = self.planning_env.GenerateRandomConfiguration()
neighbor = []
if len(temp_plan) > 1:
for i in range(len(temp_plan)):
neighbor.append(self.planning_env.ComputeDistance(temp_plan[i], end_config))
z = neighbor.index(min(neighbor))
temp_start_config = copy.deepcopy(temp_plan[z])
#back_trace.append(z)
del neighbor
if self.planning_env.Extend(temp_start_config, end_config) is not None:
if len(temp_plan) > 1:
back_trace.append(z)
temp_plan.append(end_config)
temp_start_config = copy.deepcopy(end_config)
y = len(temp_plan)-1
while y != 0 :
plan.insert(1,temp_plan[y])
y = copy.deepcopy(back_trace[y-1])
dist = 0
for i in range(len(plan)-2):
dist = dist + self.planning_env.ComputeDistance(plan[i],plan[i+1])
print dist
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)
plan.append(goal_config)
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.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 = 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):
ftree = RRTTree(self.planning_env, start_config)
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
costToCome = dict()
heuristicCost = dict()
costToCome[ftree.GetRootId()] = 0
optcost = self.planning_env.ComputeDistance(start_config, goal_config)
worstcost = 0
heuristicCost[
ftree.GetRootId()] = costToCome[ftree.GetRootId()] + optcost
# max edge length + a lot of code clean up
maxDist = .5
it = 0
while True:
sample = self.planning_env.GenerateRandomConfiguration()
#vertices = ftree.GetNearestKVertices(sample, self.kvertices)
f_vid, vtx = ftree.GetNearestVertex(sample)
it += 1
# for (f_vid, vtx) in vertices:
quality = 1 - (heuristicCost[f_vid] - optcost) / (worstcost -
optcost)
quality = min(quality, self.probfloor)
r = random.random()
# print quality, r
if quality < r: continue
f_best = self.ExtendFromTree(vtx, sample, maxDist, epsilon)
if f_best is not None:
new_id = self.AddNode(ftree, f_vid, f_best)
costToCome[new_id] = costToCome[
f_vid] + self.planning_env.ComputeDistance(f_best, vtx)
heuristicCost[new_id] = costToCome[
new_id] + self.planning_env.ComputeDistance(
f_best, goal_config)
worstcost = max(heuristicCost[new_id], worstcost)
r_best = self.planning_env.Extend(goal_config, f_best)
if r_best is not None:
dist = self.planning_env.ComputeDistance(f_best, r_best)
if (dist < epsilon):
g_id = self.AddNode(ftree, new_id, goal_config)
return (self.GeneratePlan(ftree, g_id), it)
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, 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, 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()
def Plan(self, start_config, goal_config, epsilon=0.001):
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
start_time = time.time()
start = [round(q, 3) for q in start_config]
goal = [round(f, 3) for f in goal_config]
num_turns = 1 / epsilon
sumel = float("inf")
anytime = 0
tree = [0] * 3
while anytime < 3:
tree[anytime] = RRTTree(self.planning_env, start_config,
goal_config)
count = 0
prob = 10
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 <= sumel:
it = False
prob = prob - 1
else:
rand = goal
prob = 10
#print "reached here"
Nid, neighbour = tree[anytime].GetNearestVertex(rand)
new_guy = self.planning_env.extend(neighbour, rand)
#print "count",count
if self.planning_env.state_validity_checker(new_guy):
count = count - 1
continue
New_id = tree[anytime].AddVertex(new_guy)
#print "neighbour",neighbour,"new_guy",new_guy
tree[anytime].AddEdge(Nid, New_id)
#if self.planning_env.dimension==2:
self.planning_env.PlotEdge(neighbour, new_guy)
#print "new_guy",new_guy
#c= input("stop")
if new_guy == goal:
print "Goal Reached"
last = New_id
book = []
while last != 0:
book.append(tree[anytime].vertices[last])
last = tree[anytime].edges[last]
book.append(tree[anytime].vertices[last])
x = len(book)
nicebook = [0] * x
for i in range(x):
nicebook[i] = book[x - i - 1]
#print nicebook
dist = 0
f = start
for i in nicebook:
dist = dist + self.planning_env.compute_distance(
f, i)
f = i
print "total path =", dist
sumel = self.planning_env.calc_elipse(nicebook)
print "sumel", sumel
print "count", count
anytime = anytime + 1
if anytime == 3:
print("--- %s seconds ---" %
(time.time() - start_time))
return nicebook
break
if count >= num_turns:
print "Goal NOT foud"
return 0
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 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, 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 Plan(self, start_config, goal_config, epsilon = 0.001):
dist = self.planning_env.ComputeConfigDistance(goal_config, start_config)
C_opt = dist
#configs are in form (config, current cost, currcost+heuristic)
start_config = (start_config, 0, 0+dist)
tree = RRTTree(self.planning_env, start_config)
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
prob_goal = 0.2
prob_floor = 0.5
tree.C_max = dist
while dist > epsilon:
'''
if random() < prob_goal:
target_config = goal_config
else:
target_config = self.planning_env.GenerateRandomConfiguration()
'''
if random() < prob_goal:
target_config = goal_config
n_ind, n_vertex = tree.GetNearestVertex(target_config)
else:
## loop until we find a good quality config to extend to
m_quality = -1
r = np.random.rand()
while r > m_quality:
target_config = self.planning_env.GenerateRandomConfiguration()
n_ind, n_vertex = tree.GetNearestVertex(target_config)
C_vertex = n_vertex[2]
#print(C_vertex, C_opt, tree.C_max)
m_quality = 1 - ((C_vertex-C_opt)/(tree.C_max-C_opt))
m_quality = max(m_quality, prob_floor)
r = np.random.rand()
#print m_quality
#not returning a tuple
e_vertex = self.planning_env.Extend(n_vertex[0], target_config)
if e_vertex is None:
pass
else:
#compute C_vertex: (dist from n_vertex to e_vertex) + heuristic
path_cost = self.planning_env.ComputeConfigDistance(n_vertex[0], e_vertex)
new_cost = n_vertex[1] + path_cost + \
self.planning_env.ComputeConfigDistance(e_vertex, goal_config)
e_vid = tree.AddVertex((e_vertex, n_vertex[1] + path_cost, new_cost))
if new_cost > tree.C_max:
tree.C_max = new_cost
tree.AddEdge(n_ind, e_vid)
dist = self.planning_env.ComputeConfigDistance(goal_config, e_vertex)
if self.visualize:
self.planning_env.PlotEdge(n_vertex[0], e_vertex)
plan = []
#traverse backwards
while e_vid != 0:
e_vid = tree.edges[e_vid]
#insert at front of the list
plan.insert(0, tree.vertices[e_vid][0])
plan.append(goal_config)
self.num_vertices = len(tree.vertices)
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
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)
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
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
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
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
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)
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=.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