def plan(self, start, goal, env):
"""Constructs a graph avoiding obstacles and then plans path from start to goal within the graph.
Args:
start (gennav.utils.RobotState): tuple with start point coordinates.
goal (gennav.utils.RobotState): tuple with end point coordinates.
env (gennav.envs.Environment): Base class for an envrionment.
Returns:
gennav.utils.Trajectory: The planned path as trajectory
dict: Dictionary containing additional information
"""
# Check if start and goal states are obstacle free
if not env.get_status(start):
raise InvalidStartState(start,
message="Start state is in obstacle.")
if not env.get_status(goal):
raise InvalidGoalState(goal, message="Goal state is in obstacle.")
# construct graph
graph = self.construct(env)
# find collision free point in graph closest to start_point
min_dist = float("inf")
for node in graph.nodes:
dist = compute_distance(node.position, start.position)
traj = Trajectory([node, start])
if dist < min_dist and (env.get_traj_status(traj)):
min_dist = dist
s = node
# find collision free point in graph closest to end_point
min_dist = float("inf")
for node in graph.nodes:
dist = compute_distance(node.position, goal.position)
traj = Trajectory([node, goal])
if dist < min_dist and (env.get_traj_status(traj)):
min_dist = dist
e = node
# add start_point to path
path = [start]
traj = Trajectory(path)
# perform astar search
p = astar(graph, s, e)
if len(p.path) == 1:
raise PathNotFound(p, message="Path contains only one state")
else:
traj.path.extend(p.path)
# add end_point to path
traj.path.append(goal)
return traj, {}
def test_uniform_circular_sampler():
radius = 5
centre = Point(x=1, y=2)
sampler = UniformCircularSampler(radius, centre)
for _ in range(100):
s = sampler()
p = s.position
assert compute_distance(p, centre) <= radius
def construct(self, env):
"""Constructs DStar graph.
Args:
env (gennav.envs.Environment): Base class for an envrionment.
Returns:
graph (gennav.utils.graph): A dict where the keys correspond to nodes and
the values for each key is a list of the neighbour nodes
"""
nodes = []
graph = Graph()
i = 0
# samples points from the sample space until n points
# outside obstacles are obtained
while i < self.n:
sample = self.sampler()
if not env.get_status(sample):
continue
else:
i += 1
node = NodeDstar(state=sample)
nodes.append(node)
# finds neighbours for each node in a fixed radius r
for node1 in nodes:
for node2 in nodes:
if node1 != node2:
dist = compute_distance(node1.state.position, node2.state.position)
if dist < self.r:
if env.get_traj_status(Trajectory([node1.state, node2.state])):
if node1 not in graph.nodes:
graph.add_node(node1)
if node2 not in graph.nodes:
graph.add_node(node2)
if (
node2 not in graph.edges[node1]
and node1 not in graph.edges[node2]
):
graph.add_edge(
node1, node2,
)
return graph
def plan(self, start, goal, env):
"""Path planning method
Args:
start (gennav.utils.common.RobotState): Starting state.
goal (gennav.utils.common.RobotState): Destination state
env (gennav.envs.Environment): Environment object
Returns:
gennav.utils.Trajectory: The path as a Trajectory
dict: Dictionary containing additional information like the node_list
"""
# Check if start and goal states are obstacle free
if not env.get_status(start):
raise InvalidStartState(start,
message="Start state is in obstacle.")
if not env.get_status(goal):
raise InvalidGoalState(goal, message="Goal state is in obstacle.")
# Initialize start and goal nodes.
x_start = Node(state=start)
x_goal = Node(state=goal)
# Starting the tree:
node_list = [x_start]
# X_soln contains all the nodes near the goal
X_soln = []
for i in range(self.max_iter):
# defining the best cost as cbest
if len(X_soln) == 0:
cbest = float("inf")
else:
cbest = X_soln[0].cost + compute_distance(
X_soln[0].state.position, x_goal.state.position)
for node in X_soln:
if (node.cost + compute_distance(node.state.position,
x_goal.state.position) <
cbest):
cbest = node.cost + compute_distance(
node.state.position, x_goal.state.position)
# sample the new random point using the sample function
if cbest < float("inf"):
# Converting x_start and x_goal into arrays so that matrix calculations can be handled
x_start_array = np.array(
[x_start.state.position.x, x_start.state.position.y],
dtype=np.float32,
).reshape(2, 1)
x_goal_array = np.array(
[x_goal.state.position.x, x_goal.state.position.y],
dtype=np.float32).reshape(2, 1)
cmin = np.linalg.norm((x_goal_array - x_start_array))
x_center = (x_start_array + x_goal_array) / 2
a1 = (x_goal_array - x_start_array) / cmin
I1 = np.eye(len(x_start_array))[0].reshape(
len(x_start_array), 1)
M = np.dot(a1, I1.T)
[u, s, v] = np.linalg.svd(M)
# numpy returns the transposed value for v, so to convert it back
v = v.T
# Calculating the rotation to world frame matrix given by C = U*diag{1,....1.det(U)det(V)}*V.T where * denotes matrix multiplication
diag = np.eye(2)
diag[1, 1] = np.linalg.det(u) * np.linalg.det(v)
C = np.dot(np.dot(u, diag), v.T)
L = np.eye(2)
L[0, 0] = cbest / 2
L[1, 1] = np.sqrt(cbest**2 - cmin**2) / 2
rand_state = self.circular_sampler()
x_ball = np.array(
[rand_state.position.x, rand_state.position.y],
dtype=np.float32).reshape(2, 1)
x_rand_array = np.dot(np.dot(C, L), x_ball) + x_center
x_rand = RobotState()
x_rand.position.x = x_rand_array[0, 0]
x_rand.position.y = x_rand_array[1, 0]
x_rand_node = Node(state=x_rand)
else:
x_rand = self.rectangular_sampler()
x_rand_node = Node(state=x_rand)
# Finding the nearest node to the random point
distance_list = [
compute_distance(node.state.position,
x_rand_node.state.position)
for node in node_list
]
min_dist = distance_list[0]
min_index = 0
for index, distance in enumerate(distance_list):
if distance < min_dist:
min_dist = distance
min_index = index
x_nearest = node_list[min_index]
# Steering at a distance of self.expand_dis
theta = compute_angle(x_nearest.state.position,
x_rand_node.state.position)
x = x_nearest.state.position.x + self.expand_dis * math.cos(theta)
y = x_nearest.state.position.y + self.expand_dis * math.sin(theta)
t = RobotState(position=Point(x, y))
x_new = Node(state=t)
# Defining a temporary trajectory between the new node and its nearest node
traj = Trajectory(path=[x_nearest.state, x_new.state])
# Checking whether new node and the nearest node can be connected
if env.get_traj_status(traj):
# Adding new node to the node list
node_list.append(x_new)
# Defining the neighbours of x_new
X_near = []
for node in node_list:
if (compute_distance(
node.state.position, x_new.state.position)) < (
self.neighbourhood_radius
) and ( # If it is inside the neighbourhood radius
compute_distance(node.state.position,
x_new.state.position) != 0):
X_near.append(node)
x_min = x_nearest
c_min = x_min.cost + self.expand_dis
for x_near in X_near:
c_new = x_near.cost + compute_distance(
x_near.state.position, x_new.state.position)
if c_new < c_min:
traj = Trajectory(path=[x_near.state, x_new.state])
if env.get_traj_status(traj):
x_min = x_near
c_min = c_new
# Defining the parent node and cost of x_new
x_new.parent = x_min
x_new.cost = c_min
# Rewiring
for x_near in X_near:
c_near = x_near.cost
c_new = x_new.cost + compute_distance(
x_near.state.position, x_new.state.position)
if c_new < c_near:
traj = Trajectory(path=[x_new.state, x_near.state])
if env.get_traj_status(traj):
x_near.parent = x_new
x_near.cost = c_new
goal_traj = Trajectory(path=[x_new.state, x_goal.state])
if (compute_distance(x_goal.state.position,
x_new.state.position)
) < self.goal_distance and env.get_traj_status(goal_traj):
X_soln.append(x_new)
# X_soln contains all the solutions
if len(X_soln) == 0:
print("No solution found!")
path = []
path.append(x_start.state)
traj = Trajectory(path=path)
info_dict = {}
info_dict["node_list"] = node_list
if len(traj.path) == 1:
raise PathNotFound(path,
message="Path contains only one state")
return (traj, info_dict)
else:
print("Goal reached!")
Cbest = X_soln[0].cost
Xbest = X_soln[0]
node_list.append(x_goal)
for node in X_soln:
if node.cost < Cbest:
Cbest = node.cost
Xbest = node
x_goal.parent = Xbest
# Xbest is the optimum solution to the problem.
# Back tracing the path:
path = []
path.append(x_goal.state)
path.append(Xbest.state)
p = Xbest.parent
while p is not None:
path.append(p.state)
p = p.parent
path.append(x_start.state)
path.reverse()
# print("No. of solutions " + str(len(X_soln)))
trajectory = Trajectory(path=path)
info_dict = {}
info_dict["node_list"] = node_list
return (trajectory, info_dict)
def replan(self, start, goal, env):
"""Constructs a graph avoiding obstacles and then plans path from start to goal within the graph.
Args:
start (gennav.utils.RobotState): tuple with start point coordinates.
goal (gennav.utils.RobotState): tuple with end point coordinates.
env (gennav.envs.Environment): Base class for an envrionment.
Returns:
gennav.utils.Trajectory: The planned path as trajectory
"""
global traj, open_, graph, closed
min_dist = float("inf")
old_traj = traj
for node in old_traj.path:
dist = compute_distance(node.position, start.position)
traj_chk = Trajectory([node, start])
if dist < min_dist and (env.get_traj_status(traj_chk)):
min_dist = dist
s = node
a = old_traj.path.index(s)
new_traj = Trajectory([start])
new_traj.path.extend(old_traj.path[a:])
if env.get_traj_status(new_traj):
return new_traj
print 1
min_dist = float("inf")
for node in graph.nodes:
dist = compute_distance(node.state.position, start.position)
traj = Trajectory([node.state, start])
if dist < min_dist and (env.get_traj_status(traj)):
min_dist = dist
print 2
start_node = node
# find collision free point in graph closest to end_point
min_dist = float("inf")
for node in graph.nodes:
dist = compute_distance(node.state.position, goal.position)
traj = Trajectory([node.state, goal])
if dist < min_dist and (env.get_traj_status(traj)):
min_dist = dist
print 3
goal_node = node
found=0
for state in traj.path:
if not env.get_status(state):
print 4
for item in graph.nodes:
if item.state == state:
node_ = item
break
node_.t = float("inf")
if node_.tag == "CLOSED":
print 4
closed.remove(node_)
node_.tag = "OPEN"
open_.append(node)
else:
print 999
found=1
break
if found==0:
for state in traj.path:
if traj.path.index(state) < len(traj.path) - 1 and (
not env.get_traj_status(
Trajectory([state, traj.path[traj.path.index(state) + 1]])
)
):
print 55
next_state=traj.path[traj.path.index(state) + 1]
##unable to find next_state in graph.nodes
for item in graph.nodes:
if item.state == next_state:
print 444
node_ = item
break
node_.t = float("inf")
if node_.tag == "CLOSED":
print 5
closed.remove(node_)
node_.tag = "OPEN"
open_.append(node)
else:
print 999
found=1
break
else:
print 333
if found==1:
if node_.h is not None:
for neighbour in graph.edges[node_]:
if neighbour.tag == "CLOSED":
print 6
closed.remove(neighbour)
neighbour.tag = "OPEN"
open_.append(neighbour)
elif neighbour.tag == "NEW":
print 60
neighbour.parent=node
neighbour.h=compute_distance(
node_.state.position, neighbour.state.position
)+node_.h
neighbour.k=neighbour.h
neighbour.tag = "OPEN"
open_.append(neighbour)
while len(open_) > 0:
open_.sort()
current_node = open_.pop(0)
current_node.tag = "CLOSED"
closed.append(current_node)
if current_node.k >= start_node.h and start_node.tag == "CLOSED":
print 7
current_node = start_node
path = []
path.append(start)
# while current_node.parent is not None:
# print 1
# path.append(current_node.state)
# h_ = float("inf")
# neighbours = graph.edges[current_node]
# for neighbour in neighbours:
# if neighbour.h < h_:
# node_ = neighbour
# h_ = neighbour.h
# current_node = node_
i = 0
while current_node.parent is not None:
path.append(current_node.state)
current_node = current_node.parent
if i > 40:
traj = Trajectory(path)
print 88
return traj
i += 1
path.append(goal_node.state)
path.append(goal)
traj = Trajectory(path)
print 9
return traj
if current_node.k < current_node.h and current_node.k is not None:
print 10
for neighbour in graph.edges[current_node]:
if (
neighbour.tag != "NEW"
and neighbour.h <= current_node.k
and current_node.h > neighbour.h + c(current_node, neighbour)
):
current_node.parent = neighbour
current_node.h = neighbour.h + c(current_node, neighbour)
if current_node.k == current_node.h and current_node.k is not None:
print 11
for neighbour in graph.edges[current_node]:
if (
neighbour.tag == "NEW"
or (
neighbour.parent == current_node
and neighbour.h
!= current_node.h + c(current_node, neighbour)
)
or (
neighbour.parent != current_node
and neighbour.h
> current_node.h + c(current_node, neighbour)
)
):
neighbour.parent = current_node
insert(neighbour, current_node.h + c(current_node, neighbour))
elif current_node.k is not None:
print 12
for neighbour in graph.edges[current_node]:
if neighbour.tag == "NEW" or (
neighbour.parent == current_node
and neighbour.h != current_node.h + c(current_node, neighbour)
):
neighbour.parent = current_node
insert(neighbour, current_node.h + c(current_node, neighbour))
elif (
neighbour.parent != current_node
and neighbour.h > current_node.h + c(current_node, neighbour)
):
insert(current_node, current_node.h)
elif (
neighbour.parent != current_node
and current_node.h > neighbour.h + c(current_node, neighbour)
and (neighbour.tag == "CLOSED")
and neighbour.h > current_node.k
):
insert(neighbour, neighbour.h)
path = [start]
traj = Trajectory(path)
raise PathNotFound(traj, message="Path contains only one state")
def plan(self, start, goal, env):
"""Constructs a graph avoiding obstacles and then plans path from start to goal within the graph.
Args:
start (gennav.utils.RobotState): tuple with start point coordinates.
goal (gennav.utils.RobotState): tuple with end point coordinates.
env (gennav.envs.Environment): Base class for an envrionment.
Returns:
gennav.utils.Trajectory: The planned path as trajectory
"""
# construct graph
global graph, traj
graph = self.construct(env)
# find collision free point in graph closest to start_point
min_dist = float("inf")
for node in graph.nodes:
dist = compute_distance(node.state.position, start.position)
traj = Trajectory([node.state, start])
if dist < min_dist and (env.get_traj_status(traj)):
min_dist = dist
start_node = node
# find collision free point in graph closest to end_point
min_dist = float("inf")
for node in graph.nodes:
dist = compute_distance(node.state.position, goal.position)
traj = Trajectory([node.state, goal])
if dist < min_dist and (env.get_traj_status(traj)):
min_dist = dist
goal_node = node
global open_, closed
open_ = []
closed = []
goal_node.h = 0
goal_node.k = 0
open_.append(goal_node)
while len(open_) > 0:
open_.sort()
current_node = open_.pop(0)
current_node.tag = "CLOSED"
closed.append(current_node)
if current_node.state.position == start_node.state.position:
path = []
path.append(start)
# while current_node.parent is not None:
# print 1
# path.append(current_node.state)
# h_=float("inf")
# neighbours=graph.edges[current_node]
# for neighbour in neighbours:
# if neighbour.h<h_:
# node=neighbour
# h_=neighbour.h
# current_node=node
while current_node.parent is not None:
path.append(current_node.state)
current_node = current_node.parent
path.append(goal_node.state)
path.append(goal)
traj = Trajectory(path)
return traj
neighbours = graph.edges[current_node]
for neighbour in neighbours:
if neighbour.tag=="CLOSED":
continue
neighbour.parent = current_node
neighbour.h = compute_distance(
current_node.state.position, neighbour.state.position
)+current_node.h
neighbour.k = neighbour.h
neighbour.tag = "OPEN"
open_.append(neighbour)
path = [start]
traj = Trajectory(path)
raise PathNotFound(traj, message="Path contains only one state")
def c(node1, node2):
if node1.t == float("inf") or node1.t == float("inf"):
return float("inf")
else:
return compute_distance(node1.state.position, node2.state.position)
def astar(graph, start, end, heuristic={}):
"""
Performs A-star search to find the shortest path from start to end
Args:
graph (gennav.utils.graph): Dictionary representing the graph where keys are the nodes
and the value is a list of all neighbouring nodes
start (gennav.utils.RobotState): Point representing key corresponding
to the start point
end (gennav.utils.RobotState): Point representing key corresponding
to the end point
heuristic (dict): Dictionary containing the heuristic values for all the nodes,
if not specified the default heuristic is euclidean distance
Returns:
gennav.utils.Trajectory: The planned path as trajectory
"""
if not (start in graph.nodes and end in graph.nodes):
path = [start]
traj = Trajectory(path)
return traj
open_ = []
closed = []
# calculates heuristic for start if not provided by the user
# pushes the start point in the open_ Priority Queue
start_node = NodeAstar(state=start)
if len(heuristic) == 0:
start_node.h = compute_distance(start.position, end.position)
else:
start_node.h = heuristic[start]
start_node.g = 0
start_node.f = start_node.g + start_node.h
open_.append(start_node)
# performs astar search to find the shortest path
while len(open_) > 0:
open_.sort()
current_node = open_.pop(0)
closed.append(current_node)
# checks if the goal has been reached
if current_node.state.position == end.position:
path = []
# forms path from closed list
while current_node.parent is not None:
path.append(current_node.state)
current_node = current_node.parent
path.append(start_node.state)
# returns reversed path
path = path[::-1]
traj = Trajectory(path)
return traj
# continues to search for the goal
# makes a list of all neighbours of the current_node
neighbours = graph.edges[current_node.state]
# adds them to open_ if they are already present in open_
# checks and updates the total cost for all the neighbours
for node in neighbours:
# creates neighbour which can be pushed to open_ if required
neighbour = NodeAstar(state=node, parent=current_node)
# checks if neighbour is in closed
if neighbour in closed:
continue
# calculates weight cost
neighbour.g = compute_distance(node.position,
current_node.state.position)
# calculates heuristic for the node if not provided by the user
if len(heuristic) == 0:
neighbour.h = compute_distance(node.position, end.position)
else:
neighbour.h = heuristic[node]
# calculates total cost
neighbour.f = neighbour.g + neighbour.h
# checks if the total cost of neighbour needs to be updated
# if it is presnt in open_ else adds it to open_
flag = 1
for new_node in open_:
if neighbour == new_node and neighbour.f < new_node.f:
new_node = neighbour # lgtm [py/multiple-definition]
flag = 0
break
elif neighbour == new_node and neighbour.f > new_node.f:
flag = 0
break
if flag == 1:
open_.append(neighbour)
# if path doesn't exsist it raises a PathNotFound Exception.
path = [start]
traj = Trajectory(path)
raise PathNotFound(traj, message="Path contains only one state")
def plan(self, start, end, env):
"""Plans path from start to goal avoiding obstacles.
Args:
start (gennav.utils.RobotState): Starting state
end (gennav.utils.RobotState): Goal state
env: (gennav.envs.Environment) Base class for an envrionment.
Returns:
gennav.utils.Trajectory: The planned path as trajectory
dict: Dictionary containing additional information
"""
# Check if start and end states are obstacle free
if not env.get_status(start):
raise InvalidStartState(start,
message="Start state is in obstacle.")
if not env.get_status(end):
raise InvalidGoalState(end, message="Goal state is in obstacle.")
# Initialize graph
graph = Graph()
graph.add_node(start)
# Initialize node list with Starting Position
node_list = [start]
# Loop for maximum iterations to get the best possible path
for iter in range(self.max_iter):
rnd_node = self.sampler()
# Find nearest node to the sampled point
distance_list = [
compute_distance(node.position, rnd_node.position)
for node in node_list
]
try:
# for python2
nearest_node_index = min(xrange(len(distance_list)),
key=distance_list.__getitem__)
except NameError:
# for python 3
nearest_node_index = distance_list.index(min(distance_list))
nearest_node = node_list[nearest_node_index]
# Create new point in the direction of sampled point
theta = math.atan2(
rnd_node.position.y - nearest_node.position.y,
rnd_node.position.x - nearest_node.position.x,
)
new_node = RobotState(position=Point(
nearest_node.position.x + self.exp_dis * math.cos(theta),
nearest_node.position.y + self.exp_dis * math.sin(theta),
))
# Check whether new point is inside an obstacles
trj = Trajectory([nearest_node, new_node])
if not env.get_status(new_node) or not env.get_traj_status(trj):
continue
else:
node_list.append(new_node)
# FINDING NEAREST INDICES
nnode = len(node_list) + 1
# The circle in which to check parent node and rewiring
r = self.circle * math.sqrt((math.log(nnode) / nnode))
dist_list = [
compute_distance(node.position, new_node.position)
for node in node_list
]
# Getting all the indexes within r units of new_node
near_inds = [dist_list.index(i) for i in dist_list if i <= r**2]
graph.add_node(new_node)
graph.add_edge(nearest_node, new_node)
# Add all the collision free edges to the graph.
for ind in near_inds:
node_check = node_list[ind]
trj = Trajectory([new_node, node_check])
# If the above condition is met append the edge.
if env.get_traj_status(trj):
graph.add_edge(new_node, node_check)
if end not in graph.nodes:
min_cost = 2.0
closest_node = RobotState()
for node in graph.nodes:
temp = compute_distance(node.position, end.position)
if env.get_traj_status(Trajectory([node, end
])) and temp < min_cost:
min_cost = temp
closest_node = node
graph.add_edge(closest_node, end)
graph.add_node(end)
if start in graph.edges[end]:
graph.del_edge(start, end)
path = []
p = astar(graph, start, end)
path.extend(p.path)
if len(path) != 1:
print("Goal Reached!")
path = Trajectory(path)
if len(path.path) == 1:
raise PathNotFound(path, message="Path contains only one state")
return path, {}
def plan(self, start, goal, env):
"""Path planning method
Args:
start (gennav.utils.common.RobotState): Starting state.
goal (gennav.utils.common.RobotState): Destination state
env (gennav.envs.Environment): Environment object
Returns:
gennav.utils.Trajectory: The path as a Trajectory
dict: Dictionary containing additional information like the node_list
"""
# Check if start and goal states are obstacle free
if not env.get_status(start):
raise InvalidStartState(start, message="Start state is in obstacle.")
if not env.get_status(goal):
raise InvalidGoalState(goal, message="Goal state is in obstacle.")
# Initialize start and goal nodes.
x_start = Node(state=start)
x_goal = Node(state=goal)
# Starting the tree:
node_list = [x_start]
# X_soln contains all the nodes near the goal
X_soln = []
for i in range(self.max_iter):
# defining the best cost as cbest
if len(X_soln) == 0:
cbest = float("inf")
else:
cbest = X_soln[0].cost + compute_distance(
X_soln[0].state.position, x_goal.state.position
)
for node in X_soln:
if (
node.cost
+ compute_distance(node.state.position, x_goal.state.position)
< cbest
):
cbest = node.cost + compute_distance(
node.state.position, x_goal.state.position
)
# sample the new random point using the sample function
x_rand = self.rectangular_sampler()
x_rand_node = Node(state=x_rand)
# Finding the nearest node to the random point
distance_list = [
compute_distance(node.state.position, x_rand_node.state.position)
for node in node_list
]
min_dist = distance_list[0]
min_index = 0
for index, distance in enumerate(distance_list):
if distance < min_dist:
min_dist = distance
min_index = index
x_nearest = node_list[min_index]
# Steering at a distance of self.expand_dis
theta = compute_angle(x_nearest.state.position, x_rand_node.state.position)
x = x_nearest.state.position.x + self.expand_dis * math.cos(theta)
y = x_nearest.state.position.y + self.expand_dis * math.sin(theta)
t = RobotState(position=Point(x, y))
x_new = Node(state=t)
# Defining a temporary trajectory between the new node and its nearest node
traj = Trajectory(path=[x_nearest.state, x_new.state])
# Checking whether new node and the nearest node can be connected
if env.get_traj_status(traj):
# Adding new node to the node list
node_list.append(x_new)
# Defining the neighbours of x_new
X_near = []
for node in node_list:
if (compute_distance(node.state.position, x_new.state.position)) < (
self.neighbourhood_radius
) and ( # If it is inside the neighbourhood radius
compute_distance(node.state.position, x_new.state.position) != 0
):
X_near.append(node)
x_min = x_nearest
c_min = x_min.cost + self.expand_dis
for x_near in X_near:
c_new = x_near.cost + compute_distance(
x_near.state.position, x_new.state.position
)
if c_new < c_min:
traj = Trajectory(path=[x_near.state, x_new.state])
if env.get_traj_status(traj):
x_min = x_near
c_min = c_new
# Defining the parent node and cost of x_new
x_new.parent = x_min
x_new.cost = c_min
# Rewiring
for x_near in X_near:
c_near = x_near.cost
c_new = x_new.cost + compute_distance(
x_near.state.position, x_new.state.position
)
if c_new < c_near:
traj = Trajectory(path=[x_new.state, x_near.state])
if env.get_traj_status(traj):
x_near.parent = x_new
x_near.cost = c_new
goal_traj = Trajectory(path=[x_new.state, x_goal.state])
if (
compute_distance(x_goal.state.position, x_new.state.position)
) < self.goal_distance and env.get_traj_status(goal_traj):
X_soln.append(x_new)
# X_soln contains all the solutions
if len(X_soln) == 0:
print("No solution found!")
path = []
path.append(x_start.state)
traj = Trajectory(path=path)
info_dict = {}
info_dict["node_list"] = node_list
if len(traj.path) == 1:
raise PathNotFound(path, message="Path contains only one state")
return (traj, info_dict)
else:
print("Goal reached!")
Cbest = X_soln[0].cost
Xbest = X_soln[0]
node_list.append(x_goal)
for node in X_soln:
if node.cost < Cbest:
Cbest = node.cost
Xbest = node
x_goal.parent = Xbest
# Xbest is the optimum solution to the problem.
# Back tracing the path:
path = []
path.append(x_goal.state)
path.append(Xbest.state)
p = Xbest.parent
while p is not None:
path.append(p.state)
p = p.parent
path.append(x_start.state)
path.reverse()
# print("No. of solutions " + str(len(X_soln)))
trajectory = Trajectory(path=path)
info_dict = {}
info_dict["node_list"] = node_list
return (trajectory, info_dict)
def plan(self, start, goal, env):
"""Plans path from start to goal avoiding obstacles.
Args:
start_point(gennav.utils.RobotState): with start point coordinates.
end_point (gennav.utils.RobotState): with end point coordinates.
env: (gennav.envs.Environment) Base class for an envrionment.
Returns:
gennav.utils.Trajectory: The planned path as trajectory
dict: Dictionary containing additional information like the node_list
"""
# Check if start and goal states are obstacle free
if not env.get_status(start):
raise InvalidStartState(start,
message="Start state is in obstacle.")
if not env.get_status(goal):
raise InvalidGoalState(goal, message="Goal state is in obstacle.")
# Initialize start and goal nodes
start_node = Node(state=start)
goal_node = Node(state=goal)
# Initialize node_list with start
node_list = [start_node]
# Loop to keep expanding the tree towards goal if there is no direct connection
traj = Trajectory(path=[start, goal])
if not env.get_traj_status(traj):
while True:
# Sample random state in specified area
rnd_state = self.sampler()
# Find nearest node to the sampled point
distance_list = [
compute_distance(node.state.position, rnd_state.position)
for node in node_list
]
try:
# for python2
nearest_node_index = min(xrange(len(distance_list)),
key=distance_list.__getitem__)
except NameError:
# for python 3
nearest_node_index = distance_list.index(
min(distance_list))
nearest_node = node_list[nearest_node_index]
# Create new point in the direction of sampled point
theta = math.atan2(
rnd_state.position.y - nearest_node.state.position.y,
rnd_state.position.x - nearest_node.state.position.x,
)
new_point = Point()
new_point.x = (nearest_node.state.position.x +
self.expand_dis * math.cos(theta))
new_point.y = (nearest_node.state.position.y +
self.expand_dis * math.sin(theta))
# Check whether new point is inside an obstacles
if not env.get_status(RobotState(position=new_point)):
continue
else:
new_node = Node.from_coordinates(new_point)
new_node.parent = nearest_node
node_list.append(new_node)
# Check if goal has been reached or if there is direct connection to goal
distance_to_goal = compute_distance(goal_node.state.position,
new_node.state.position)
traj = Trajectory(path=[RobotState(position=new_point), goal])
if distance_to_goal < self.expand_dis or env.get_traj_status(
traj):
goal_node.parent = node_list[-1]
node_list.append(goal_node)
print("Goal reached!")
break
else:
goal_node.parent = start_node
node_list = [start_node, goal_node]
# Construct path by traversing backwards through the tree
path = []
last_node = node_list[-1]
while last_node.parent is not None:
path.append(last_node.state)
last_node = last_node.parent
path.append(start)
info_dict = {}
info_dict["node_list"] = node_list
path = Trajectory(path[::-1])
if len(path.path) == 1:
raise PathNotFound(path, message="Path contains only one state")
return (path, info_dict)
