def aStarSingle(cable,
dest,
baseIndex,
robotIndex,
heuristic,
enforceCable=None,
debug=False) -> SolutionLog:
"""
baseIndex and robotIndex: 0 | -1
"""
solutionLog = SolutionLog(heuristic)
nodeMap = {
} # We keep a map of nodes here to update their child-parent relationship
q = PriorityQ(key1=Node.pQGetPrimaryCost,
key2=Node.pQGetSecondaryCost) # The Priority Queue container
if debug:
logger.log("CABLE-O: %s - L = %.2f" %
(repr(cable), Geom.lengthOfCurve(cable)))
root = Node(cable=cable,
parent=None,
heuristicFuncName=heuristic,
fractions=[1, 1])
q.enqueue(root)
count = 0
destinationsFound = 0
while not q.isEmpty():
n: Node = q.dequeue()
count += 1
if debug:
logger.log("-------------MAX=%.2f, MIN=%.2f @ %s-------------" %
(Node.pQGetPrimaryCost(n), Node.pQGetSecondaryCost(n),
getCableId(n.cable, n.fractions)))
if isAtDestination(n, dest, robotIndex):
solutionLog.content = Solution.createFromNode(n)
solutionLog.expanded = count
solutionLog.genereted = len(nodeMap)
if debug:
logger.log(
"At Destination after expanded %d nodes, discovering %d configs"
% (solutionLog.expanded, solutionLog.genereted))
destinationsFound += 1
return solutionLog
base = n.cable[baseIndex]
gaps = n.cable[robotIndex].gaps if n.cable[
robotIndex].name != dest.name else {n.cable[robotIndex]}
for gap in gaps:
if gap.name == n.cable[baseIndex].name: continue
if isUndoingLastMove(n, gap, robotIndex): continue
if areBothStaying(n, gap if robotIndex == 0 else base,
base if robotIndex == 0 else gap):
continue
newCable = None
# FIXME: Defensively ignoring exceptions
try:
newCable = tightenCable(n.cable,
gap if robotIndex == 0 else base,
base if robotIndex == 0 else gap)
except Exception as err:
model.removeTriangulationEdges()
continue
if enforceCable:
ind = findSubCable(
newCable,
enforceCable[1:] if robotIndex == 0 else enforceCable[:-1])
if ind < 0: continue
l = Geom.lengthOfCurve(newCable)
if l <= model.MAX_CABLE:
addChildNode(newCable, n, nodeMap, q, heuristic, debug)
if debug:
logger.log("Total Nodes: %d, %d configs, %d destinations" %
(count, len(nodeMap), destinationsFound))
solutionLog.expanded = count
solutionLog.genereted = len(nodeMap)
solutionLog.setEndTime()
return solutionLog
def aStar(heuristic, debug=False) -> SolutionLog:
model.setSolution(SolutionLog(heuristic, model.MAX_CABLE))
nodeMap = {
} # We keep a map of nodes here to update their child-parent relationship
q = PriorityQ(key1=Node.pQGetPrimaryCost,
key2=Node.pQGetSecondaryCost) # The Priority Queue container
logger.log("##############################################")
logger.log("################## A-STAR ##################")
logger.log("CABLE-O: %s - L = %.2f" %
(repr(model.cable), Geom.lengthOfCurve(model.cable)))
logger.log("Heuristic = %s" % heuristic)
root = Node(cable=model.cable,
parent=None,
heuristicFuncName=heuristic,
debug=debug)
q.enqueue(root)
count = 0
destinationsFound = 0
while not q.isEmpty():
n: Node = q.dequeue()
count += 1
# visited.add(n)
if debug:
logger.log("-------------MAX=%.2f, MIN=%.2f @ %s-------------" %
(Node.pQGetPrimaryCost(n), Node.pQGetSecondaryCost(n),
getCableId(n.cable, n.fractions)))
if isAtDestination(n):
model.solution.content = Solution.createFromNode(n)
model.solution.expanded = count
model.solution.genereted = len(nodeMap)
logger.log(
"At Destination after expanded %d nodes, discovering %d configs"
% (model.solution.expanded, model.solution.genereted))
destinationsFound += 1
return model.solution
# Va = n.cable[0].gaps if n.fractions[0] == 1 else {n.cable[0]}
Va = n.cable[0].gaps if n.cable[0].name != "D1" else {n.cable[0]}
for va in Va:
if isUndoingLastMove(n, va, 0): continue
# Vb = n.cable[-1].gaps if n.fractions[1] == 1 else {n.cable[-1]}
Vb = n.cable[-1].gaps if n.cable[-1].name != "D2" else {
n.cable[-1]
}
for vb in Vb:
if isUndoingLastMove(n, vb, -1): continue
if areBothStaying(n, va, vb): continue
# For now I deliberately avoid cross movement because it crashes the triangulation
# In reality we can fix this by mirorring the space (like I did in the previous paper)
if isThereCrossMovement(n.cable, va, vb): continue
newCable = None
# FIXME: Defensively ignoring exceptions
try:
newCable = tightenCable(n.cable, va, vb)
except:
continue
l = Geom.lengthOfCurve(newCable)
if l <= model.MAX_CABLE:
addChildNode(newCable, n, nodeMap, q, heuristic, debug)
# else:
# (frac, fracCable) = getPartialMotion(n.cable, newCable, isRobotA=True, debug=debug)
# if not isnan(frac): addChildNode(fracCable, n, nodeMap, q, debug, fractions=[frac, 1])
# (frac, fracCable) = getPartialMotion(n.cable, newCable, isRobotA=False, debug=debug)
# if not isnan(frac): addChildNode(fracCable, n, nodeMap, q, debug, fractions=[1, frac])
logger.log("Total Nodes: %d, %d configs, %d destinations" %
(count, len(nodeMap), destinationsFound))
model.solution.expanded = count
model.solution.genereted = len(nodeMap)
model.solution.setEndTime()
return model.solution
