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