def nodeToList(n):
#if n == None:
#print("ntl:", n)
b1 = heap.GetID(n)
#print("1", b1)
b2 = heap.GetDependents(n)
#print("2", b2)
b3 = heap.GetStep(n)
#print("3", b3)
b4 = heap.GetDecision(n)
#print("4", b4)
b5 = heap.GetParent(n)
#print("5", b5)
b6 = float(heap.GetTime(n))
#print("6", b6)
b7 = float(heap.GetVelocity(n))
#print("7", b7)
b8 = (heap.GetGear(n), heap.GetDelta(n), heap.GetTimeToShift(n))
#print("8", b8)
b9 = float(heap.GetScore(n))
#print("9", b9)
b = [b1, b2, b3, b4, b5, b6, b7, b8, b9]
#print(b)
return b
def nodeToList(n):
return [
heap.GetID(n),
heap.GetDependents(n),
heap.GetStep(n),
heap.GetDecision(n),
heap.GetParent(n),
float(heap.GetTime(n)),
float(heap.GetVelocity(n)),
(heap.GetGear(n), heap.GetDelta(n), heap.GetTimeToShift(n)),
float(heap.GetScore(n))
]
def printNode(n):
print("ID: ", heap.GetID(n))
print("Dependents: ", heap.GetDependents(n))
print("Step: ", heap.GetStep(n))
print("Decision: ", GetDecision(n))
print("Parent: ", heap.GetParent(n))
print("Time: ", heap.GetTime(n))
print("Velocity: ", heap.GetVelocity(n))
print("Gear: ", heap.GetGear(n))
print("Delta: ", heap.GetDelta(n))
print("Time to shift: ", heap.GetTimeToShift(n))
print("Score: ", heap.GetScore(n))
def solve(self, vehicle, segments):
# constants/structures
self.vp = vehicle
self.segments = segments
self.n = len(segments) + 1
self.rk = RelationshipKeeper(self.axes, self.n)
# get incumbent solution
i = sim_pointmass()
self.incumbent = i.solve(vehicle, segments)
self.max_cost = self.incumbent[-1, O_TIME]
print("max time:", self.max_cost)
print("max int: ", self.maxint)
# set up base case/start node
heap.Init(self.n**(self.axes - 1))
init_state = heap.MakeInitNode()
if not init_state:
print("init fuqqed")
exit()
print("initialized: ", init_state)
self.rk.addNode(0, init_state)
# prepare for loop
heapIndex = 1
processed = -1
end_states = []
axisLeaders = [heap.MakeInitNode() for a in range(self.axes)]
for a in axisLeaders:
if not a:
print("axis fuqqed")
exit()
heap.SetParent(a, init_state)
print("AL0: ", axisLeaders)
### TEMPORARILY HERE
for i, leader in enumerate(axisLeaders):
if leader != None:
listLeader = nodeToList(leader)
newNode = self.policies[i](listLeader)
#print("newleaders: ", i, newNode)
if newNode == None:
axisLeaders[i] = None
# heap.KillNode(leader)
else:
newNode[G_ID] = listLeader[G_ID] + (self.n
**(self.axes - i - 1))
newNode[G_PARENT] = leader
axisLeaders[i] = listToNode(newNode)
self.rk.addNode(newNode[G_ID], axisLeaders[i])
children = self.rk.makeChildren(newNode[G_ID])
for child in children:
heap.SetParent(child, axisLeaders[i])
heap.Insert(child, heapIndex, newNode[G_TIME])
added = True
### AAAAAAAH
levels = 0
while (added):
added = False
levels += 1
# move reserved nodes to working heap
# heap.SwapHeaps()
#print("working: ", heap.cvar.workingHeapSize)
#print("reserve: ", heap.cvar.reserveHeapSize)
#print("heapInd: ", heapIndex)
workingNode = heap.DeleteMin(heapIndex)
while (workingNode != None):
# handy counter
processed += 1
if processed % 10000 == 0:
print(
"processed: ", levels, processed, heap.cvar.nodesMade,
heap.cvar.workingHeapSize + heap.cvar.reserveHeapSize)
# constants
workingID = heap.GetID(workingNode)
#print("working on: ", workingID, workingNode, nodeToList(workingNode))
optimalState = None
minCost = self.maxint
parents = self.rk.getParentPointers(workingID)
#print("parents: ", parents)
# find best parent if any
for parent in parents:
decisionID, parentID = parent
parentPointer = self.rk.getNode(parentID)
if parentPointer != None:
parentList = nodeToList(parentPointer)
cost, newState = self.compute_RT(
parentList, decisionID)
#print("NS: ", cost, newState)
if newState != None and cost < minCost:
minCost = cost
optimalState = newState
optimalState[G_PARENT] = parentPointer
optimalState[G_DECISION] = decisionID
optimalState[G_ID] = workingID
#print("OS: ", optimalState)
# assign parent or kill all the nodes
if minCost < self.maxint:
#print("success: ", minCost)
#print("here")
if optimalState[G_STEP] == self.n - 1:
end_states.append(listToNode(optimalState))
else:
children = self.rk.makeChildren(workingID)
for child in children:
added = True
heap.SetParent(child, workingNode)
heap.Insert(child, abs(heapIndex - 1), minCost)
else:
print("or here", levels, heap.cvar.nodesMade,
(heap.cvar.workingHeapSize +
heap.cvar.reserveHeapSize))
# heap.KillNode(workingNode)
workingNode = heap.DeleteMin(heapIndex)
# refresh axis leaders
for i, leader in enumerate(axisLeaders):
if leader != None:
listLeader = nodeToList(leader)
newNode = self.policies[i](listLeader)
#print("newleaders: ", i, newNode)
if newNode == None:
axisLeaders[i] = None
# heap.KillNode(leader)
else:
newNode[G_ID] = listLeader[G_ID] + (self.n**(
self.axes - i - 1))
newNode[G_PARENT] = leader
axisLeaders[i] = listToNode(newNode)
self.rk.addNode(newNode[G_ID], axisLeaders[i])
children = self.rk.makeChildren(newNode[G_ID])
for child in children:
heap.SetParent(child, axisLeaders[i])
heap.Insert(child, abs(heapIndex - 1),
newNode[G_TIME])
added = True
#print("AL: ", axisLeaders)
heapIndex = abs(heapIndex - 1)
print("ES: ", end_states)
path, output = self.find_optimum(end_states)
print(path)
return output
def addNode(self, num, pointer):
if num != heap.GetID(pointer):
pdb.set_trace()
self.nodes[num] = pointer
def solve(self, vehicle, segments):
# constants/structures
self.vp = vehicle
self.segments = segments
self.n = len(segments) + 1
self.rk = RelationshipKeeper(self.axes, self.n)
print(self.n)
# get incumbent solution
i = sim_pointmass()
self.incumbent = i.solve(vehicle, segments)
self.max_cost = self.incumbent[-1, O_TIME]
# set up base case/start node
heap.Init(self.n**(self.axes - 1))
init_state = heap.MakeInitNode()
init_list = nodeToList(init_state)
self.rk.addNode(0, init_state)
# prepare for loop
heapIndex = 0
processed = -1
end_states = []
for i in range(self.axes):
listLeader = self.policies[i](init_list)
if listLeader != None:
listLeader[G_ID] = ((self.n + 1)**(self.axes - i - 1))
leader = listToNode(listLeader)
heap.SetParent(leader, 0)
self.rk.addNode(listLeader[G_ID], leader)
children = self.rk.makeChildren(listLeader[G_ID])
for child in children:
heap.Insert(child, heapIndex, listLeader[G_TIME])
added = True
levels = 0
while (added):
added = False
levels += 1
print("processed: ", levels, processed, heap.cvar.nodesMade,
heap.cvar.workingHeapSize + heap.cvar.reserveHeapSize)
if levels == 44:
pdb.set_trace()
workingNode = heap.DeleteMin(heapIndex)
while (workingNode != None):
# handy counter
processed += 1
# constants
workingID = heap.GetID(workingNode)
optimalState = None
minCost = self.maxint
minParent = None
parents = self.rk.getParentPointers(workingID)
# find best parent if any
for parent in parents:
decisionID, parentID = parent
parentPointer = self.rk.getNode(parentID)
if parentPointer != None:
parentList = nodeToList(parentPointer)
cost, newState = self.compute_RT(
parentList, decisionID)
#print("NS: ", cost, newState)
if newState != None and cost < minCost:
minCost = cost
minParent = parentPointer
optimalState = newState
optimalState[G_DECISION] = decisionID
optimalState[G_PARENT] = parentID
optimalState[G_ID] = workingID
# assign parent or kill all the nodes
if minCost < self.maxint:
self.rk.addNode(workingID, listToNode(optimalState))
if heap.GetParent(self.rk.getNode(workingID)) == None:
pdb.set_trace()
if optimalState[G_STEP] == self.n - 1:
end_states.append(workingID)
else:
children = self.rk.makeChildren(workingID)
for child in children:
added = True
heap.Insert(child, abs(heapIndex - 1), minCost)
else:
heap.FreeNode(workingNode)
self.rk.nodes[workingID] = None
workingNode = heap.DeleteMin(heapIndex)
heapIndex = abs(heapIndex - 1)
path, output = self.find_optimum(end_states)
print(path)
return output
def find_optimum(self, endStates):
for k in self.rk.nodes.keys():
if self.rk.getNode(k) != None and heap.GetID(
self.rk.getNode(k)) != k:
print("uh oh:", k)
for e in endStates:
print(e, self.rk.flatToList(e), heap.GetTime(self.rk.getNode(e)))
if len(endStates) <= 0:
print("did not succeed")
exit()
min_t = self.maxint
min_S = None
for s in endStates:
statePointer = self.rk.getNode(s)
#if statePointer == None:
#print("fo:", statePointer)
if heap.GetTime(statePointer) < min_t:
min_t = heap.GetTime(statePointer)
min_S = statePointer
#min_S = self.rk.getNode(1931)
data = nodeToList(min_S)
print(data)
path = []
output = np.zeros([self.n, O_MATRIX_COLS])
#if min_S == None:
#print("foo:", min_S)
output[self.n - 1, :] = np.array([
heap.GetTime(min_S),
heap.GetStep(min_S),
heap.GetVelocity(min_S), 0, 0, 0,
heap.GetDecision(min_S),
heap.GetGear(min_S), 0, 0, 0, 0,
self.segments[heap.GetStep(min_S) - 1].curvature, 0, 0
])
i = self.n - 2
while data[4] >= 0:
min_S = self.rk.getNode(data[4])
print("##########################################")
data = nodeToList(min_S)
print(data)
path.append(heap.GetDecision(min_S))
output[i, :] = np.array([
heap.GetTime(min_S),
heap.GetStep(min_S),
heap.GetVelocity(min_S), 0, 0, 0,
heap.GetDecision(min_S),
heap.GetGear(min_S), 0, 0, 0, 0,
self.segments[heap.GetStep(min_S) - 1].curvature, 0, 0
])
i = i - 1
print(output[:, O_STATUS])
print(output[:, O_VELOCITY])
print(output[:, O_CURVATURE])
return (path[::-1], output)