def test_dstar():
obstacles = [[(8, 5), (7, 8), (2, 9), (3, 5)],
[(3, 3), (3, 5), (5, 5), (5, 3)]]
sampler = UniformRectSampler(-5, 15, -5, 15)
poly = PolygonEnv()
start = RobotState(position=Point(0, 0))
goal = RobotState(position=Point(12, 10))
my_tree = DStar(sampler=sampler, r=3, n=75)
poly.update(obstacles)
path = my_tree.plan(start, goal, poly)
from gennav.envs.common import visualize_path
visualize_path(path, poly)
obstacles = [
[(8, 5), (7, 8), (2, 9), (3, 5)],
[(3, 3), (3, 5), (5, 5), (5, 3)],
[(10, 8), (12, 8), (11, 6)],
]
# obstacles = [[(10, 7), (9, 10), (4, 11), (5, 7)], [(5, 5), (5, 7), (7, 7), (7, 5)],[(9,8),(13,8),(11,5)]]
poly.update(obstacles)
path_new = my_tree.replan(start, goal, poly)
from gennav.envs.common import visualize_path
visualize_path(path_new, poly)
def test_prm_plan():
general_obstacles_list = [
[[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]],
[
[(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)],
[(4, 0), (4, 5), (5, 5), (5, 0)],
[(8, 2), (8, 7), (10, 7), (10, 2)],
],
]
sampler = UniformRectSampler(-5, 15, -5, 15)
poly = PolygonEnv(buffer_dist=0.1)
start = RobotState(position=Point(0, 0))
goal = RobotState(position=Point(12, 10))
my_tree = PRM(sampler=sampler, r=5, n=100)
for obstacles in general_obstacles_list:
poly.update(obstacles)
path, _ = my_tree.plan(start, goal, poly)
# from gennav.envs.common import visualize_path
# visualize_path(path, poly)
if len(path.path) != 1:
assert poly.get_traj_status(path) is True
def test_rrt():
general_obstacles_list = [
[[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]],
[
[(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)],
[(4, 0), (4, 5), (5, 5), (5, 0)],
[(8, 2), (8, 7), (10, 7), (10, 2)],
],
]
poly = PolygonEnv()
# poly.update(general_obstacles_list)
for obstacles in general_obstacles_list:
poly.update(obstacles)
# Instatiate rrt planner object
my_tree = RRT(sample_area=(-5, 15), sampler=sampler, expand_dis=0.1)
start = RobotState(position=Point(1, 1))
goal = RobotState(position=Point(10, 10))
path = my_tree.plan(start, goal, poly)
# from gennav.envs.common import visualize_path
# visualize_path(path, poly)
assert poly.get_traj_status(path) is True
def prmstar_plan():
general_obstacles_list = [
[[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]],
[
[(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)],
[(4, 0), (4, 5), (5, 5), (5, 0)],
[(8, 2), (8, 7), (10, 7), (10, 2)],
],
]
sampler = UniformRectSampler(-5, 15, -5, 15)
polygon = PolygonEnv()
start = RobotState(position=Point(0, 0))
goal = RobotState(position=Point(12, 10))
my_prmstar = PRMStar(sampler=sampler, c=30, n=100)
# Plan a path using the PRMStar.plan method for each obstacle:
for obstacles in general_obstacles_list:
polygon.update(obstacles) # updates the environment with the obstacle
path = my_prmstar.plan(start, goal, polygon)
visualize_path(path, polygon)
if len(path.path) != 1: # check if the path has only the start state
assert polygon.get_traj_status(path) is True
def test_rrtstar():
general_obstacles_list = [
[
(1, 1),
(2, 1),
(2, 2),
(1, 2),
],
[(3, 4), (4, 4), (4, 5), (3, 5)],
]
poly = PolygonEnv(buffer_dist=0.1)
poly.update(general_obstacles_list)
sampler = UniformRectSampler(0, 6, 0, 6)
my_tree = RRTstar(
sampler=sampler,
expand_dis=0.1,
neighbourhood_radius=0.5,
goal_distance=0.5,
max_iter=2000,
)
start = RobotState(position=Point(0, 0))
goal = RobotState(position=Point(5, 5))
path, _ = my_tree.plan(start, goal, poly)
# from gennav.utils.visualisation import visualize_node
# node_list = _['node_list']
# visualize_node(node_list, poly)
# from gennav.envs.common import visualize_path
# visualize_path(path, poly)
if len(path.path) != 1:
assert poly.get_traj_status(path) is True
def test_rrt():
general_obstacles_list = [
[[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]],
[
[(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)],
[(4, 0), (4, 5), (5, 5), (5, 0)],
[(8, 2), (8, 7), (10, 7), (10, 2)],
],
]
sampler = UniformRectSampler(-5, 15, -5, 15)
poly = PolygonEnv(buffer_dist=0.5)
my_tree = RRT(sampler=sampler, expand_dis=0.1)
start = RobotState(position=Point(1, 1))
goal = RobotState(position=Point(10, 10))
for obstacles in general_obstacles_list:
poly.update(obstacles)
path, _ = my_tree.plan(start, goal, poly)
# from gennav.utils.visualisation import visualize_node
# node_list = _['node_list']
# visualize_node(node_list, poly)
# from gennav.envs.common import visualize_path
# visualize_path(path, poly)
assert poly.get_traj_status(path) is True
def test_potential_field():
obstacle_list = [[(1, 1), (1.5, 1.5), (1.5, 1)]]
env = PolygonEnv(buffer_dist=0.1)
env.update(obstacle_list)
my_planner = PotentialField(KP=5, ETA=100, THRESH=15, STEP_SIZE=0.1, error=0.2)
start = RobotState(position=Point(0, 0))
goal = RobotState(position=Point(7, 7))
path, _ = my_planner.plan(start, goal, env)
assert env.get_traj_status(path) is True
def uniform_random_sampler(sample_area):
"""
Randomly sample point in sample area
Args:
sample_area(tuple): area to sample point in (min and max)
Return:
Randomly selected point as a Point(gennav.utils.geometry.Point).
"""
new_point = Point()
new_point.x = random.uniform(sample_area[0], sample_area[1])
new_point.y = random.uniform(sample_area[0], sample_area[1])
return new_point
def __call__(self):
"""
Randomly sample point in area while sampling goal point at a specified rate.
Return:
gennav.utils.RobotState: The sampled robot state.
"""
if random.random() > self.goal_sample_p:
new_point = Point()
new_point.x = random.uniform(self.min_x, self.max_x)
new_point.y = random.uniform(self.min_y, self.max_y)
return RobotState(position=new_point)
else:
return self.goal
def __call__(self):
"""Randomly sample point in area while sampling goal point at a specified rate.
Return:
gennav.utils.RobotState: The sampled robot state.
"""
if random.random() > self.goal_sample_p:
theta = 2 * pi * random.random()
u = random.random() * self.r
new_point = Point()
new_point.x = self.centre.x + u * cos(theta)
new_point.y = self.centre.y + u * sin(theta)
return RobotState(position=new_point)
else:
return self.goal
def transform(self, frame1, frame2, rsf1):
"""Transform robotPose from one pose to the other
Args:
frame1 (string) : from the frame (world, bot, map)
frame2 (string) : to the frame (world, bot, map)
rsf1 (gennav.utils.common.RobotState) : RobotState in frame1
Returns:
rsf2 (gennav.utils.common.RobotState) : RobotState in frame2
"""
# TODO: Make it more robust in terms of checking frames
# Check if the required trnasform or the inverse of the transform exists
frame = frame2 + "T" + frame1
frame_inv = frame1 + "T" + frame2
if frame in self.transforms.keys():
t_matrix = self.transforms[frame]["transform"]
elif frame_inv in self.transforms.keys():
t_matrix = np.linalg.inv(self.transforms[frame_inv]["transform"])
else:
raise Exception("Transform for the frames not found")
# Transform using matrix multiplication
pf2 = np.dot(
t_matrix,
np.array([rsf1.position.x, rsf1.position.y, 1]).reshape(3, 1))
rsf2 = RobotState(position=Point(pf2[0].item(), pf2[1].item()))
# Return RobotState
return rsf2
def fillOccupancy(self):
"""Function that fill the occupnacy grid on every update
Assumptions:
1. RobotPose is considered (0, 0, 0) to accomodate the laser scan, which produces ranges wrt to the bot
2. The RobotPose in the occupancy grid is (X * scale_factor/2, Y * scale_factor /2, 0)
3. The attribute robotPose is the real pose of the robot wrt to the world Frame,
thus it helps us to calculate the transform for trajectory and pose validity queries
"""
self.grid[:] = 0
ang_min, ang_max, ranges = self.scan
angle_step = (ang_max - ang_min) / len(ranges)
for i, rng in enumerate(ranges):
# Check for obstacles
if np.abs(rng) is not np.inf:
x, y = (
rng * np.cos(ang_min + i * angle_step),
rng * np.sin(ang_max + i * angle_step),
)
newState = self.transform("bot", "map",
RobotState(Point(x, y, 0)))
x_, y_ = newState.position.x, newState.position.y
# Checking if the range is within the grid, to mark them as occupied
if 0 <= x_ < self.grid.shape[0] and 0 <= y_ < self.grid.shape[
1]:
if self.grid[int(x_)][int(-y_ - 1)] != 1:
self.grid[int(x_)][int(-y_ - 1)] = 1
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 grad_repulsive(self):
"""Calculates the gradient due to the repulsive component of the potential field, i.e, the potential field created by the obstacles
Args:
None
Returns:
total_grad (float) : Total gradient due to potential field of all obstacles at the state of the robot
"""
total_grad = [0.0, 0.0]
min_dist = self.env.minimum_distances(self.current)
for i, _ in enumerate(self.env.obstacle_list):
if min_dist[i] < self.THRESH:
dummy_state_x = RobotState(
position=Point(
self.current.position.x + 0.01,
self.current.position.y,
self.current.position.z,
)
)
x_grad = self.env.minimum_distances(dummy_state_x)[i]
dummy_state_y = RobotState(
position=Point(
self.current.position.x,
self.current.position.y + 0.01,
self.current.position.z,
)
)
y_grad = self.env.minimum_distances(dummy_state_y)[i]
grad = [
(x_grad - min_dist[i]) / 0.01,
(y_grad - min_dist[i]) / 0.01,
]
factor = (
self.ETA
* ((1 / self.THRESH) - 1 / min_dist[i])
* 1
/ (min_dist[i] ** 2 + 1)
)
grad = [x * factor for x in grad]
total_grad[0] = total_grad[0] + grad[0]
total_grad[1] = total_grad[1] + grad[1]
return total_grad
def __init__(self, radius, centre=Point(), goal=None, goal_sample_p=0):
super(UniformCircularSampler, self).__init__()
self.centre = centre
self.r = radius
self.goal = goal
self.goal_sample_p = goal_sample_p
if goal is None and goal_sample_p > 0:
raise SamplingFailed(
goal, message="goal must be specified if goal_sample_p > 0")
def uniform_random_circular_sampler(r):
"""Randomly samples point in a circular region of radius r around the origin.
Args:
r: radius of the circle.
Return:
Randomly selected point as Point(gennav.utils.geometry.Point).
"""
# Generate a random angle theta.
theta = 2 * pi * random.random()
u = random.random() + random.random()
if u > 1:
a = 2 - u
else:
a = u
new_point = Point()
new_point.x = r * a * cos(theta)
new_point.y = r * a * sin(theta)
return new_point
def uniform_adjustable_random_sampler(sample_area, goal, goal_sample_rate):
"""
Randomly sample point in area while sampling goal point at a specified rate.
Args:
sample_area(tuple): area to sample point in (min and max)
goal(gennav.utils.geometry.Point):Point representing goal point.
goal_sample_rate: number between 0 and 1 specifying how often
to sample the goal point.
Return:
Randomly selected point as a Point(gennav.utils.geometry.Point).
"""
if random.random() > goal_sample_rate:
new_point = Point()
new_point.x = random.uniform(sample_area[0], sample_area[1])
new_point.y = random.uniform(sample_area[0], sample_area[1])
return new_point
else:
return goal
def test_astar_heuristic():
graph = Graph()
node1 = RobotState(position=Point(0, 0))
node2 = RobotState(position=Point(1, 1))
node3 = RobotState(position=Point(3, 1))
node4 = RobotState(position=Point(2, 2))
node5 = RobotState(position=Point(3, 2))
node6 = RobotState(position=Point(2, 3))
graph.add_node(node1)
graph.add_node(node2)
graph.add_node(node3)
graph.add_node(node4)
graph.add_node(node5)
graph.add_node(node6)
graph.add_edge(node1, node2)
graph.add_edge(node2, node3)
graph.add_edge(node2, node4)
graph.add_edge(node4, node5)
graph.add_edge(node4, node6)
graph.add_edge(node5, node6)
heuristic = {
node1: 5,
node2: 3,
node3: 1,
node4: 1,
node5: 0,
node6: 2,
}
start = node1
end = node5
_ = astar(graph, start, end, heuristic)
print(_.path)
def test_astar():
graph = Graph()
node1 = RobotState(position=Point(0, 0))
node2 = RobotState(position=Point(1, 1))
node3 = RobotState(position=Point(3, 1))
node4 = RobotState(position=Point(2, 2))
node5 = RobotState(position=Point(3, 2))
node6 = RobotState(position=Point(2, 3))
graph.add_node(node1)
graph.add_node(node2)
graph.add_node(node3)
graph.add_node(node4)
graph.add_node(node5)
graph.add_node(node6)
graph.add_edge(node1, node2)
graph.add_edge(node2, node3)
graph.add_edge(node2, node4)
graph.add_edge(node4, node5)
graph.add_edge(node4, node6)
graph.add_edge(node5, node6)
start = node1
end = node5
_ = astar(graph, start, end)
print(_.path)
def test_prm_plan():
general_obstacles_list = [
[[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]],
[
[(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)],
[(4, 0), (4, 5), (5, 5), (5, 0)],
[(8, 2), (8, 7), (10, 7), (10, 2)],
],
]
poly = PolygonEnv()
for obstacles in general_obstacles_list:
poly.update(obstacles)
# Instatiate prm constructer object
start = RobotState(position=Point(0, 0))
goal = RobotState(position=Point(12, 10))
my_tree = PRM(sample_area=(-5, 15), sampler=sampler, r=5, n=100)
path = my_tree.plan(start, goal, poly)
# from gennav.envs.common import visualize_path
# visualize_path(path, poly)
assert poly.get_traj_status(path) is True
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)
raise NotImplementedError("Specified controller ", env_name,
" is not implemented")
# Get message type object
msg_dtype_name = param_dict["msg_dtype_name"]
if msg_dtype_name in msg_dtype_registry.keys():
msg_dtype = msg_dtype_registry[msg_dtype_name]
else:
raise NotImplementedError("Specified message type ", planner_name,
" is not compatible")
# Construct controller
controller_node = gennav_ros.Controller(controller)
# Construct commander
replan_interval = rospy.Duration(param_dict["replan_interval"])
commander_node = gennav_ros.Commander(
planner=planner,
env=env,
replan_interval=replan_interval,
msg_dtype=msg_dtype,
)
# Execute command
goal_state = RobotState(position=Point(*goal))
start_state = RobotState(position=Point(*start))
commander_node.goto(goal_state, start_state)
# ROS Magic (spin)
rospy.spin()
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 test_astar_heuristic():
graph = {
Point(0, 0): [Point(1, 1)],
Point(1, 1): [Point(0, 0), Point(3, 1),
Point(2, 2)],
Point(3, 1): [Point(1, 1)],
Point(2, 2): [Point(1, 1), Point(3, 2),
Point(2, 3)],
Point(3, 2): [Point(2, 2), Point(2, 3)],
Point(2, 3): [Point(2, 2), Point(3, 2)],
}
heuristic = {
Point(0, 0): 5,
Point(1, 1): 3,
Point(3, 1): 1,
Point(2, 2): 1,
Point(3, 2): 0,
Point(2, 3): 2,
}
start = Point(0, 0)
end = Point(3, 2)
_ = astar(graph, start, end, heuristic)
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):
"""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
"""
# 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 point in specified area
rnd_point = self.sampler(self.sample_area, goal.position,
self.goal_sample_rate)
# Find nearest node to the sampled point
distance_list = [(node.state.position.x - rnd_point.x)**2 +
(node.state.position.y - rnd_point.y)**2 +
(node.state.position.z - rnd_point.z)**2
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_point.y - nearest_node.state.position.y,
rnd_point.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
del_x, del_y, del_z = (
new_node.state.position.x - goal_node.state.position.x,
new_node.state.position.y - goal_node.state.position.y,
new_node.state.position.z - goal_node.state.position.z,
)
distance_to_goal = math.sqrt(del_x**2 + del_y**2 + del_z**2)
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 node_list[node_list.index(last_node)].parent is not None:
node = node_list[node_list.index(last_node)]
path.append(RobotState(position=node.state.position))
last_node = node.parent
path.append(start)
path = Trajectory(list(reversed(path)))
return path
def test_astar():
graph = {
Point(0, 0): [Point(1, 1)],
Point(1, 1): [Point(0, 0), Point(3, 1),
Point(2, 2)],
Point(3, 1): [Point(1, 1)],
Point(2, 2): [Point(1, 1), Point(3, 2),
Point(2, 3)],
Point(3, 2): [Point(2, 2), Point(2, 3)],
Point(2, 3): [Point(2, 2), Point(3, 2)],
}
start = Point(0, 0)
end = Point(3, 2)
_ = astar(graph, start, end)
UniformRectSampler, )
general_obstacles_list = [
[[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]],
[
[(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)],
[(4, 0), (4, 5), (5, 5), (5, 0)],
[(8, 2), (8, 7), (10, 7), (10, 2)],
],
]
# A list of obstacles for the planner.
sampler = UniformRectSampler(-5, 15, -5, 15) # for uniformly sampling points
poly = PolygonEnv(
buffer_dist=1.0
) # intializing the environment object needed for updating the obstacles of the environment
my_tree = RRG(sampler=sampler, expand_dis=1.0,
max_iter=500) # initializing RRG planner object
start = RobotState(position=Point(0, 0)) # setting start point to (0,0)
goal = RobotState(position=Point(10, 10)) # setting end point to (0,0)
for obstacles in general_obstacles_list: # updating the environment with all obstacles
poly.update(obstacles)
path, _ = my_tree.plan(start, goal, poly) # planning the path
visualize_path(path, poly) # visualizing path and obstacles