def Decompose(self, node):
cell = 'free'
r = node[0]
rx = r.x
ry = r.y
rwidth = r.width
rheight = r.height
area = rwidth * rheight
for o in self.domain.obstacles:
if (o.CalculateOverlap(r) >= rwidth * rheight):
cell = 'obstacle'
break
elif (o.CalculateOverlap(r) > 0.0):
cell = 'mixed'
break
if (cell == 'mixed'):
entropy = self.CalcEntropy(r)
igH = 0.0
hSplitTop = None
hSplitBottom = None
vSplitLeft = None
vSplitRight = None
if (r.height / 2.0 > self.minimumSize):
hSplitTop = Rectangle(rx, ry + rheight / 2.0, rwidth,
rheight / 2.0)
entHSplitTop = self.CalcEntropy(hSplitTop)
hSplitBottom = Rectangle(rx, ry, rwidth, rheight / 2.0)
entHSplitBottom = self.CalcEntropy(hSplitBottom)
igH = entropy - ( r.width * r.height / 2.0 ) / area * entHSplitTop \
- ( r.width * r.height / 2.0 ) / area * entHSplitBottom
igV = 0.0
if (r.width / 2.0 > self.minimumSize):
vSplitLeft = Rectangle(rx, ry, rwidth / 2.0, rheight)
entVSplitLeft = self.CalcEntropy(vSplitLeft)
vSplitRight = Rectangle(rx + rwidth / 2.0, ry, rwidth / 2.0,
rheight)
entVSplitRight = self.CalcEntropy(vSplitRight)
igV = entropy - ( r.width/2.0 * r.height ) / area * entVSplitLeft \
- ( r.width/2.0 * r.height ) / area * entVSplitRight
children = []
if (igH > igV):
if (igH > 0.0):
if (hSplitTop is not None) and (hSplitBottom is not None):
childTop = [hSplitTop, 'unknown', []]
childBottom = [hSplitBottom, 'unknown', []]
children = [childTop, childBottom]
else:
if (igV > 0.0):
if (vSplitLeft is not None) and (vSplitRight is not None):
childLeft = [vSplitLeft, 'unknown', []]
childRight = [vSplitRight, 'unknown', []]
children = [childLeft, childRight]
for c in children:
self.Decompose(c)
node[2] = children
node[1] = cell
return node
def Decompose(self, node):
cell = 'free'
r = node[0]
rx = r.x
ry = r.y
rwidth = r.width
rheight = r.height
for o in self.domain.obstacles:
if (o.CalculateOverlap(r) >= rwidth * rheight):
cell = 'obstacle'
break
elif (o.CalculateOverlap(r) > 0.0):
cell = 'mixed'
break
if (cell == 'mixed'):
if (rwidth / 2.0 > self.minimumSize) and (rheight / 2.0 >
self.minimumSize):
childt1 = [
Rectangle(rx, ry, rwidth / 2.0, rheight / 2.0), 'unknown',
[], [],
float('inf'), False
]
qchild1 = self.Decompose(childt1)
childt2 = [
Rectangle(rx + rwidth / 2.0, ry, rwidth / 2.0,
rheight / 2.0), 'unknown', [], [],
float('inf'), False
]
qchild2 = self.Decompose(childt2)
childt3 = [
Rectangle(rx, ry + rheight / 2.0, rwidth / 2.0,
rheight / 2.0), 'unknown', [], [],
float('inf'), False
]
qchild3 = self.Decompose(childt3)
childt4 = [
Rectangle(rx + rwidth / 2.0, ry + rheight / 2.0,
rwidth / 2.0, rheight / 2.0), 'unknown', [], [],
float('inf'), False
]
qchild4 = self.Decompose(childt4)
children = [qchild1, qchild2, qchild3, qchild4]
node[2] = children
else:
cell = 'obstacle'
elif cell == 'free':
graph.append(node)
node[1] = cell
return node
def Decompose(self, node):
cell = 'free'
r = node[0]
rx = r.x
ry = r.y
rwidth = r.width
rheight = r.height
initialCell = Rectangle(self.initialCell[0], self.initialCell[1], 0.1,
0.1)
goalCell = Rectangle(self.goalsCell[0][0], self.goalsCell[0][1], 0.1,
0.1)
for o in self.domain.obstacles:
if o.CalculateOverlap(r) >= rwidth * rheight:
cell = 'obstacle'
break
elif o.CalculateOverlap(r) > 0.0:
cell = 'mixed'
break
if cell == 'mixed':
if (((rwidth / 2.0 > self.minimumSize) and
(rheight / 2.0 > self.minimumSize))
or goalCell.CalculateOverlap(r) > 0.0
or initialCell.CalculateOverlap(r) > 0.0):
childt1 = [
Rectangle(rx, ry, rwidth / 2.0, rheight / 2.0), 'unknown',
[]
]
qchild1 = self.Decompose(childt1)
childt2 = [
Rectangle(rx + rwidth / 2.0, ry, rwidth / 2.0,
rheight / 2.0), 'unknown', []
]
qchild2 = self.Decompose(childt2)
childt3 = [
Rectangle(rx, ry + rheight / 2.0, rwidth / 2.0,
rheight / 2.0), 'unknown', []
]
qchild3 = self.Decompose(childt3)
childt4 = [
Rectangle(rx + rwidth / 2.0, ry + rheight / 2.0,
rwidth / 2.0, rheight / 2.0), 'unknown', []
]
qchild4 = self.Decompose(childt4)
children = [qchild1, qchild2, qchild3, qchild4]
node[2] = children
else:
cell = 'obstacle'
node[1] = cell
return node
def Decompose(self, parent):
cell = 'free'
r = parent[0]
rx = r.x
ry = r.y
rwidth = r.width
rheight = r.height
for o in self.domain.obstacles:
if (o.CalculateOverlap(r) >= rwidth * rheight):
cell = 'obstacle'
break
elif (o.CalculateOverlap(r) > 0.0):
cell = 'mixed'
break
# break down only when it s mixed
if cell == 'mixed':
if (rwidth / 2.0 > self.minimumSize) and (rheight / 2.0 >
self.minimumSize):
# node structure [rec, name, child, parent]
soth_west = [
Rectangle(rx, ry, rwidth / 2.0, rheight / 2.0), 'unknown',
[], parent, self.SW, 0
]
q_soth_west = self.Decompose(soth_west)
south_east = [
Rectangle(rx + rwidth / 2.0, ry, rwidth / 2.0,
rheight / 2.0), 'unknown', [], parent, self.SE, 0
]
q_south_east = self.Decompose(south_east)
north_west = [
Rectangle(rx, ry + rheight / 2.0, rwidth / 2.0,
rheight / 2.0), 'unknown', [], parent, self.NW, 0
]
q_north_west = self.Decompose(north_west)
north_east = [
Rectangle(rx + rwidth / 2.0, ry + rheight / 2.0,
rwidth / 2.0, rheight / 2.0), 'unknown', [],
parent, self.NE, 0
]
q_north_east = self.Decompose(north_east)
children = [
q_soth_west, q_south_east, q_north_west, q_north_east
]
parent[2] = children
else:
cell = 'obstacle'
parent[1] = cell
return parent
def nbTopTest(self, rect):
x = rect.x
height = self.minimumSize
y = rect.y + rect.height
width = rect.width
test = Rectangle(x, y, width, height)
return test
def __init__(self, domain, minimumSize):
self.domain = domain
self.minimumSize = minimumSize
self.root = [
Rectangle(0.0, 0.0, domain.width, domain.height), 'unknown', [],
None, 0, 0
]
def nbRightTest(self, rect):
x = rect.x + rect.width
width = self.minimumSize
y = rect.y
height = rect.height
test = Rectangle(x, y, width, height)
return test
def nbBotTest(self, rect):
x = rect.x
height = self.minimumSize
y = rect.y - self.minimumSize
width = rect.width
test = Rectangle(x, y, width, height)
return test
def __init__(self, domain, minimumSize):
self.domain = domain
self.minimumSize = minimumSize
self.root = [
Rectangle(0.0, 0.0, domain.width, domain.height), 'unknown', []
]
self.free_cell = []
a = 1
def __init__(self, domain, minimumSize):
self.domain = domain
self.minimumSize = minimumSize
self.root = [
Rectangle(0.0, 0.0, domain.width, domain.height), 'unknown', [],
[],
float('inf'), False
]
def Decompose(self, node, depth = 1):
cell = 'free'
r = node.rectangle
rx = r.x
ry = r.y
rwidth = r.width
rheight = r.height
if depth > self.maxDepth:
self.maxDepth = depth
for o in self.domain.obstacles:
if ( o.CalculateOverlap(r) >= rwidth * rheight ):
cell = 'obstacle'
break
elif ( o.CalculateOverlap(r) > 0.0 ):
cell = 'mixed'
break
if ( cell == 'mixed'):
# Decomposition order
# 1 2
# 3 4
if (rwidth / 2.0 > self.minimumSize) and (rheight / 2.0 > self.minimumSize):
childt1 = Cell(Rectangle(rx, ry, rwidth/2.0, rheight/2.0), 'unknown', node)
qchild1 = self.Decompose(childt1, depth + 1)
childt2 = Cell(Rectangle(rx + rwidth/2.0, ry, rwidth/2.0, rheight/2.0), 'unknown', node)
qchild2 = self.Decompose(childt2, depth + 1)
childt3 = Cell(Rectangle(rx, ry + rheight/2.0, rwidth/2.0, rheight/2.0), 'unknown', node)
qchild3 = self.Decompose(childt3, depth + 1)
childt4 = Cell(Rectangle(rx + rwidth/2.0, ry + rheight/2.0, rwidth/2.0, rheight/2.0), 'unknown', node)
qchild4 = self.Decompose(childt4, depth + 1)
children = [ qchild1, qchild2, qchild3, qchild4 ]
node.children = children
else:
cell = 'obstacle'
node.occupancy = cell
return node
def ExploreDomain( domain, initial, steps ):
log = np.zeros((steps,2))
pos = np.array(initial)
dd = 2
theta = 0.00/180.0 * math.pi
for i in range(steps):
newpos = pos + dd * np.array([dd * math.cos(theta), dd * math.sin(theta)])
r = Rectangle(newpos[0], newpos[1], 1, 1)
if ( newpos[0] >= 0.0 ) and ( newpos[0] < domain.width ) and ( newpos[1] >= 0.0 ) and ( newpos[1] < domain.height ):
if ( not domain.CheckOverlap( r ) ):
pos = newpos
theta = theta + random.uniform(-180.0/180.0 * math.pi, 180.0/180.0 * math.pi)
while( theta >= math.pi ):
theta = theta - 2 * math.pi
while( theta < - math.pi ):
theta = theta + 2 * math.pi
log[i,:] = pos
return log
def __init__(self, domain, minimumSize):
self.domain = domain
self.minimumSize = minimumSize
self.root = Cell(Rectangle(0.0, 0.0, domain.width, domain.height), 'unknown', None)
self.maxDepth = 1
def Decompose(self, node, depth = 1):
cell = 'free'
r = node.rectangle
rx = r.x
ry = r.y
rwidth = r.width
rheight = r.height
area = rwidth * rheight
if depth > self.maxDepth:
self.maxDepth = depth
for o in self.domain.obstacles:
if ( o.CalculateOverlap(r) >= rwidth * rheight ):
cell = 'obstacle'
break
elif ( o.CalculateOverlap(r) > 0.0 ):
cell = 'mixed'
break
if ( cell == 'mixed'):
entropy = self.CalcEntropy(r)
hSplitTop = None
hSplitBottom = None
vSplitLeft = None
vSplitRight = None
igH = 0.0
if ( r.height / 2.0 > self.minimumSize):
hSplitTop = Rectangle(rx, ry + rheight/2.0, rwidth, rheight/2.0)
entHSplitTop = self.CalcEntropy(hSplitTop)
hSplitBottom = Rectangle(rx, ry, rwidth, rheight/2.0)
entHSplitBottom = self.CalcEntropy( hSplitBottom )
igH = entropy - ( r.width * r.height / 2.0 ) / area * entHSplitTop \
- ( r.width * r.height / 2.0 ) / area * entHSplitBottom
igV = 0.0
if ( r.width / 2.0 > self.minimumSize ):
vSplitLeft = Rectangle(rx, ry, rwidth/2.0, rheight )
entVSplitLeft = self.CalcEntropy( vSplitLeft )
vSplitRight = Rectangle( rx + rwidth/2.0, ry, rwidth/2.0, rheight)
entVSplitRight = self.CalcEntropy( vSplitRight)
igV = entropy - ( r.width/2.0 * r.height ) / area * entVSplitLeft \
- ( r.width/2.0 * r.height ) / area * entVSplitRight
children = []
if ( igH > igV ):
if ( igH > 0.0 ):
if ( hSplitTop is not None ) and ( hSplitBottom is not None ):
node.split = "horizontal"
# Horizontal split child order: Top, Bottom
childTop = CellFBSP(hSplitTop, 'unknown', node, "N")
childBottom = CellFBSP(hSplitBottom, 'unknown', node, "S")
children = [childTop, childBottom]
else:
if ( igV > 0.0 ):
if ( vSplitLeft is not None ) and ( vSplitRight is not None ):
node.split = "vertical"
# Vertical split child order: Left, Right
childLeft = CellFBSP(vSplitLeft, 'unknown', node, "W")
childRight = CellFBSP(vSplitRight, 'unknown', node, "E")
children = [childLeft, childRight]
for c in children:
self.Decompose(c, depth + 1)
node.children = children
node.occupancy = cell
return node
def ExploreDomain(domain,
initial,
goal,
maxSteps,
stepDistance=0.1,
biasToGoal=0,
goalRadius=0.25):
treeNodes = []
treeRoot = Node(initial[0], initial[1])
treeNodes.append(treeRoot)
nodeNearGoal = None
for i in range(maxSteps):
# Get a target point in space to expand towards
targetPoint = goal
if random.uniform(
0, 1
) < 1 - biasToGoal: # If we're going in a random direction this step
targetPoint = [
random.uniform(0, domain.width),
random.uniform(0, domain.height)
]
# Expand node nearest to point towards point
nearestToRandomPoint = treeRoot
distNearestToRandomPoint = getDistanceBetweenPoints(
nearestToRandomPoint.pos, targetPoint)
for node in treeNodes:
dist = getDistanceBetweenPoints(node.pos, targetPoint)
if (dist < distNearestToRandomPoint):
nearestToRandomPoint = node
distNearestToRandomPoint = dist
# Get a vector towards the target point
dirVector = [
targetPoint[0] - nearestToRandomPoint.pos[0],
targetPoint[1] - nearestToRandomPoint.pos[1]
] # Create direction vector
dirVector[0] /= distNearestToRandomPoint # Normalize vector
dirVector[1] /= distNearestToRandomPoint
# Move the point nearest to target point towards target point, create new node
newPoint = [
nearestToRandomPoint.pos[0] + dirVector[0] * stepDistance,
nearestToRandomPoint.pos[1] + dirVector[1] * stepDistance
]
if (newPoint[0] >= 0.0) and (newPoint[0] < domain.width) and (
newPoint[1] >= 0.0) and (newPoint[1] <
domain.height): # within map
newPointRect = Rectangle(newPoint[0], newPoint[1], 0.1, 0.1)
if (not domain.CheckOverlap(newPointRect)
): # Not inside of an obstacle
newNode = Node(newPoint[0], newPoint[1], nearestToRandomPoint)
treeNodes.append(
newNode) # Create new node parented to its nearest node
# If node near goal then stop searching
if getDistanceBetweenPoints(newNode.pos, goal) <= goalRadius:
nodeNearGoal = newNode
break
return treeNodes, nodeNearGoal
def main(argv=None):
if (argv == None):
argv = sys.argv[1:]
width = 100.0
height = 100.0
pp = PathPlanningProblem(width, height, 60, 40, 40)
initial, goals = pp.CreateProblemInstance()
fig = plt.figure()
ax = fig.add_subplot(1, 2, 1, aspect='equal')
ax.set_xlim(0.0, width)
ax.set_ylim(0.0, height)
for o in pp.obstacles:
ax.add_patch(copy.copy(o.patch))
ip = plt.Rectangle((initial[0], initial[1]), 1.0, 1.0, facecolor='#ff0000')
ax.add_patch(ip)
for g in goals:
g = plt.Rectangle((g[0], g[1]), 1.0, 1.0, facecolor='#00ff00')
ax.add_patch(g)
qtd = QuadTreeDecomposition(pp, 0.2, initial, goals)
qtd.Draw(ax)
n = qtd.CountCells()
start = timeit.default_timer()
astar = AStarSearch(
qtd.domain, qtd.root,
Rectangle(qtd.initialCell[0], qtd.initialCell[1], 0.1, 0.1),
Rectangle(qtd.goalsCell[0][0], qtd.goalsCell[0][1], 0.1, 0.1))
stop = timeit.default_timer()
print("A* Running Time: ", stop - start)
print("A* Path Length : ", astar.path_length)
plt.plot([x for (x, y) in astar.path], [y for (x, y) in astar.path], '-')
ax.set_title('Quadtree Decomposition\n{0} cells'.format(n))
ax = fig.add_subplot(1, 2, 2, aspect='equal')
ax.set_xlim(0.0, width)
ax.set_ylim(0.0, height)
for o in pp.obstacles:
ax.add_patch(copy.copy(o.patch))
ip = plt.Rectangle((initial[0], initial[1]), 1, 1, facecolor='#ff0000')
ax.add_patch(ip)
goal = None
for g in goals:
goal = g
g = plt.Rectangle((g[0], g[1]), 1, 1, facecolor='#00ff00')
ax.add_patch(g)
start = timeit.default_timer()
spath = RRT.ExploreDomain(RRT(8), pp, initial, goal, 5000)
path = spath[0]
final = spath[1]
if len(final) == 0:
print("No path found")
RRT.draw(RRT(0), plt, path, final)
stop = timeit.default_timer()
print("RRT Running Time: ", stop - start)
if len(final) > 0:
print("RRT Path Length : ", RRT.pathLen(RRT(0), final))
ax.set_title('RRT')
plt.show()
def ExploreDomain(self, domain, initial, goal, steps):
init = Node(initial[0], initial[1])
g = Node(goal[0], goal[1])
dd = self.get_step_len(domain)
newpos = None
nodeList = [init]
while True:
if steps == 0:
break
if random.randint(0, 100) > 5:
rnd = [random.uniform(0, 100), random.uniform(0, 100)]
else:
rnd = [g.x, g.y]
nind = self.getNearNeighbour(nodeList, rnd)
nearestNode = nodeList[nind]
theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
newNode = Node((nearestNode.x), (nearestNode.y))
newNode.parent = nearestNode
newpos = Node((newNode.x + dd * math.cos(theta)),
(newNode.y + dd * math.sin(theta)))
newpos.parent = newNode
xp = newNode.x
yp = newNode.y
over = False
for i in range(dd):
xp = xp + math.cos(theta)
yp = yp + math.sin(theta)
rec = Rectangle(xp, yp, 1, 1)
if domain.CheckOverlap(rec):
over = True
break
r = Rectangle(newpos.x, newpos.y, 1, 1)
if (newpos.x >= 0.0) and (newpos.x < domain.width) and (
newpos.y >= 0.0) and (newpos.y < domain.height):
if (not over and not domain.CheckOverlap(r)):
steps = steps - 1
dx = newpos.x - g.x
dy = newpos.y - g.y
d = math.sqrt(dx * dx + dy * dy)
if d <= dd:
newpos = Node(g.x, g.y)
newpos.parent = newNode
nodeList.append(newpos)
break
nodeList.append(newpos)
finalpath = []
lastIndex = len(nodeList) - 1
node = nodeList[lastIndex]
if node.x == g.x and node.y == g.y:
while node.parent is not None:
finalpath.append(node)
node = node.parent
finalpath.append(init)
result = [nodeList, finalpath]
return result
def ExploreDomain(domain, initial, steps, goals, name):
logging.info("Thread %s: starting", name)
startTime = time.process_time()
global solutionFound
if name.__eq__("Thread 1"):
pathColour = BLACK
goalColour = BLUE
thread = 1
global all_nodes
else:
pathColour = BROWN
goalColour = PURPLE
thread = 2
global all_nodes2
cost = None
foundGoal = False
numGoalPaths = 0
pos = np.array(initial)
# Create the first node, this is where our implementation starts to deviate from sample
initialNode = Node(initial[0], initial[1])
if thread is 1:
all_nodes = [initialNode]
else:
all_nodes2 = [initialNode]
print("Our initial Node on the graph is : ", initialNode)
# Init random path
randomPath = [[initialNode.x, initialNode.y]]
# TEST CODE
for i in range(steps):
if solutionFound is not True:
# Pick a spot on the graph, proceed
rand = Node(random.uniform(0, MAX_FIELD_WIDTH),
random.uniform(0, MAX_FIELD_HEIGHT))
# Init distance, closestNode = startingNode
my_nodes = []
if thread is 1:
closestNode = all_nodes[0]
my_nodes = all_nodes
else:
closestNode = all_nodes2[0]
my_nodes = all_nodes2
d = 0
# Iterate to max cycles (Can be changed to give our algorithm more time to find a solution
for theNodes in my_nodes:
# Find closest node to random point
if distance(theNodes, rand) < distance(closestNode, rand):
closestNode = theNodes
d = math.hypot(rand.x - closestNode.x,
rand.y - closestNode.y)
newnode = steer(closestNode, rand)
# Call nearest node function to find our neighbour
neighbourNode = nearestNode(my_nodes, newnode)
# Store the path inside each node, later we can fetch for goal
if d <= PATH_DD:
r = Rectangle(rand.x, rand.y, 1 * SCALE, 1 * SCALE)
# If no violation, add the random point to the list of coordinates in our current node
if not domain.CheckOverlap(r):
newnode.xList.append(rand.x)
newnode.yList.append(rand.y)
# Our current node's parent is simply its neighbour
newnode.parent = neighbourNode
# Check for obstacles
r = Rectangle(newnode.x, newnode.y, 1 * SCALE, 1 * SCALE)
if not domain.CheckOverlap(r):
my_nodes.append(newnode)
# Draw the current connection made to the simulation
pygame.draw.line(screen, pathColour,
[closestNode.x, closestNode.y],
[newnode.x, newnode.y])
pygame.display.update()
# Check for goal and show goal path if found
atGoal, cost, numGoalPaths = checkGoal(newnode, goals, my_nodes,
cost, numGoalPaths,
goalColour, thread)
#Update if first time hitting goal
if foundGoal is False and atGoal is True:
foundGoal = True
solutionFound = True
print("Found goal")
# Optional, if we wish to end program once any goal path is found (good for dual rrt)
if atGoal:
print(name, "\n")
print(
"Solution found, took the following length of time to complete : ",
(time.process_time() - startTime), "s")
return foundGoal
# Check if user wishes to leave, IE ESCAPE
endFlag = checkExitSimulation()
if endFlag is True:
print("Leaving simulation, sorry to see you go")
exit(1)
else:
exit(1)
# If we are out of the loop and still no goal, notify and return
if foundGoal is False:
print("Goal not found, try increasing max iterations")
return foundGoal
def ExploreDomain(domain, initial, steps, goals):
cost = None
foundGoal = False
numGoalPaths = 0
timeToGoal = 0
pos = np.array(initial)
# Create the first node, this is where our implementation starts to deviate from sample
initialNode = Node(initial[0], initial[1])
all_nodes = [initialNode]
print("Our initial Node on the graph is : ", initialNode)
# Init random path
randomPath = [[initialNode.x, initialNode.y]]
# TEST CODE
for i in range(steps):
# Pick a spot on the graph, proceed
rand = Node(random.uniform(0, MAX_FIELD_WIDTH),
random.uniform(0, MAX_FIELD_HEIGHT))
# Init distance, closestNode = startingNode
closestNode = all_nodes[0]
d = 0
# Iterate to max cycles (Can be changed to give our algorithm more time to find a solution
for theNodes in all_nodes:
# Find closest node to random point
if distance(theNodes, rand) < distance(closestNode, rand):
closestNode = theNodes
d = math.hypot(rand.x - closestNode.x, rand.y - closestNode.y)
newnode = steer(closestNode, rand)
# Call nearest node function to find our neighbour
neighbourNode = nearestNode(all_nodes, newnode)
# Store the path inside each node, later we can fetch for goal
if d <= PATH_DD:
r = Rectangle(rand.x, rand.y, 1 * SCALE, 1 * SCALE)
# If no violation, add the random point to the list of coordinates in our current node
if not domain.CheckOverlap(r):
newnode.xList.append(rand.x)
newnode.yList.append(rand.y)
# Our current node's parent is simply its neighbour
newnode.parent = neighbourNode
# Check for obstacles
r = Rectangle(newnode.x, newnode.y, 1 * SCALE, 1 * SCALE)
if not domain.CheckOverlap(r):
all_nodes.append(newnode)
# Draw the current connection made to the simulation
pygame.draw.line(screen, BLACK, [closestNode.x, closestNode.y],
[newnode.x, newnode.y])
pygame.display.update()
# Check for goal and show goal path if found
atGoal, cost, numGoalPaths = checkGoal(newnode, goals, all_nodes, cost,
numGoalPaths)
#Update if first time hitting goal
if foundGoal is False and atGoal is True:
foundGoal = True
timeToGoal = time.process_time()
print("Found goal")
# Optional, if we wish to end program once any goal path is found
#if atGoal:
#return
# Check if user wishes to leave, IE ESCAPE
checkExitSimulation("Leaving simulation, sorry to see you go")
# Print best cost path
print("The current optimal cost of our goal path is: ", cost)
# The number of unique goal paths found
print("The number of goal paths explored: ", numGoalPaths)
# If we are out of the loop and still no goal, notify and return
if foundGoal is False:
print("Goal not found, try increasing max iterations")
return foundGoal, timeToGoal
def ExploreDomain( domain, initial, steps, goal ):
paths = []
pos = np.array(initial)
tree = [pos]
dd = 0.1
total_steps = int( steps / dd )
diagLength = math.sqrt( 50*50 + 50*50 )
goal = np.array( goal )
atGoal = False
iter = 0
while atGoal == False and iter < MAX_ITERS:
newSpot = None
log = np.zeros( ( total_steps, 2 ) )
rand = random.randint( 1, 4 )
if rand < 4:
theta = random.uniform(-180.0/180.0 * math.pi, 180.0/180.0 * math.pi)
while( theta >= math.pi ):
theta = theta - 2 * math.pi
while( theta < - math.pi ):
theta = theta + 2 * math.pi
# get a random spot.
newSpot = [domain.width/2, domain.height/2];
newSpot = np.array( newSpot )
newDist = random.uniform( 0.0, diagLength )
newSpot = newSpot + newDist * np.array([newDist * math.cos( theta ), newDist * math.sin( theta )])
else:
newSpot = goal
# Get the cloeset node to the new point.
pos, distance = getClosestNode( newSpot, tree )
if pos is not None:
pos = np.array( pos )
log[0,:] = pos
distanceRemaining = distance
lineToMove = newSpot - pos
lengthOfLine = lineLength( lineToMove )
lineToMove[0] /= lengthOfLine
lineToMove[1] /= lengthOfLine
i = 1
stop = False
while i < total_steps and not stop:
# newpos = pos + dd * np.array([dd * math.cos(theta), dd * math.sin(theta)])
newpos = np.array( pos + dd * np.array( lineToMove ) )
distanceRemaining -= dd
r = Rectangle(newpos[0], newpos[1], 0.1, 0.1)
if ( newpos[0] >= 0.0 ) and ( newpos[0] < domain.width ) and ( newpos[1] >= 0.0 ) and ( newpos[1] < domain.height ):
if ( not domain.CheckOverlap( r ) ):
pos = newpos
log[i,:] = pos
# Check if we reached the goal.
atGoal = hitGoal( goal, pos )
if atGoal:
stop = True
else:
stop = True
else:
stop = True
i = i + 1
# we are at the new node.
if distanceRemaining < 1e-9:
stop = True
# add the final position to the tree.
tree.append( pos )
i = 1
while i < total_steps and log[i][0] != 0:
i = i + 1
log = log[:i, :i]
if len( log ) > 1:
paths.append( log )
else:
print( "no closest node found.... This is an error" )
iter = iter + 1
atGoal = hitGoal( goal, tree[len( tree ) - 1] )
if atGoal == True:
print( "Found it!" )
return paths, atGoal
def Decompose(self, node):
cell = 'free'
r = node[0]
rx = r.x
ry = r.y
rwidth = r.width
rheight = r.height
area = rwidth * rheight
for o in self.domain.obstacles:
# if fill the rect guarantee to be obstacle
if (o.CalculateOverlap(r) >= rwidth * rheight):
cell = 'obstacle'
break
# still can be obstacle when overlap
elif (o.CalculateOverlap(r) > 0.0):
cell = 'mixed'
break
cal = self.CalMultiOverLapArea(r, self.domain.obstacles, 1, 0)
if cal >= rwidth * rheight:
cell = 'obstacle'
# elif cal>0:
# cell='mixed'
if (cell == 'mixed'):
entropy = self.CalcEntropy(r)
igH = 0.0
# horizontal split
hSplitTop = None
hSplitBottom = None
# vertical split
vSplitLeft = None
vSplitRight = None
# split horizontally into 2 pieces
if (r.height / 2.0 > self.minimumSize):
hSplitTop = Rectangle(rx, ry + rheight / 2.0, rwidth,
rheight / 2.0)
entHSplitTop = self.CalcEntropy(hSplitTop)
hSplitBottom = Rectangle(rx, ry, rwidth, rheight / 2.0)
entHSplitBottom = self.CalcEntropy(hSplitBottom)
# information gain horizontal
igH = entropy - ( r.width * r.height / 2.0 ) / area * entHSplitTop \
- ( r.width * r.height / 2.0 ) / area * entHSplitBottom
igV = 0.0
# split vertically into 2 pieces
if (r.width / 2.0 > self.minimumSize):
vSplitLeft = Rectangle(rx, ry, rwidth / 2.0, rheight)
entVSplitLeft = self.CalcEntropy(vSplitLeft)
vSplitRight = Rectangle(rx + rwidth / 2.0, ry, rwidth / 2.0,
rheight)
entVSplitRight = self.CalcEntropy(vSplitRight)
# information gain vertical
igV = entropy - ( r.width/2.0 * r.height ) / area * entVSplitLeft \
- ( r.width/2.0 * r.height ) / area * entVSplitRight
# decomposition
children = []
if (igH > igV):
if (igH > 0.0):
if (hSplitTop is not None) and (hSplitBottom is not None):
childTop = [hSplitTop, 'unknown', [], node, -1, 0]
childBottom = [
hSplitBottom, 'unknown', [], node, -1, 0
]
children = [childTop, childBottom]
else:
if (igV > 0.0):
if (vSplitLeft is not None) and (vSplitRight is not None):
childLeft = [vSplitLeft, 'unknown', [], node, -1, 0]
childRight = [vSplitRight, 'unknown', [], node, -1, 0]
children = [childLeft, childRight]
for c in children:
self.Decompose(c)
node[2] = children
node[1] = cell
return node
