def PRM(self,
environment,
bounds,
start_pose,
goal_region,
object_radius,
resolution=3,
isLAzy=True,
drawResults=False):
"""Returns a path from the start_pose to the goal region in the current environment using PRM.
Args:
environment (A yaml environment): Environment where the planner will be run. Includes obstacles.
bounds( (int int int int) ): min x, min y, max x, and max y coordinates of the bounds of the world.
start_pose( (float float) ): Starting x and y coordinates of the object in question.
goal_region (Polygon): A polygon representing the region that we want our object to go to.
object_radius (float): Radius of the object.
resolution (int): Number of segments used to approximate a quarter circle around a point.
isLazy (bool): Optional, if True it adds edges to the graph regardless of whether it collides with an obstacle and only checks for collisions when building the path.
drawResults (bool): Optional, if set to True it plots the path and enviornment using a matplotlib plot.
Returns:
path (list<(int,int)>): A list of tuples/coordinates representing the nodes in a path from start to the goal region
self.V (set<(int,int)>): A set of Vertices (coordinates) of nodes in the graph.
self.E (set<(int,int),(int,int)>): A set of Edges connecting one node to another node in the graph.
"""
start_time = time.time()
path, V, E = self.PRMSolver.path(environment, bounds, start_pose,
goal_region, object_radius,
resolution, isLAzy)
elapsed_time = time.time() - start_time
if path and drawResults:
draw_results("PRM", path, V, E, environment, bounds, object_radius,
resolution, start_pose, goal_region, elapsed_time)
return path, V, E
def InformedRRTStar(self,
environment,
bounds,
start_pose,
goal_region,
object_radius,
steer_distance,
num_iterations,
resolution=3,
drawResults=False):
start_time = time.time()
path, V, E = self.RRTFamilySolver.path(environment,
bounds,
start_pose,
goal_region,
object_radius,
steer_distance,
num_iterations,
resolution,
runForFullIterations=True,
RRT_Flavour="InformedRRT*")
elapsed_time = time.time() - start_time
if path and drawResults:
draw_results("InformedRRT*", path, V, E, environment, bounds,
object_radius, resolution, start_pose, goal_region,
elapsed_time)
return path, V, E
def RRT(self, environment, bounds, start_pose, goal_region, object_radius, \
steer_distance, num_iterations, resolution=3, runForFullIterations=False, drawResults=False):
"""Returns a path from the start_pose to the goal region in the current environment using RRT.
Args:
environment (A yaml environment): Environment where the planner will run. Includes obstacles.
bounds( (int int int int) ): min x, min y, max x, and max y coordinates of the bounds of the world.
start_pose( (float float) ): Starting x and y coordinates of the object in question.
goal_region (Polygon): A polygon representing the region that we want our object to go to.
object_radius (float): Radius of the object.
steer_distance (float): Limits the length of the branches.
num_iterations (int): How many points are sampled for the creating of the tree.
resolution (int): Number of segments used to appromixate a quater circle around a point.
runForFullIterations (bool): Optional, if True return the first path found without having to sample all num_iterations points.
drawResults (bool): Optional, if set to True it plots the path and environment using a matplotlib plot.
Returns:
path (list<(int, int)>): A list of tuples/coordinates representing the nodes in a path from start to the goal region.
self.V (set<(int, int)>): A list of Vertices (coordinates) of nodes in the tree.
self.E (set<(int, int),(int, int)>): A set of Edges connecting one node to another node in the tree.
"""
start_time = time.time()
path, V, E = self.RRTFamilySolver.path(environment, bounds, start_pose, goal_region, \
object_radius, steer_distance, num_iterations, \
resolution, runForFullIterations, RRT_Flavour="RRT")
elapsed_time = time.time() - start_time
if path and drawResults:
draw_results("RRT", path, V, E, environment, bounds, object_radius,
resolution, star_pose, goal_region, elapsed_time)
return path, V, E
def RRT(self, environment, bounds, start_pose, goal_region, object_radius, steer_distance, num_iterations, resolution, drawResults, runForFullIterations, RRT_Flavour= "RRT"):
"""Returns a path from the start_pose to the goal region in the current environment using the specified RRT-variant algorithm.
Args:
environment (A yaml environment): Environment where the planner will be run. Includes obstacles.
bounds( (int int int int) ): min x, min y, max x, and max y coordinates of the bounds of the world.
start_pose( (float float) ): Starting x and y coordinates of the object in question.
goal_region (Polygon): A polygon representing the region that we want our object to go to.
object_radius (float): Radius of the object.
steer_distance (float): Limits the length of the branches
num_iterations (int): How many points are sampled for the creationg of the tree
resolution (int): Number of segments used to approximate a quarter circle around a point.
runForFullIterations (bool): If True RRT and RRTStar return the first path found without having to sample all num_iterations points.
RRT_Flavour (str): A string representing what type of algorithm to use.
Options are 'RRT', 'RRT*', and 'InformedRRT*'. Anything else returns None,None,None.
Returns:
path (list<(int,int)>): A list of tuples/coordinates representing the nodes in a path from start to the goal region
self.V (set<(int,int)>): A set of Vertices (coordinates) of nodes in the tree
self.E (set<(int,int),(int,int)>): A set of Edges connecting one node to another node in the tree
"""
self.env = environment
self.initialise(environment, bounds, start_pose, goal_region, object_radius, steer_distance, num_iterations, resolution, runForFullIterations)
# Define start and goal in terms of coordinates. The goal is the centroid of the goal polygon.
x0, y0 = start_pose
x1, y1 = goal_region.centroid.coords[0]
start = (x0, y0)
goal = (x1, y1)
elapsed_time=0
path=[]
# Handle edge case where where the start is already at the goal
if start == goal:
path = [start, goal]
self.V.union([start, goal])
self.E.union([(start, goal)])
# There might also be a straight path to goal, consider this case before invoking algorithm
elif self.isEdgeCollisionFree(start, goal):
path = [start, goal]
self.V.union([start, goal])
self.E.union([(start, goal)])
# Run the appropriate RRT algorithm according to RRT_Flavour
else:
if RRT_Flavour == "RRT":
start_time = time.time()
path = self.RRTSearch()
elapsed_time = time.time() - start_time
if path and drawResults:
draw_results("RRT", path, self.V, self.E, environment, bounds, object_radius, resolution, start_pose, goal_region, elapsed_time)
return path
def PRM(self,
environment,
bounds,
start_pose,
goal_region,
object_radius,
resolution=3,
isLAzy=True,
drawResults=False):
start_time = time.time()
path, V, E = self.PRMSolver.path(environment, bounds, start_pose,
goal_region, object_radius,
resolution, isLAzy)
elapsed_time = time.time() - start_time
if path and drawResults:
draw_results("PRM", path, V, E, environment, bounds, object_radius,
resolution, start_pose, goal_region, elapsed_time)
return path, V, E