def lazy_states_contraction(self, best_path):
"""Implementation of lazy states contraction algorithm, prunes
the path by removing any lazy states
Arg's:
best_path: list of nodes forming the best path
Returns:
best_path: pruned best_path
"""
#lazy states contraction
curr_idx = 0
mid_idx = 1
next_idx = 2
while next_idx < len(best_path):
node1 = best_path[curr_idx]
node2 = best_path[next_idx]
_, X = steer(node2['state'], node1['state'], self.dt_des,
self.steer_order)
if self.env.check_path_collision(X):
curr_idx += 1
mid_idx = curr_idx + 1
next_idx = curr_idx + 2
continue
best_path.pop(mid_idx)
best_path[curr_idx]['path'] = X
return best_path
def lazy_states_contraction(self, best_path):
#lazy states contraction
curr_idx = 0
mid_idx = 1
next_idx = 2
while next_idx < len(best_path):
node1 = best_path[curr_idx]
node2 = best_path[next_idx]
if self.check_collision(node1['state'][0:3], node2['state'][0:3]):
curr_idx += 1
mid_idx = curr_idx + 1
next_idx = curr_idx + 2
continue
_, X = steer(node2['state'], node1['state'], self.dt_des,
self.steer_order)
if self.check_path_collision(X):
curr_idx += 1
mid_idx = curr_idx + 1
next_idx = curr_idx + 2
continue
best_path.pop(mid_idx)
best_path[curr_idx]['path'] = X
return best_path
def lazy_states_contraction(self, best_path):
#lazy states contraction
curr_idx = 0
mid_idx = 1
next_idx = 2
while next_idx < len(best_path):
node1 = best_path[curr_idx]
node2 = best_path[next_idx]
if self.check_collision(node1['state'][0:3], node2['state'][0:3]):
curr_idx += 1
mid_idx = curr_idx + 1
next_idx = curr_idx + 2
continue
_, X = steer(node2['state'], node1['state'], self.dt_des,
self.steer_order)
if self.check_path_collision(X):
curr_idx += 1
mid_idx = curr_idx + 1
next_idx = curr_idx + 2
continue
best_path.pop(mid_idx)
best_path[curr_idx]['path'] = X
#total distance
s_t = 0
for node in best_path:
if node['state'][0] == 0:
continue
X = node['path']
x = X[0, :]
y = X[1, :]
z = X[2, :]
N = X.shape[1]
dx = x[1:N] - x[0:N - 1]
dy = y[1:N] - y[0:N - 1]
dz = z[1:N] - z[0:N - 1]
ds2 = dx**2 + dy**2 + dz**2
ds = np.sqrt(ds2)
s = np.sum(ds)
s_t += s
return best_path, s_t
def rrt_plan(self):
"""Function that Runs RRT* with parabolic sampling in the vicinity
of narrow windows
Returns:
best path to the goal
"""
#Iterate to get converges state
for it in range(1, self.max_iter):
if it % 100 == 0:
print('iteration = ', it)
#Random sample random point
state_rand = np.zeros(9)
state_rand[0] = np.random.uniform(self.x_range[0], self.x_range[1])
state_rand[1] = np.random.uniform(self.y_range[0], self.y_range[1])
state_rand[2] = np.random.uniform(self.z_range[0], self.z_range[1])
#sample velocity
angle_rand = np.random.uniform(-np.pi / 2., np.pi / 2.)
state_rand[3:5] = np.array(
[np.cos(angle_rand), np.sin(angle_rand)])
#randomly sample a window every 10 iterations, else random rest of space
if it % 10 == 0:
win = np.random.choice(self.env.windows)
#state_rand, next_node = win.generate_parabolic_nodes(it, self.dt_des)
found_path, state_rand, next_node = self.env.sample_parabolas(
win, it, self.dt_des)
in_win = True
if not (found_path):
continue
else:
#check if in window
in_win, win = self.env.check_in_window(state_rand[0:3])
if in_win:
#state_rand, next_node = win.generate_parabolic_nodes(it, self.dt_des)
found_path, state_rand, next_node = self.env.sample_parabolas(
win, it, self.dt_des)
if not (found_path):
continue
#look for nearby nodes
Near_nodes, Nearest_node, key_Nearest_node = nearby_nodes(
state_rand, self.G, self.r_search)
#Connect to best node
min_cost = float('inf')
best_node = None
for key in Near_nodes.keys():
node = self.G[key]
stage_cost, X = steer(node['state'], state_rand, self.dt_des,
self.steer_order)
if self.env.check_path_collision(X):
continue
total_cost = stage_cost + get_total_cost(self.G, key)
if total_cost < min_cost:
min_cost = total_cost
best_node = key
best_path = X
best_cost = stage_cost
#Continue if node is not found
if best_node == None:
continue
#Wire new node
self.G[it] = {
'parent': best_node,
'state': state_rand,
'cost': best_cost,
'path': best_path,
'free': True
}
#Wire next node if in window
if in_win:
self.G[-it] = next_node
#rewire close nodes to reduce cost
for key in Near_nodes.keys():
node = self.G[key]
stage_cost, X = steer(state_rand, node['state'], self.dt_des,
self.steer_order)
if self.env.check_path_collision(X):
continue
total_cost = stage_cost + get_total_cost(self.G, it)
if total_cost < node['cost']:
self.G[key]['parent'] = it
self.G[key]['cost'] = stage_cost
self.G[key]['path'] = X
#find best node to connect to goal
min_cost = float('inf')
best_node = None
Near_nodes, Nearest_node, key_Nearest_node = nearby_nodes(
self.state_goal, self.G, self.r_search)
for key in Near_nodes.keys():
node = self.G[key]
stage_cost, X = steer(node['state'], self.state_goal, self.dt_des,
self.steer_order)
if self.env.check_path_collision(X):
continue
total_cost = stage_cost + get_total_cost(self.G, key)
if total_cost < min_cost:
min_cost = total_cost
best_node = key
best_path = X
#wire goal state
self.G['goal'] = {
'parent': best_node,
'state': self.state_goal,
'cost': min_cost,
'path': best_path,
'free': True
}
#generate best path
best_path = [self.G['goal']]
parent = best_node
while parent != None:
best_path.append(self.G[parent])
parent = self.G[parent]['parent']
return best_path