#!/usr/bin/env python3 # # prm_mattress.py # import bisect import math import matplotlib.pyplot as plt import numpy as np import random from planarutils import PointInTriangle from planarutils import SegmentCrossTriangle from planarutils import SegmentNearSegment # # General World Definitions # # List of objects, start, and goal # (xmin, xmax) = (0, 30) (ymin, ymax) = (0, 20) xw1 = 12 xw2 = 15 xw3 = 18 xw4 = 21 yw1 = 10 yw2 = 12 walls = (((xmin, ymin), (xmax, ymin)), ((xmax, ymin), (xmax, ymax)), ((xmax, ymax), (xmin, ymax)), ((xmin, ymax), (xmin, ymin)), ((xmin, yw1), ( xw2, yw1)), (( xw3, yw1), (xmax, yw1)), (( xw3, yw2), ( xw4, yw2)), (( xw1, yw2), ( xw2, yw2)), (( xw2, yw2), ( xw2, ymax))) (startx, starty, startt) = ( 5, 15, 0.5*math.pi) (goalx, goaly, goalt) = (10, 5, 0.0*math.pi) L = 6 D = 1/2 N = 300 K = 15 K2 = 3*K # # Visualization Tools # class Visual(): def StartFigure(): # Clear the current, or create a new figure. plt.clf() # Create a new axes, enable the grid, and set axis limits. plt.axes() plt.grid(True) plt.gca().axis('on') plt.gca().set_xlim(xmin, xmax) plt.gca().set_ylim(ymin, ymax) plt.gca().set_xticks([xmin, xw1, xw2, xw3, xw4, xmax]) plt.gca().set_yticks([ymin, yw1, yw2, ymax]) plt.gca().set_aspect('equal') # Show the walls. for wall in walls: plt.plot([wall[0][0], wall[1][0]], [wall[0][1], wall[1][1]], 'k', linewidth=2) plt.plot([xw3, xw4], [yw2, yw2], 'b:', linewidth=3) def FlushFigure(): # Show the plot. plt.pause(0.001) def ShowFigure(): # Flush the figure and wait. Visual.FlushFigure() input("Hit return to continue") def DrawState(s, *args, **kwargs): plt.plot([s.x1, s.x2], [s.y1, s.y2], *args, **kwargs) def DrawLocalPath(head, tail, *args, **kwargs): n = math.ceil(head.Distance(tail) / D) for i in range(n+1): Visual.DrawState(head.Intermediate(tail, i/n), *args, **kwargs) # # Object State # def AngleDiff(t1, t2): return (t1-t2) - math.pi * round((t1-t2)/math.pi) class State: def __init__(self, x, y, t): # Remember the centroid position and angle theta. self.x = x self.y = y self.t = t # Pre-compute the endpoint positions. self.x1 = x + L/2 * math.cos(t) self.y1 = y + L/2 * math.sin(t) self.x2 = x - L/2 * math.cos(t) self.y2 = y - L/2 * math.sin(t) # Compute/create an intermediate state. This can be useful if you # need to check the local planner by testing intermediate states. def Intermediate(self, other, alpha): return State(self.x + alpha * (other.x - self.x), self.y + alpha * (other.y - self.y), self.t + alpha * AngleDiff(other.t, self.t)) # In case we want to print the state. def __repr__(self): return ("" % (self.x, self.y, self.t)) #### These are necessary for the PRM: # Check whether in free space. def InFreespace(self): for wall in walls: if SegmentNearSegment(D, (self.x1, self.y1), (self.x2, self.y2), wall[0], wall[1]): return False return True # Compute the relative distance to another state. def Distance(self, other): return math.sqrt((self.x - other.x)**2 + (self.y - other.y)**2 + (L*AngleDiff(self.t, other.t))**2) # Check the local planner - whether this connects to another state. def ConnectsTo(self, other): n = math.ceil(self.Distance(other) / D) for i in range(1,n): if not self.Intermediate(other, i/n).InFreespace(): return False return True # # PRM Graph and A* Search Tree # class Node(State): def __init__(self, *args): # Initialize the state super().__init__(*args) # List of neighbors (for the graph) self.neighbors = [] # Parent and costs (for A* search tree) self.seen = False self.done = False self.parent = [] self.costToReach = 0 self.costToGoEst = 0 self.cost = self.costToReach + self.costToGoEst # Define the "less-than" to enable sorting by cost in A*. def __lt__(self, other): return self.cost < other.cost # Estimate the cost to go to another node, for use in A*. def CostToGoEst(self, other): return self.Distance(other) # # A* Planning Algorithm # def AStar(nodeList, start, goal): # Prepare the still empty *sorted* on-deck queue. onDeck = [] # Clear the search tree (to keep track). for n in nodeList: n.seen = False n.done = False # Begin with the start state on-deck. start.done = False start.seen = True start.parent = None start.costToReach = 0 start.costToGoEst = start.CostToGoEst(goal) start.cost = start.costToReach + start.costToGoEst bisect.insort(onDeck, start) # Continually expand/build the search tree. while True: # Make sure we still have something to look at! if not (len(onDeck) > 0): return [] # Grab the next node (first on deck). n = onDeck.pop(0) # Check whether we have found the goal. if (n == goal): break # Add the children to the on-deck queue. for c in n.neighbors: # Skip if already done. if c.done: continue # Check the cost for the new path to reach the child. costToReach = n.costToReach + n.Distance(c) costToGoEst = c.CostToGoEst(goal) cost = costToReach + costToGoEst # Check whether it is already seen (hence on-deck) if c.seen: # Skip if the previous cost was better! if c.cost <= cost: continue # Else remove from the on-deck list (to keep sorted). onDeck.remove(c) # Add to the on-deck list in the correct order. c.seen = True c.parent = n c.costToReach = costToReach c.costToGoEst = costToGoEst c.cost = cost bisect.insort(onDeck, c) # Declare the node done. n.done = True # Build the path. path = [goal] while path[0].parent is not None: path.insert(0, path[0].parent) # Return the path. return path # # Select the Nodes # def AddNodesToListUniform(nodeList, N): # Add uniformly distributed samples while (N > 0): node = Node(random.uniform(xmin, xmax), random.uniform(ymin, ymax), random.uniform(0, math.pi)) if node.InFreespace(): nodeList.append(node) N = N-1 def AddNodesToListAtEdges(nodeList, N): # Add nodes near edges while (N > 0): node1 = Node(random.uniform(xmin+D/2, xmax-D/2), random.uniform(ymin+D/2, ymax-D/2), random.uniform(0, math.pi)) node2 = Node(node1.x + random.uniform(-D/2, D/2), node1.y + random.uniform(-D/2, D/2), node1.t + random.uniform(-D/2/L, D/2/L)) if node1.InFreespace() and not node2.InFreespace(): nodeList.append(node1) N = N-1 elif node2.InFreespace() and not node1.InFreespace(): nodeList.append(node2) N = N-1 def AddNodesToList(nodeList, N): # AddNodesToListUniform(nodeList, N) AddNodesToListAtEdges(nodeList, N) # # Brute-Force Nearest Neighbor # def NearestNodes(node, nodeList, K): # Create a sorted list of (distance, node) tuples. list = [] # Process/add all nodes to the sorted list, except the original # node itself. Continually cap the list at K elements. for n in nodeList: if n is not node: bisect.insort(list, (node.Distance(n), n)) list = list[0:K] # Return only the near nodes. return [n for _,n in list] def ConnectNearestNeighbors(nodeList, K): # Clear any and all existing neighbors. for node in nodeList: node.neighbors = [] # Process every node in the list for find (K) nearest neighbors. # Get the (K2) nearest nodes, as some might not connection. Once # a match in found, be sure to create the connnection from both # sides, i.e. add each node to each other's neighbor list. for node in nodeList: for nearnode in NearestNodes(node, nodeList, K2): if len(node.neighbors) >= K: break if node.ConnectsTo(nearnode): if (nearnode not in node.neighbors): node.neighbors.append(nearnode) if (node not in nearnode.neighbors): nearnode.neighbors.append(node) # # Post Process the Path # def PostProcess(path): i = 0 while (i < len(path)-2): if path[i].ConnectsTo(path[i+2]): path.pop(i+1) else: i = i+1 # # Main Code # def main(): # Create the figure. Visual.StartFigure() Visual.FlushFigure() # Create the start/goal nodes. startnode = Node(startx, starty, startt) goalnode = Node(goalx, goaly, goalt) # Show the start/goal states. Visual.DrawState(startnode, 'r', linewidth=2) Visual.DrawState(goalnode, 'r', linewidth=2) Visual.ShowFigure() # Create the list of sample points. nodeList = [] AddNodesToList(nodeList, N) # Show the sample states. for node in nodeList: Visual.DrawState(node, 'k', linewidth=1) Visual.ShowFigure() # Add the start/goal nodes. nodeList.append(startnode) nodeList.append(goalnode) # Connect to the nearest neighbors. ConnectNearestNeighbors(nodeList, K) # # Show the neighbor connections. # for node in nodeList: # for neighbor in node.neighbors: # Visual.DrawLocalPath(node, neighbor, 'g', linewidth=0.5) # Visual.ShowFigure() # Run the A* planner. path = AStar(nodeList, startnode, goalnode) if not path: print("UNABLE TO FIND A PATH") return # Show the path. for i in range(len(path)-1): Visual.DrawLocalPath(path[i], path[i+1], 'r', linewidth=1) Visual.FlushFigure() # Post Process the path. PostProcess(path) # Show the post-processed path. for i in range(len(path)-1): Visual.DrawLocalPath(path[i], path[i+1], 'b', linewidth=2) Visual.ShowFigure() if __name__== "__main__": main()