def __init__(self, start, end, robotRadius, clearance):
self.start = (start[1], start[0])
self.end = (end[1], end[0])
self.allNodes = []
############### nodeID [0], pareent [1], node [2], cost [3] ## For BFS
# self.mainData = [[0, 0, self.start, 0]]
############### cost , node #### For Dijkstar
self.Data = []
self.allPose = [self.start]
self.actionSet()
self.possibleMove = len(self.actionset)
self.temp = []
self.obss = Obstructions(300, 200, robotRadius, clearance)
self.goalReach = False
self.view = True
self.finalGoalState = []
self.trace = []
self.showCounter = 0
self.skipFrame = 1
class PathFinder():
def __init__(self, start, end, clearance, rpm1, rpm2):
self.start = (start[1], start[0], start[2])
self.end = (end[1], end[0])
self.rpm1 = rpm1
self.rpm2 = rpm2
self.allNodes = []
############### nodeID [0], pareent [1], node [2], cost [3] ## For BFS
# self.mainData = [[0, 0, self.start, 0]]
############### cost , node #### For Dijkstar
self.Data = []
self.allPose = [self.start]
self.actionSet(self.start)
self.possibleMove = len(self.actionset)
self.temp = []
self.obss = Obstructions(102, 102, clearance)
self.goalReach = False
self.view = True
self.finalGoalState = []
self.trace = []
self.showCounter = 0
self.skipFrame = 1
self.output = []
def plot_arrow(self, node, parentnode, color):
Xi, Yi = parentnode[1], parentnode[0]
UL, UR = node[3], node[4] #node[1], node[0]
Thetai = parentnode[2]
t = 0
r = 0.15
L = 3.54
dt = 0.1
Xn = Xi
Yn = Yi
Thetan = 3.14 * Thetai / 180
while t < 1:
t = t + dt
Xs = Xn
Ys = Yn
Xn += 0.5 * r * (UL + UR) * math.cos(Thetan) * dt
Yn += 0.5 * r * (UL + UR) * math.sin(Thetan) * dt
Thetan += (r / L) * (UR - UL) * dt
plt.plot([Xs, Xn], [Ys, Yn], color=color)
Thetan = 180 * (Thetan) / 3.14
# print(Xn, Yn, Thetan)
return Xn, Yn, Thetan
def initialCheck(self):
if not self.obss.checkFeasibility(self.start):
print(
"Start node is in obstacle field. Please provide correct starting position."
)
return False
elif not self.obss.checkFeasibility(self.end):
print(
"Goal node is in obstacle field. Please provide correct goal position."
)
return False
else:
return True
def callforaction(self, Xi, Yi, Thetai, UL, UR):
t = 0
r = 0.38
L = 3.54
dt = 0.1
Xn = Xi
Yn = Yi
Thetan = 3.14 * Thetai / 180
while t < 1:
t = t + dt
Xs = Xn
Ys = Yn
Xn += 0.5 * r * (UL + UR) * math.cos(Thetan) * dt
Yn += 0.5 * r * (UL + UR) * math.sin(Thetan) * dt
Thetan += (r / L) * (UR - UL) * dt
# print(Thetan, "first")
# plt.plot([Xs, Xn], [Ys, Yn], color="blue")
Thetan = (180 * (Thetan) / 3.14)
# print(Thetan, "second")
# print(Xn, Yn, Thetan)
return Xn, Yn, Thetan, UL, UR
def actionSet(self, node):
x1, y1, th1, ac11, ac12 = self.callforaction(node[0], node[1], node[2],
0, self.rpm1)
x2, y2, th2, ac21, ac22 = self.callforaction(node[0], node[1], node[2],
self.rpm1, 0)
x3, y3, th3, ac31, ac32 = self.callforaction(node[0], node[1], node[2],
self.rpm1, self.rpm1)
x4, y4, th4, ac41, ac42 = self.callforaction(node[0], node[1], node[2],
0, self.rpm2)
x5, y5, th5, ac51, ac52 = self.callforaction(node[0], node[1], node[2],
self.rpm2, 0)
x6, y6, th6, ac61, ac62 = self.callforaction(node[0], node[1], node[2],
self.rpm2, self.rpm2)
x7, y7, th7, ac71, ac72 = self.callforaction(node[0], node[1], node[2],
self.rpm1, self.rpm2)
x8, y8, th8, ac81, ac82 = self.callforaction(node[0], node[1], node[2],
self.rpm2, self.rpm1)
self.actionset = (
[round(x1, 0), round(y1, 0), th1, ac11, ac12], # straight
[round(x2, 0), round(y2, 0), th2, ac21, ac22], # moveup1
[round(x3, 0), round(y3, 0), th3, ac31, ac32], # moveup2
[round(x4, 0), round(y4, 0), th4, ac41, ac42], # movedown1
[round(x5, 0), round(y5, 0), th5, ac51, ac52], # movedown2
[round(x6, 0), round(y6, 0), th6, ac61, ac62],
[round(x7, 0), round(y7, 0), th7, ac71, ac72],
[round(x8, 0), round(y8, 0), th8, ac81, ac82])
# print("actionset", self.actionset)
pass
def checkEnd(self, currentNode):
return (((currentNode[0] - self.end[0])**2) +
((currentNode[1] - self.end[1])**2)) <= 2.25
def findNewPose(self, nodeState, action):
tmp = nodeState[2]
tmp = (tmp[0] + action[0], tmp[1] + action[1])
return tmp
def dijNewPose(self, node, action):
tmp = (node[0] + action[0], node[1] + action[1], node[2] + action[2])
return tmp
def viewer(self, num):
self.showCounter += 1
if self.showCounter % num == 0:
#cv2.namedWindow("Solver", cv2.WINDOW_NORMAL)
#cv2.resizeWindow("Solver", 600, 600)
#cv2.imshow("Solver", self.obss.animationImage)
# plt.show(self.obss.obstaclespace)
if cv2.waitKey(1) & 0xFF == ord('q'):
self.view = False
pass
def trackBack(self, node, ax):
path = [self.end]
child = tuple(node)
tmp = self.end
# print("child", tuple(child), self.start)
count = 0
while True: #child[0] != self.start[0] and child[1] != self.start[1]:
self.obss.path(tmp)
parent = self.obss.getParent(child)
self.output.append([int(parent[3]), int(parent[4])])
# print("temp",temp)
self.plot_arrow(child, parent, 'b')
# self.obss.showMap(ax)
self.x_obs = [col[0] for col in self.obss.obstaclespace]
self.y_obs = [col[1] for col in self.obss.obstaclespace]
ax.scatter(self.x_obs, self.y_obs, c='r')
filename = 'fig' + str(count) + '.png'
count += 1
plt.savefig(filename, bbox_inches='tight')
tmp = (int(child[0]), int(child[1]))
# print(tmp)
path.append(tmp)
# print('parent', parent)
child = parent
if child[0] == self.start[0] and child[1] == self.start[1]:
break
self.viewer(1)
#print(child, "child")
#print(self.start, "start")
print(self.output)
return
def DijkstraSolve(self):
fig, ax = plt.subplots()
plt.grid()
ax.set_aspect('equal')
plt.xlim(0, 102)
plt.ylim(0, 102)
plt.title('Final Output', fontsize=10)
# plt.savefig('final_output.png', bbox_inches='tight')
heappush(self.Data, (0, self.start, 0))
# self.obss.astar(start, end)
con = 0
while len(self.Data) > 0:
cost, node, cost_to_come = heappop(self.Data)
# print("cost", cost)
if self.checkEnd(node):
self.goalReach = True
print("goal reached")
self.trackBack(node, ax)
break
# return
self.actionSet(node)
for action in self.actionset:
# self.dg = math.sqrt(abs(((action[0] - end[0]) ** 2) + ((action[1] - end[1]) ** 2)))
newPose = action
act1, act2 = newPose[3], newPose[4]
# self.vect(newPose, fig, ax,newPose)
# print("newpose",newPose)
if self.obss.checkFeasibility(newPose):
dg = math.sqrt(((newPose[0] - self.end[0])**2) +
((newPose[1] - self.end[1])**2))
newCost = cost_to_come + (
(newPose[3] + newPose[4]) * 0.38 / 2)
# print("dg",self.dg)
if self.obss.checkVisited(newPose):
self.obss.addExplored(newPose)
self.plot_arrow(newPose, node, 'g')
# self.obss.showMap(ax)
self.x_obs = [
col[0] for col in self.obss.obstaclespace
]
self.y_obs = [
col[1] for col in self.obss.obstaclespace
]
ax.scatter(self.x_obs, self.y_obs, c='r')
filename = 'figs' + str(con) + '.png'
con += 1
plt.savefig(filename, bbox_inches='tight')
# print(act1, act2, "act")
self.obss.addParent(newPose, node, newCost, act1, act2)
heappush(self.Data, (newCost + dg, newPose, newCost))
else:
if self.obss.getCost(newPose) > newCost + dg:
# self.obss.addExplored(newPose)
self.obss.addParent(newPose, node, newCost, act1,
act2)
if self.view:
pass
self.viewer(30)
if self.goalReach:
self.obss.showMap(ax)
# fig.show()
# plt.draw()
# plt.show()
else:
print("Could not find goal node...Leaving..!!")
return
class PathFinder():
def __init__(self, start, end, robotRadius, clearance):
self.start = (start[1], start[0],start[2])
self.end = (end[1], end[0])
self.allNodes = []
############### nodeID [0], pareent [1], node [2], cost [3] ## For BFS
# self.mainData = [[0, 0, self.start, 0]]
############### cost , node #### For Dijkstar
self.Data = []
self.allPose = [self.start]
self.actionSet(self.start)
self.possibleMove = len(self.actionset)
self.temp = []
self.obss = Obstructions(300,200, robotRadius, clearance)
self.goalReach = False
self.view = True
self.finalGoalState = []
self.trace = []
self.showCounter = 0
self.skipFrame = 1
#self.dg = math.sqrt(abs(((action[0] - end [0])** 2) + ((action1[1] - end [1])** 2)))
def vect(self,start, fig, ax, node):
X0 = np.array((0))
Y0 = np.array((0))
U0 = np.array((2))
V0 = np.array((-2))
#fig, ax = plt.subplots()
#q0 = ax.quiver(X0, Y0, U0, V0, units='xy', scale=1, color='r', headwidth=1, headlength=0)
Node = [X0 + U0, Y0 + V0]
print('Node0: ')
print(Node)
#straight
x2, y2 = node[1], node[0]
u2, v2 = tuple(np.subtract([x2, y2], [self.actionset[0][0], self.actionset[0][1]]))
# U2 = np.array((self.end[0]))
# V2 = np.array((self.end[1]))
print(u2,v2)
ax.quiver(x2, y2, u2, v2, units='xy', scale=1,color='r')
Node2 = [x2 + u2, y2 + v2]
print('Node2: ')
print(Node2)
#moveup1
x3, y3 = node[1], node[0]
u3, v3 = tuple(np.subtract([x3, y3], [self.actionset[1][0], self.actionset[1][1]]))
# U2 = np.array((self.end[0]))
# V2 = np.array((self.end[1]))
ax.quiver(x3, y3, u3, v3, units='xy', scale=1)
Node3 = [x3 + u3, y3 + v3]
print('Node3: ')
print(Node3)
#moveup2
x4, y4 = node[1], node[0]
u4, v4 = tuple(np.subtract([x4, y4], [self.actionset[2][0], self.actionset[2][1]]))
# U2 = np.array((self.end[0]))
# V2 = np.array((self.end[1]))
ax.quiver(x4, y4, u4, v4, units='xy', scale=1)
Node4 = [x4 + u4, y4 + v4]
print('Node4: ')
print(Node4)
# movedown1
x5, y5 = node[1], node[0]
u5, v5 = tuple(np.subtract([x5, y5], [self.actionset[3][0], self.actionset[3][1]]))
# U2 = np.array((self.end[0]))
# V2 = np.array((self.end[1]))
ax.quiver(x5, y5, u5, v5, units='xy', scale=1)
Node5 = [x5 + u5, y5 + v5]
print('Node5: ')
print(Node5)
#movedown2
x6, y6 = node[1], node[0]
u6, v6 = tuple(np.subtract([x6, y6], [self.actionset[4][0], self.actionset[4][1]]))
# U2 = np.array((self.end[0]))
# V2 = np.array((self.end[1]))
ax.quiver(x6, y6, u6, v6, units='xy', scale=1)
Node6 = [x6 + u6, y6+ v6]
print('Node6: ')
print(Node6)
def plot_arrow(self, node, parentnode, ax, color):
ax.quiver(parentnode[1], parentnode[0], node[1]-parentnode[1], node[0]-parentnode[0], units='xy', scale=1, color=color)
#Node6 = [x6 + u6, y6 + v6]
def initialCheck(self):
if not self.obss.checkFeasibility(self.start):
print("Start node is in obstacle field. Please provide correct starting position.")
return False
elif not self.obss.checkFeasibility(self.end):
print("Goal node is in obstacle field. Please provide correct goal position.")
return False
else:
return True
def actionSet(self, node):
################# [ h, w, cost]
ang = 30
#print('aaaaaa',start)
#print('bbbbb', self.start)
self.actionset = ([round(node[0] + STEP*math.sin(math.radians(node[2] + 0)), 2),round(node[1] + STEP*math.cos(math.radians(node[2] + 0)), 2), node[2]], #straight
[round(node[0] + STEP*math.sin(math.radians(node[2] + ang)), 2), round(node[1] + STEP*math.cos(math.radians(node[2] + ang)),2), node[2] + ang], #moveup1
[round(node[0] + STEP*math.sin(math.radians(node[2] + ang*2)), 2), round(node[1] + STEP*math.cos(math.radians(node[2] + ang*2)), 2), node[2] + ang * 2], #moveup2
[round(node[0] + STEP*math.sin(math.radians(node[2] - ang)), 2), round(node[1] +STEP* math.cos(math.radians(node[2] - ang )), 2),node[2] - ang], #movedown1
[round(node[0] + STEP*math.sin(math.radians(node[2] - ang * 2)), 2),round(node[1] + STEP*math.cos(math.radians(node[2] - ang * 2)), 2), node[2] - ang * 2] #movedown2
)
pass
def checkEnd(self, currentNode):
return (((currentNode[0]-self.end[0])**2)+((currentNode[1]-self.end[1])**2)) <= 2.25
def findNewPose(self, nodeState, action):
tmp = nodeState[2]
tmp = (tmp[0]+action[0], tmp[1]+action[1])
return tmp
def dijNewPose(self, node, action):
tmp = (node[0]+action[0], node[1]+action[1], node[2]+ action[2])
return tmp
def viewer(self, num):
self.showCounter += 1
if self.showCounter%num == 0:
cv2.imshow("Solver", self.obss.animationImage)
#plt.show(self.obss.obstaclespace)
if cv2.waitKey(1) & 0xFF == ord('q'):
self.view = False
pass
def trackBack(self, node, ax):
path = [self.end]
child = tuple(node)
print("child",tuple(child), self.start)
while child != self.start:
#self.obss.path(tmp)
parent = self.obss.getParent(child)
#print("temp",temp)
self.plot_arrow(child, parent, ax, 'b')
tmp = (int(child[0]), int(child[1]))
# print(tmp)
path.append(tmp)
print('parent', parent)
child = parent
#self.viewer(1)
return
def DijkstarSolve(self):
fig, ax = plt.subplots()
plt.grid()
ax.set_aspect('equal')
plt.xlim(0, 300)
plt.ylim(0, 200)
plt.title('How to plot a vector in matplotlib ?', fontsize=10)
plt.savefig('how_to_plot_a_vector_in_matplotlib_fig3.png', bbox_inches='tight')
heappush(self.Data, (0, self.start,0))
#self.obss.astar(start, end)
while len(self.Data) > 0:
cost, node, costtocome = heappop(self.Data)
print("cost",cost)
if self.checkEnd(node):
self.goalReach = True
print("goal reached")
self.trackBack(node, ax)
break
#return
self.actionSet(node)
for action in self.actionset:
#self.dg = math.sqrt(abs(((action[0] - end[0]) ** 2) + ((action[1] - end[1]) ** 2)))
newPose = action
#self.vect(newPose, fig, ax,newPose)
#print("newpose",newPose)
if self.obss.checkFeasibility(newPose):
dg = math.sqrt(((newPose[0] - self.end[0]) ** 2) + ((newPose[1] - self.end[1]) ** 2))
newCost = costtocome + 1
#print("dg",self.dg)
if self.obss.checkVisited(newPose):
self.obss.addExplored(newPose)
self.plot_arrow(newPose, node, ax, 'g')
self.obss.addParent(newPose, node, newCost)
heappush(self.Data, (newCost + dg, newPose, newCost))
else:
if self.obss.getCost(newPose) > newCost+dg :
#self.obss.addExplored(newPose)
self.obss.addParent(newPose, node, newCost)
if self.view:
pass
#self.viewer(30)
if self.goalReach:
self.obss.showMap(ax)
fig.show()
plt.draw()
plt.show()
else:
print("Could not find goal node...Leaving..!!")
return
class PathFinder():
def __init__(self, start, end, robotRadius, clearance):
self.start = (start[1], start[0])
self.end = (end[1], end[0])
self.allNodes = []
############### nodeID [0], pareent [1], node [2], cost [3] ## For BFS
# self.mainData = [[0, 0, self.start, 0]]
############### cost , node #### For Dijkstar
self.Data = []
self.allPose = [self.start]
self.actionSet()
self.possibleMove = len(self.actionset)
self.temp = []
self.obss = Obstructions(300, 200, robotRadius, clearance)
self.goalReach = False
self.view = True
self.finalGoalState = []
self.trace = []
self.showCounter = 0
self.skipFrame = 1
def initialCheck(self):
if not self.obss.checkFeasibility(self.start):
print(
"Start node is in obstacle field. Please provide correct starting position."
)
return False
elif not self.obss.checkFeasibility(self.end):
print(
"Goal node is in obstacle field. Please provide correct goal position."
)
return False
else:
return True
def actionSet(self):
################# [ h, w, cost]
self.actionset = [[1, 0, 1], [0, 1, 1], [-1, 0, 1], [0, -1, 1],
[1, 1, np.sqrt(2)], [1, -1, np.sqrt(2)],
[-1, 1, np.sqrt(2)], [-1, -1, np.sqrt(2)]]
pass
def checkEnd(self, currentNode):
return self.end == currentNode
def findNewPose(self, nodeState, action):
tmp = nodeState[2]
tmp = (tmp[0] + action[0], tmp[1] + action[1])
return tmp
def dijNewPose(self, node, action):
tmp = (node[0] + action[0], node[1] + action[1])
return tmp
def viewer(self, num):
self.showCounter += 1
if self.showCounter % num == 0:
cv2.imshow("Solver", self.obss.animationImage)
if cv2.waitKey(1) & 0xFF == ord('q'):
self.view = False
pass
def trackBack(self):
path = [self.end]
tmp = self.end
while tmp != self.start:
self.obss.path(tmp)
temp = self.obss.getParent(tmp)
tmp = (int(temp[0]), int(temp[1]))
# print(tmp)
path.append(tmp)
self.viewer(1)
return
def DijkstarSolve(self):
heappush(self.Data, (0, self.start))
while len(self.Data) > 0:
cost, node = heappop(self.Data)
if self.checkEnd(node):
self.goalReach = True
print("goal reached")
self.trackBack()
return
for action in self.actionset:
newPose = self.dijNewPose(node, action)
if self.obss.checkFeasibility(newPose):
newCost = cost + action[2]
if self.obss.checkVisited(newPose):
self.obss.addExplored(newPose)
self.obss.addParent(newPose, node, newCost)
heappush(self.Data, (newCost, newPose))
else:
if self.obss.getCost(newPose) > newCost:
# self.obss.addExplored(newPose)
self.obss.addParent(newPose, node, newCost)
if self.view:
self.viewer(30)
print("Could not find goal node...Leaving..!!")
return