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