def plot_problem(self):
plot_line_segments(self.obstacles, color="red", linewidth=2, label="obstacles")
plt.scatter([self.x_init[0], self.x_goal[0]], [self.x_init[1], self.x_goal[1]], color="green", s=30, zorder=10)
plt.annotate(r"$x_{init}$", self.x_init[:2] + [.2, 0], fontsize=16)
plt.annotate(r"$x_{goal}$", self.x_goal[:2] + [.2, 0], fontsize=16)
plt.legend(loc='upper center', bbox_to_anchor=(0.5, -0.03), fancybox=True, ncol=3)
plt.axis('scaled')
def solve(self, eps, max_iters = 1000, goal_bias = 0.05):
state_dim = len(self.x_init)
# V stores the states that have been added to the RRT (pre-allocated at its maximum size
# since numpy doesn't play that well with appending/extending)
V = np.zeros((max_iters, state_dim))
V[0,:] = self.x_init # RRT is rooted at self.x_init
n = 1 # the current size of the RRT (states accessible as V[range(n),:])
# P stores the parent of each state in the RRT. P[0] = -1 since the root has no parent,
# P[1] = 0 since the parent of the first additional state added to the RRT must have been
# extended from the root, in general 0 <= P[i] < i for all i < n
P = -np.ones(max_iters, dtype=int)
## Intermediate Outputs
# You must update and/or populate:
# - V, P, n: the represention of the planning tree
# - succcess: whether or not you've found a solution within max_iters RRT iterations
# - solution_path: if success is True, then must contain list of states (tree nodes)
# [x_init, ..., x_goal] such that the global trajectory made by linking steering
# trajectories connecting the states in order is obstacle-free.
#Iteration Limit
K = max_iters
p = goal_bias
# TODO: fill me in!
# fork=1 to K do
for i in range(K):
z = np.random.random()
#if z<pthen
if z < p:
x_rand = x_goal
else:
#Get Random State and assign to x_rand
x_rand = np.random.uniform(self.statespace_lo,self.statespace_hi)
x_near = V[find_nearest(V,x_rand)]
X_new = steer_towards(x_near,x_rand,eps)
#if COLLISION FREE(xnear,xnew) then
if self.is_free_motion(self.obstacles,x_near,x_new):
#T .ADD VERTEX(xnew)
np.vstack((V,x_new))
#T .ADD EDGE(xnear , xnew )
np.vstack((P,find_nearest(V,x_rand)))
#if xnew = xgoal then
if x_new == self.x_goal:
#return T.PATH(xinit,xgoal)
success = True
path = []
for j in range(np.shape(V)[0]):
path.append(V[j])
solution_path = (np.asarray(path))
plt.figure()
plot_line_segments(self.obstacles, color="red", linewidth=2, label="obstacles")
self.plot_tree(V, P, color="blue", linewidth=.5, label="RRT tree")
if success:
self.plot_path(solution_path, color="green", linewidth=2, label="solution path")
plt.scatter(V[:n,0], V[:n,1])
plt.scatter([self.x_init[0], self.x_goal[0]], [self.x_init[1], self.x_goal[1]], color="green", s=30, zorder=10)
plt.annotate(r"$x_{init}$", self.x_init[:2] + [.2, 0], fontsize=16)
plt.annotate(r"$x_{goal}$", self.x_goal[:2] + [.2, 0], fontsize=16)
plt.legend(loc='upper center', bbox_to_anchor=(0.5, -0.03), fancybox=True, ncol=3)
def solve(self, eps, max_iters = 1000, goal_bias = 0.05):
state_dim = len(self.x_init)
# V stores the states that have been added to the RRT (pre-allocated at its maximum size
# since numpy doesn't play that well with appending/extending)
V = np.zeros((max_iters, state_dim))
V[0,:] = self.x_init # RRT is rooted at self.x_init
n = 1 # the current size of the RRT (states accessible as V[range(n),:])
# P stores the parent of each state in the RRT. P[0] = -1 since the root has no parent,
# P[1] = 0 since the parent of the first additional state added to the RRT must have been
# extended from the root, in general 0 <= P[i] < i for all i < n
P = -np.ones(max_iters, dtype=int)
## Intermediate Outputs
# You must update and/or populate:
# - V, P, n: the represention of the planning tree
# - succcess: whether or not you've found a solution within max_iters RRT iterations
# - solution_path: if success is True, then must contain list of states (tree nodes)
# [x_init, ..., x_goal] such that the global trajectory made by linking steering
# trajectories connecting the states in order is obstacle-free.
# TODO: fill me in!
plt.figure()
plot_line_segments(self.obstacles, color="red", linewidth=2, label="obstacles")
self.plot_tree(V, P, color="blue", linewidth=.5, label="RRT tree")
if success:
self.plot_path(solution_path, color="green", linewidth=2, label="solution path")
plt.scatter(V[:n,0], V[:n,1])
plt.scatter([self.x_init[0], self.x_goal[0]], [self.x_init[1], self.x_goal[1]], color="green", s=30, zorder=10)
plt.annotate(r"$x_{init}$", self.x_init[:2] + [.2, 0], fontsize=16)
plt.annotate(r"$x_{goal}$", self.x_goal[:2] + [.2, 0], fontsize=16)
plt.legend(loc='upper center', bbox_to_anchor=(0.5, -0.03), fancybox=True, ncol=3)
def plot_tree(self, V, P, resolution = np.pi/24, **kwargs):
line_segments = []
for i in range(V.shape[0]):
if P[i] >= 0:
pts = path_sample(V[P[i],:], V[i,:], self.turning_radius, self.turning_radius*resolution)[0]
pts.append(V[i,:])
for j in range(len(pts) - 1):
line_segments.append((pts[j], pts[j+1]))
plot_line_segments(line_segments, **kwargs)
def plot_tree(self, point_size=15):
plot_line_segments([(x, self.came_from[x]) for x in self.open_set if x !=
self.x_init], linewidth=1, color="blue", alpha=0.2)
plot_line_segments([(x, self.came_from[x]) for x in self.closed_set if x !=
self.x_init], linewidth=1, color="blue", alpha=0.2)
px = [x[0] for x in self.open_set | self.closed_set if x !=
self.x_init and x != self.x_goal]
py = [x[1] for x in self.open_set | self.closed_set if x !=
self.x_init and x != self.x_goal]
plt.scatter(px, py, color="blue", s=point_size, zorder=10, alpha=0.2)
def plot_tree(self, V, P, **kwargs):
plot_line_segments([(V[P[i], :], V[i, :])
for i in range(V.shape[0]) if P[i] >= 0], **kwargs)
def solve(self, eps, max_iters=1000, goal_bias=0.05):
state_dim = len(self.x_init)
# V stores the states that have been added to the RRT (pre-allocated at its maximum size
# since numpy doesn't play that well with appending/extending)
V = np.zeros((max_iters, state_dim))
V[0, :] = self.x_init # RRT is rooted at self.x_init
n = 1 # the current size of the RRT (states accessible as V[range(n),:])
# P stores the parent of each state in the RRT. P[0] = -1 since the root has no parent,
# P[1] = 0 since the parent of the first additional state added to the RRT must have been
# extended from the root, in general 0 <= P[i] < i for all i < n
P = -np.ones(max_iters, dtype=int)
## Intermediate Outputs
# You must update and/or populate:
# - V, P, n: the represention of the planning tree
# - succcess: whether or not you've found a solution within max_iters RRT iterations
# - solution_path: if success is True, then must contain list of states (tree nodes)
# [x_init, ..., x_goal] such that the global trajectory made by linking steering
# trajectories connecting the states in order is obstacle-free.
# TODO: fill me in!
success = False
i = 0
while not success and i <= max_iters:
i = i + 1
z = np.random.uniform()
if z < goal_bias:
x_rand = self.x_goal
else:
x_rand = np.random.uniform(self.statespace_lo,
self.statespace_hi)
x_near = V[self.find_nearest(V[:n, :], x_rand), :]
x_new = self.steer_towards(x_near, x_rand, eps)
if self.is_free_motion(self.obstacles, x_near, x_new):
V[n, :] = x_new
# print V - x_near
# print np.where(V[:n,:] == x_near)
P[n] = np.where(V[:n, :] == x_near)[0][0]
n = n + 1
if np.all(x_new == self.x_goal):
success = True
P = P[:n]
V = V[:n, :]
for i in range(len(P)):
if i < P[i]:
print "Error"
# print P
solution_path = [self.x_goal]
# print "path"
while np.all(solution_path[0] != self.x_init):
idx = np.where(V == solution_path[0])[0][0]
solution_path = [V[P[idx], :]] + solution_path
C = 0
for i in range(1, len(solution_path)):
C = C + np.linalg.norm(solution_path[i] - solution_path[i - 1])
print C
# print type(solution_path)
plt.figure()
plot_line_segments(self.obstacles,
color="red",
linewidth=2,
label="obstacles")
self.plot_tree(V, P, color="blue", linewidth=.5, label="RRT tree")
if success:
self.plot_path(solution_path,
color="green",
linewidth=2,
label="solution path")
plt.scatter(V[:n, 0], V[:n, 1])
plt.scatter([self.x_init[0], self.x_goal[0]],
[self.x_init[1], self.x_goal[1]],
color="green",
s=30,
zorder=10)
plt.annotate(r"$x_{init}$", self.x_init[:2] + [.2, 0], fontsize=16)
plt.annotate(r"$x_{goal}$", self.x_goal[:2] + [.2, 0], fontsize=16)
plt.legend(loc='upper center',
bbox_to_anchor=(0.5, -0.03),
fancybox=True,
ncol=3)
def solve(self, eps, max_iters = 1000, goal_bias = 0.05):
state_dim = len(self.x_init)
# V stores the states that have been added to the RRT (pre-allocated at its maximum size
# since numpy doesn't play that well with appending/extending)
V = np.zeros((max_iters, state_dim))
V[0,:] = self.x_init # RRT is rooted at self.x_init
n = 1 # the current size of the RRT (states accessible as V[range(n),:])
# P stores the parent of each state in the RRT. P[0] = -1 since the root has no parent,
# P[1] = 0 since the parent of the first additional state added to the RRT must have been
# extended from the root, in general 0 <= P[i] < i for all i < n
P = -np.ones(max_iters, dtype=int)
## Intermediate Outputs
# You must update and/or populate:
# - V, P, n: the represention of the planning tree
# - success: whether or not you've found a solution within max_iters RRT iterations
# - solution_path: if success is True, then must contain list of states (tree nodes)
# [x_init, ..., x_goal] such that the global trajectory made by linking steering
# trajectories connecting the states in order is obstacle-free.
success = False
for k in range(max_iters):
# Steer towards random state with probability (1 - goal_bias).
z = np.random.random_sample()
if z < goal_bias:
x_rand = self.x_goal
else:
x_rand = np.array([np.random.uniform(lo, hi) for lo, hi in zip(self.statespace_lo, self.statespace_hi)])
idx_near = self.find_nearest(V[:n,:], x_rand)
x_near = V[idx_near,:]
x_new = self.steer_towards(x_near, x_rand, eps)
# Add to RRT if path from x_near to x_new is collision-free.
if self.is_free_motion(self.obstacles, x_near, x_new):
V[n,:] = x_new
P[n] = idx_near
n = n + 1
# Reconstruct solution path when goal reached.
if np.array_equal(x_new, self.x_goal):
idx_cur = n - 1
solution_path = [self.x_goal]
while not np.array_equal(V[idx_cur,:], self.x_init):
solution_path.append(V[P[idx_cur],:])
idx_cur = P[idx_cur]
solution_path = list(reversed(solution_path))
success = True
break
plt.figure()
plot_line_segments(self.obstacles, color="red", linewidth=2, label="obstacles")
self.plot_tree(V, P, color="blue", linewidth=.5, label="RRT tree")
if success:
self.plot_path(solution_path, color="green", linewidth=2, label="solution path")
plt.scatter(V[:n,0], V[:n,1])
plt.scatter([self.x_init[0], self.x_goal[0]], [self.x_init[1], self.x_goal[1]], color="green", s=30, zorder=10)
plt.annotate(r"$x_{init}$", self.x_init[:2] + [.2, 0], fontsize=16)
plt.annotate(r"$x_{goal}$", self.x_goal[:2] + [.2, 0], fontsize=16)
plt.legend(loc='upper center', bbox_to_anchor=(0.5, -0.03), fancybox=True, ncol=3)
def solve(self, eps, max_iters = 1000, goal_bias = 0.05):
state_dim = len(self.x_init)
# V stores the states that have been added to the RRT (pre-allocated at its maximum size
# since numpy doesn't play that well with appending/extending)
V = np.zeros((max_iters, state_dim))
V[0,:] = self.x_init # RRT is rooted at self.x_init
n = 1 # the current size of the RRT (states accessible as V[range(n),:])
self.tree_size = 1 # TARIQ EDIT: self.tree_size is n. It's a self-variable so it can be accessed by methods
# NOTE: self.tree_size is initialized in the _init_ of the class as well
# P stores the parent of each state in the RRT. P[0] = -1 since the root has no parent,
# P[1] = 0 since the parent of the first additional state added to the RRT must have been
# extended from the root, in general 0 <= P[i] < i for all i < n
P = -np.ones(max_iters, dtype=int)
## Intermediate Outputs
# You must update and/or populate:
# - V, P, n: the represention of the planning tree
# - succcess: whether or not you've found a solution within max_iters RRT iterations
# - solution_path: if success is True, then must contain list of states (tree nodes)
# [x_init, ..., x_goal] such that the global trajectory made by linking steering
# trajectories connecting the states in order is obstacle-free.
# TODO: fill me in!
solution_path = []
for k in range(1, max_iters):
#sample a random point
z = np.random.random()
if(z <= goal_bias):
x_rand = self.x_goal
else :
x_rand = np.zeros(state_dim)
for i in range(state_dim):
x_rand[i] = np.random.uniform(self.statespace_lo[i], self.statespace_hi[i],1)
x_rand = np.array(x_rand)
#find the nearest node from our tree
near_index = self.find_nearest(V[:self.tree_size], x_rand)
x_near = V[near_index, :]
#ensure that the point we're steering towards is an eps
#step away
x_new = self.steer_towards(x_near, x_rand, eps)
#if we can connect, connect!
if self.is_free_motion(self.obstacles, x_near, x_new):
#Add vertex
V[self.tree_size, :] = x_new
#Add edge
P[self.tree_size] = near_index
#increment the tree length
n=n+1 #catching any legacy code; self.tree_size and n are the same value
self.tree_size = self.tree_size+1
#if the new point is the goal, then we have a path!
if np.linalg.norm(x_new - self.x_goal) <= eps:
success = True
#note: V and P are initialized with max_iters size
#must keep track of tree size with "n"
solution_path = self.reconstructPath(V[:self.tree_size],P[:self.tree_size])
break
if(k == max_iters-1):
print("Failed to find a path")
success = False
break
plt.figure()
plot_line_segments(self.obstacles, color="red", linewidth=2, label="obstacles")
self.plot_tree(V, P, color="blue", linewidth=.5, label="RRT tree")
if success:
self.plot_path(solution_path, color="green", linewidth=2, label="solution path")
plt.scatter(V[:n,0], V[:n,1])
plt.scatter([self.x_init[0], self.x_goal[0]], [self.x_init[1], self.x_goal[1]], color="green", s=30, zorder=10)
plt.annotate(r"$x_{init}$", self.x_init[:2] + [.2, 0], fontsize=16)
plt.annotate(r"$x_{goal}$", self.x_goal[:2] + [.2, 0], fontsize=16)
plt.legend(loc='upper center', bbox_to_anchor=(0.5, -0.03), fancybox=True, ncol=3)
def solve(self, eps, max_iters=1000, goal_bias=0.05):
state_dim = len(self.x_init)
# V stores the states that have been added to the RRT (pre-allocated at its maximum size
# since numpy doesn't play that well with appending/extending)
V = np.zeros((max_iters, state_dim))
V[0, :] = self.x_init # RRT is rooted at self.x_init
n = 1 # the current size of the RRT (states accessible as V[range(n),:])
# P stores the parent of each state in the RRT. P[0] = -1 since the root has no parent,
# P[1] = 0 since the parent of the first additional state added to the RRT must have been
# extended from the root, in general 0 <= P[i] < i for all i < n
P = -np.ones(max_iters, dtype=int)
## Intermediate Outputs
# You must update and/or populate:
# - V, P, n: the represention of the planning tree
# - succcess: whether or not you've found a solution within max_iters RRT iterations
# - solution_path: if success is True, then must contain list of states (tree nodes)
# [x_init, ..., x_goal] such that the global trajectory made by linking steering
# trajectories connecting the states in order is obstacle-free.
success = False
solution_path = []
for k in range(max_iters):
z = np.random.uniform(0.0, 1.0, 1)
if z < goal_bias:
x_rand = self.x_goal
else:
x_rand = np.zeros(state_dim)
for i in range(state_dim):
x_rand[i] = np.random.uniform(self.statespace_lo[i],
self.statespace_hi[i], 1)
x_rand = np.array(x_rand)
near_idx = self.find_nearest(V[:n], x_rand)
x_near = V[near_idx, :]
x_new = self.steer_towards(x_near, x_rand, eps)
if self.is_free_motion(self.obstacles, x_near, x_new):
V[n, :] = x_new
P[n] = near_idx
n += 1
if np.linalg.norm(x_new[:2] - self.x_goal[:2]) <= 0.1:
success = True
# reconstruct path
solution_path = self.reconstruct_path(V[:n], P[:n])
break
plt.figure()
plot_line_segments(self.obstacles,
color="red",
linewidth=2,
label="obstacles")
self.plot_tree(V, P, color="blue", linewidth=.5, label="RRT tree")
if success:
self.plot_path(solution_path,
color="green",
linewidth=2,
label="solution path")
plt.scatter(V[:n, 0], V[:n, 1])
plt.scatter([self.x_init[0], self.x_goal[0]],
[self.x_init[1], self.x_goal[1]],
color="green",
s=30,
zorder=10)
plt.annotate(r"$x_{init}$", self.x_init[:2] + [.2, 0], fontsize=16)
plt.annotate(r"$x_{goal}$", self.x_goal[:2] + [.2, 0], fontsize=16)
plt.legend(loc='upper center',
bbox_to_anchor=(0.5, -0.03),
fancybox=True,
ncol=3)
def solve(self, eps, max_iters=1000, goal_bias=0.05):
state_dim = len(self.x_init)
# V stores the states that have been added to the RRT (pre-allocated at its maximum size
# since numpy doesn't play that well with appending/extending)
V = np.zeros((max_iters, state_dim))
V[0, :] = self.x_init # RRT is rooted at self.x_init
n = 1 # the current size of the RRT (states accessible as V[range(n),:])
# P stores the parent of each state in the RRT. P[0] = -1 since the root has no parent,
# P[1] = 0 since the parent of the first additional state added to the RRT must have been
# extended from the root, in general 0 <= P[i] < i for all i < n
P = -np.ones(max_iters, dtype=int)
## Intermediate Outputs
# You must update and/or populate:
# - V, P, n: the represention of the planning tree
# - succcess: whether or not you've found a solution within max_iters RRT iterations
# - solution_path: if success is True, then must contain list of states (tree nodes)
# [x_init, ..., x_goal] such that the global trajectory made by linking steering
# trajectories connecting the states in order is obstacle-free.
# TODO: fill me in!
for i in range(1, max_iters):
print 'iteration ', i
# Randomly sample z
z = np.random.random()
# Bias the sample towards the goal state
if z <= goal_bias:
x_rand = self.x_goal
else:
x_rand = self.getRandomState()
# Find the nearest point in the list of past points we've seen
near_idx = self.find_nearest(V, x_rand)
x_near = V[near_idx]
# Move in the direction of the point by epsilon, unless it's closer than epsilon
x_new = self.steer_towards(x_near, x_rand, eps)
# Check whether new point is collision free
if self.is_free_motion(self.obstacles, x_near, x_new):
V[self.V_size, :] = np.asarray(x_new)
P[self.V_size] = near_idx
self.V_size += 1
if np.array_equal(x_new, self.x_goal):
success = True
solution_path = self.reconstructPath(V, P)
break
if i == max_iters - 1:
print 'Was not able to find a path in ', max_iters, ' iterations'
success = False
break
plt.figure()
plot_line_segments(self.obstacles,
color="red",
linewidth=2,
label="obstacles")
self.plot_tree(V, P, color="blue", linewidth=.5, label="RRT tree")
if success:
self.plot_path(solution_path,
color="green",
linewidth=2,
label="solution path")
plt.scatter(V[:n, 0], V[:n, 1])
plt.scatter([self.x_init[0], self.x_goal[0]],
[self.x_init[1], self.x_goal[1]],
color="green",
s=30,
zorder=10)
plt.annotate(r"$x_{init}$", self.x_init[:2] + [.2, 0], fontsize=16)
plt.annotate(r"$x_{goal}$", self.x_goal[:2] + [.2, 0], fontsize=16)
plt.legend(loc='upper center',
bbox_to_anchor=(0.5, -0.03),
fancybox=True,
ncol=3)
def solve(self, eps, max_iters=1000, goal_bias=0.05):
state_dim = len(self.x_init)
# V stores the states that have been added to the RRT (pre-allocated at its maximum size
# since numpy doesn't play that well with appending/extending)
x_init = node(self.x_init)
x_init.setP(None)
x_init.setT(0)
V = [x_init]
n = 1 # the current size of the RRT (states accessible as V[range(n),:])
# P stores the parent of each state in the RRT. P[0] = -1 since the root has no parent,
# P[1] = 0 since the parent of the first additional state added to the RRT must have been
# extended from the root, in general 0 <= P[i] < i for all i < n
## Intermediate Outputs
# You must update and/or populate:
# - V, P, n: the represention of the planning tree
# - succcess: whether or not you've found a solution within max_iters RRT iterations
# - solution_path: if success is True, then must contain list of states (tree nodes)
# [x_init, ..., x_goal] such that the global trajectory made by linking steering
# trajectories connecting the states in order is obstacle-free.
# TODO: fill me in!
success = False
i = 0
while not success and i <= max_iters:
i = i + 1
z = np.random.uniform()
if z < goal_bias:
x_rand = self.x_goal
else:
x_rand = np.random.uniform(self.statespace_lo,
self.statespace_hi)
success, V = self.extend(V, x_rand, eps)
# print P
# print type(solution_path)
plt.figure()
plot_line_segments(self.obstacles,
color="red",
linewidth=2,
label="obstacles")
self.plot_tree(V, color="blue", linewidth=.5, label="RRT tree")
nodes = np.zeros((self.n, 2))
for i in range(len(V)):
nodes[i, :] = V[i].loc
if np.all(V[i].loc == self.x_goal):
goalnode = V[i]
if success:
solution_path_node = [goalnode]
solution_path = [goalnode.loc]
while np.all(solution_path[0] != self.x_init):
parent = solution_path_node[0].parent
solution_path_node = [parent] + solution_path
solution_path = [parent.loc] + solution_path
self.plot_path(solution_path,
color="green",
linewidth=2,
label="solution path")
print goalnode.T
plt.scatter(nodes[:, 0], nodes[:, 1])
plt.scatter([self.x_init[0], self.x_goal[0]],
[self.x_init[1], self.x_goal[1]],
color="green",
s=30,
zorder=10)
plt.annotate(r"$x_{init}$", self.x_init[:2] + [.2, 0], fontsize=16)
plt.annotate(r"$x_{goal}$", self.x_goal[:2] + [.2, 0], fontsize=16)
plt.legend(loc='upper center',
bbox_to_anchor=(0.5, -0.03),
fancybox=True,
ncol=3)
def plot_tree(self, V, **kwargs):
plot_line_segments([(V[i].parent.loc, V[i].loc)
for i in range(len(V)) if V[i].parent != None],
**kwargs)
