def bfs(board, start, goal):
"""Return open nodes, closed nodes and path"""
_node_cost = {
'w': 100,
'm': 50,
'f': 10,
'g': 5,
'r': 1,
'.': 1,
'A': 1,
'B': 1
}
neighbors = [(0, 1), (0, -1), (1, 0), (-1, 0)]
explored_coordinates = set()
# Board x size
board_x = len(board[0])
# Board y size
board_y = len(board)
start = Node(start[1], start[0])
goal = Node(goal[1], goal[0])
visited = set()
queue = Queue()
queue.put(start)
no_children = 0
while queue:
current = queue.get()
visited.add(current)
if current.x == goal.x and current.y == goal.y:
return list(queue.queue), visited, current.retrace_path()
for n in neighbors:
child_x = current.x + n[0]
child_y = current.y + n[1]
# Outside board
if child_x >= board_x or child_y >= board_y or child_x < 0 or child_y < 0:
continue
# Not a wall
if board[child_y][child_x] != '#':
no_children += 1
current.add_child(Node(child_x, child_y))
for child in current.children:
if (child.x, child.y) not in explored_coordinates:
explored_coordinates.add((child.x, child.y))
child.parent = current
queue.put(child)
return None
def successors(x, y):
ss = [Node(x + 1, y), Node(x - 1, y),
Node(x, y + 1), Node(x, y - 1)]
ss += [Node(x + 1, y + 1), Node(x - 1, y - 1),
Node(x - 1, y + 1), Node(x + 1, y - 1)]
yield from ss
def buildRRT(n, q0, qgoal):
""" Perform the RRT algorithm
"""
global discriminantRefusals
# Initialisation of trees
startNode = Node(q0)
endNode = Node(qgoal)
T = [[startNode], [endNode]]
currentTree = 0 # Tree to grow
otherTree = 1
mergesNumber = 0 # Number of merges achieved
# Main loop
i = 0
while i < n and mergesNumber < maxMergesNumber:
print(i)
i = i + 1
(node_qnear, node_qnew) = randomNode(T[currentTree])
if not (node_qnew is None):
extendRRT(T[currentTree], node_qnear, node_qnew)
# Try to merge qnew to the other tree:
for node_q in T[otherTree]:
if (cart_dist(node_q.q[:2], node_qnew.q[:2]) < merge_threshold
and not checkcollision(node_q.q, node_qnew.q)):
node_qnew.neighbors.append(node_q)
node_q.neighbors.append(node_qnew)
mergesNumber = mergesNumber + 1
break
# If you're here merging hasn't succeeded: swap trees and continue
currentTree = (currentTree + 1) % 2
otherTree = (otherTree + 1) % 2
print("Number of merges : {}".format(mergesNumber))
print("Discriminant refusals : {}".format(discriminantRefusals))
return T
def __init__(self, x, y, children=[]):
Node.__init__(self, children)
self.x = x
self.y = y
def randomNode(T):
""" Choose a random command and apply it to a node
"""
global discriminantRefusals
# Pick a random config
x_rand = rd.random() * Xmax
y_rand = rd.random() * Ymax
theta_rand = rd.random() * 2 * pi
beta_rand = rd.random() * 2 * pi
q_rand = [x_rand, y_rand, theta_rand, beta_rand]
# Find the closest node of this random config in T
bestDistance = 100000
bestNode = None
for node in T:
distance = cart_dist(node.q, q_rand)
if distance < bestDistance:
bestDistance = distance
bestNode = node
q_near = bestNode.q
node_qnear = bestNode
# Compute the desired velocity of the sugar
sugarPos = sugarPosition(q_near, a, d)
desiredSugarPos = sugarPosition(q_rand, a, d)
dx = desiredSugarPos[0] - sugarPos[0]
dy = desiredSugarPos[1] - sugarPos[1]
vx = Vmax * dx / sqrt(dx**2 + dy**2)
vy = Vmax * dy / sqrt(dx**2 + dy**2)
# Applying the sugar tracking command
[x, y, theta, beta] = q_near
success = True # to check whether this configuration is ok
for i in range(Ne):
xm_dot = beta_dot = 0
if beta != 0:
A = L / tan(beta) * cos(theta) - a * sin(theta) - d * sin(theta +
beta)
B = -d * sin(theta + beta)
C = L / tan(beta) * sin(theta) + a * cos(theta) + d * cos(theta +
beta)
D = d * cos(theta + beta)
invDet = 1 / (A * D - B * C)
xm_dot = L / tan(beta) * invDet * (D * vx - B * vy)
beta_dot = invDet * (A * vy - C * vx)
else:
xm_dot = Vmax
beta_dot = 1 / d * (cos(theta) * vy - sin(theta) * vx)
beta += step_size / Ne * beta_dot
theta += step_size / Ne * tan(beta) / L
x += step_size / Ne * cos(theta) * xm_dot
y += step_size / Ne * sin(theta) * xm_dot
# Check for collision
if textMap[int(x * Xpix / Xmax)][int(y * Ypix / Ymax)] != 0:
success = False # We found at least one collision
break
beta = beta % (2 * pi)
theta = theta % (2 * pi)
q = [x, y, theta, beta]
if success:
# Check whether the resulting node is not too near from an existing one
for node in T:
if cart_dist(node.q, q) < discriminant:
success = False
discriminantRefusals = discriminantRefusals + 1
break
if success:
# Create the resulting node
node_qnew = Node(q)
return node_qnear, node_qnew
return None, None
def classic_8_neighbors():
side = 10
walls = (
Node(2, 3), Node(3, 3), Node(4, 3), Node(5, 3), Node(6, 3), Node(7, 3),
Node(2, 4), Node(3, 4), Node(4, 4), Node(5, 4), Node(6, 4), Node(7, 4),
Node(6, 5), Node(7, 5),
Node(6, 6), Node(7, 6)
)
start = Node(0, 9)
stop = Node(9, 0)
graph = build_graph(side, successors, *walls)
filename = 'illustrations/classic_8_neighbors.gif'
a_star(start, stop, graph, g, euclid, filename=filename)
def alpha():
side = 10
walls = (Node(2, 3), Node(3, 3), Node(4, 3), Node(5, 3), Node(6, 3),
Node(7, 3), Node(2, 4), Node(3, 4), Node(4, 4), Node(5, 4),
Node(6, 4), Node(7, 4), Node(6, 5), Node(7,
5), Node(6,
6), Node(7, 6))
start = Node(0, 9)
stop = Node(9, 0)
graph = build_graph(side, successors, *walls)
filename = 'illustrations/alpha.gif'
fa_h = partial(h, l1=1, l2=2, g=g)
a_star(start, stop, graph, g, fa_h, filename=filename)
def rectangle_walk(width, height):
for x in range(width):
for y in range(height):
yield Node(x, y)
def dijkstra(board, start, goal):
_node_cost = {
'w': 100,
'm': 50,
'f': 10,
'g': 5,
'r': 1,
'.': 1,
'A': 1,
'B': 1
}
# Board x size
board_x = len(board[0])
# Board y size
board_y = len(board)
# Explored coordinates
explored_coordinate = set()
explored_coordinate.add((start[1], start[0]))
# The set of nodes already evaluated
closed_set = set()
# The set of currently discovered nodes still to be evaluated
open_set = set()
# Make A and B nodes
start = Node(start[1], start[0])
goal = Node(goal[1], goal[0])
# Add start node
open_set.add(start)
neighbors = [(0, 1), (0, -1), (1, 0), (-1, 0)]
while open_set:
current = min(open_set, key=lambda o: o.g)
if current.x == goal.x and current.y == goal.y:
return open_set, closed_set, current.retrace_path()
# Generate children
for n in neighbors:
# Give the neighbors coordinates
child_x = current.x + n[0]
child_y = current.y + n[1]
# Outside board
if child_x >= board_x or child_y >= board_y or child_x < 0 or child_y < 0:
continue
# Not a wall
if board[child_y][child_x] != '#':
current.add_child(Node(child_x, child_y))
open_set.remove(current)
closed_set.add(current)
for child in current.children:
# Calculate new node distances
if (child.x, child.y) not in explored_coordinate:
explored_coordinate.add((child.x, child.y))
child.parent = current
child.g = current.g + _node_cost[board[child.y][child.x]]
open_set.add(child)
elif current.g + _node_cost[board[child.y][child.x]] < child.g:
child.parent = current
child.g = current.g + _node_cost[board[child.y][child.x]]
open_set.add(child)
return None
def dijkstra():
side = 10
walls = (Node(2, 3), Node(3, 3), Node(4, 3), Node(5, 3), Node(6, 3),
Node(7, 3), Node(2, 4), Node(3, 4), Node(4, 4), Node(5, 4),
Node(6, 4), Node(7, 4), Node(6, 5), Node(7,
5), Node(6,
6), Node(7, 6))
start = Node(0, 9)
stop = Node(9, 0)
graph = build_graph(side, successors, *walls)
a_star(start, stop, graph, g, h, filename='illustrations/dijkstra.gif')
def dynamic_weighting():
side = 10
walls = (
Node(2, 3), Node(3, 3), Node(4, 3), Node(5, 3), Node(6, 3), Node(7, 3),
Node(2, 4), Node(3, 4), Node(4, 4), Node(5, 4), Node(6, 4), Node(7, 4),
Node(6, 5), Node(7, 5),
Node(6, 6), Node(7, 6)
)
start = Node(0, 9)
stop = Node(9, 0)
graph = build_graph(side, successors, *walls)
filename = 'illustrations/dynamic_weighting.gif'
dm_h = partial(h, N=euclid(start, stop), e=5, d=g)
a_star(start, stop, graph, g, dm_h, filename=filename)