def test_init(self):
node1 = Node('f')
self.assertEqual(node1.name, 'f')
self.assertEqual(node1.arguments, [])
self.assertEqual(node1.find, node1)
self.assertEqual(node1.parents, set())
node2 = Node('g', [node1])
self.assertEqual(node2.name, 'g')
self.assertEqual(node2.arguments, [node1])
self.assertEqual(node2.find, node2)
self.assertEqual(node2.parents, set())
self.assertEqual(node1.parents, {node2})
def test_add_single_parent(self):
node1 = Node('f')
node2 = Node('g')
node3 = Node('h')
self.assertEqual(node1.parents, set())
self.assertEqual(node2.parents, set())
self.assertEqual(node3.parents, set())
node1.add_single_parent(node2)
self.assertEqual(node1.parents, {node2})
node1.add_single_parent(node3)
self.assertEqual(node1.parents, {node2, node3})
def search(problem, step, treshold):
time = 0
current_node = Node(problem.get_initial_state())
while True:
T = cooling_rate(time)
if T < treshold:
return current_node
adjacents = current_node.get_adjacents(step)
next = random.choice(adjacents)
value_difference = problem.find_value(next.state) - problem.find_value(
current_node.state)
if value_difference > 0:
current_node = next
else:
prob = math.exp(value_difference / T)
if random.random() <= prob:
current_node = next
time += 1
def test_add_single_parent(self):
node1 = Node('f')
node2 = Node('g')
node3 = Node('h')
self.assertEqual(node1.parents, set())
self.assertEqual(node2.parents, set())
self.assertEqual(node3.parents, set())
node1.add_single_parent(node2)
self.assertEqual(node1.parents, {node2})
node1.add_single_parent(node3)
self.assertEqual(node1.parents, {node2, node3})
def _heuristicTrmpp(self) -> Cost:
root = Node(cable=self.cable,
parent=None,
debug=self.debug,
heuristicFuncName="_heuristicShortestPath")
solution = _privateAStar(root=root,
MAX_CABLE=model.MAX_CABLE *
(len(self.cable) + 1) * 1.25,
debug=self.debug)
model.solution.expanded += solution.expanded
model.solution.genereted += solution.genereted
if self.debug: logger.log("T = %.2f" % solution.time)
if not solution.content:
return Cost()
return solution.content.cost
def addChildNode(newCable,
parent,
nodeMap,
pQ,
debug,
fractions=[1, 1]) -> None:
cableStr = getCableId(newCable, fractions)
if cableStr in nodeMap:
nodeMap[cableStr].updateParent(parent)
if debug:
logger.log("UPDATE %s @ %s" %
(repr(nodeMap[cableStr].f), cableStr))
else:
child = Node(cable=newCable,
parent=parent,
debug=parent.debug,
heuristicFuncName="_heuristicShortestPath")
if debug: logger.log("ADDING %s @ %s" % (repr(child.f), cableStr))
nodeMap[cableStr] = child
pQ.enqueue(child)
def test_equality(self):
# Create a few nodes.
node1 = Node('f')
node2 = Node('f')
node3 = Node('g')
node4 = Node('f')
node4.add_argument(node1)
node4.add_argument(node3)
node5 = Node('f')
node5.add_argument(node1)
node5.add_argument(node2)
# Nodes 1 and 2 should be equal to each other.
self.assertEqual(node1, node2)
# Nodes 1 and 3 are not equal because their function names differ.
self.assertNotEqual(node1, node3)
# Nodes 1 and 4 are not equal because they do not have the same number
# of arguments.
self.assertNotEqual(node1, node4)
# Nodes 4 and 5 are not equal because their arguments are not congruent.
self.assertNotEqual(node4, node5)
def test_get_class_parents(self):
node1 = Node('f')
node2 = Node('f', [node1])
node3 = Node('f')
node4 = Node('f', [node3])
# Without setting anything else, there are no parents.
self.assertEqual(node1.get_class_parents(), {node2})
self.assertEqual(node2.get_class_parents(), set())
self.assertEqual(node3.get_class_parents(), {node4})
self.assertEqual(node4.get_class_parents(), set())
# Attention: Changing the find value will not change the parents sets!
node2.find = node1
node4.find = node3
self.assertEqual(node1.get_class_parents(), {node2})
self.assertEqual(node2.get_class_parents(), {node2})
self.assertEqual(node3.get_class_parents(), {node4})
self.assertEqual(node4.get_class_parents(), {node4})
node3.find = node1
self.assertEqual(node1.get_class_parents(), {node2})
self.assertEqual(node2.get_class_parents(), {node2})
self.assertEqual(node3.get_class_parents(), {node2})
self.assertEqual(node4.get_class_parents(), {node2})
def test_get_class_representative(self):
node1 = Node('f')
node2 = Node('f')
node3 = Node('f')
node4 = Node('f')
# Without setting anything else, all nodes are their own
# representatives.
self.assertEqual(node1.get_class_representative(), node1)
self.assertEqual(node2.get_class_representative(), node2)
self.assertEqual(node3.get_class_representative(), node3)
self.assertEqual(node4.get_class_representative(), node4)
node1.find = node2
self.assertEqual(node1.get_class_representative(), node2)
self.assertEqual(node2.get_class_representative(), node2)
self.assertEqual(node3.get_class_representative(), node3)
self.assertEqual(node4.get_class_representative(), node4)
node3.find = node4
self.assertEqual(node1.get_class_representative(), node2)
self.assertEqual(node2.get_class_representative(), node2)
self.assertEqual(node3.get_class_representative(), node4)
self.assertEqual(node4.get_class_representative(), node4)
node2.find = node3
self.assertEqual(node1.get_class_representative(), node4)
self.assertEqual(node2.get_class_representative(), node4)
self.assertEqual(node3.get_class_representative(), node4)
self.assertEqual(node4.get_class_representative(), node4)
def test_add_argument(self):
# A few nodes.
node1 = Node('f')
node2 = Node('g')
node3 = Node('h')
# The functions have the following forms: g(f, f), h(f)
node2.add_argument(node1)
node2.add_argument(node1)
node3.add_argument(node1)
# Now node 2 should have an argument list with two elements, both being
# node 1 and node 3 should have node 1 as its only argument. Node 1
# should have nodes 2 and 3 as its parents, but both only once.
self.assertEqual(node1.arguments, [])
self.assertEqual(node1.parents, {node2, node3})
self.assertEqual(node2.arguments, [node1, node1])
self.assertEqual(node2.parents, set())
self.assertEqual(node3.arguments, [node1])
self.assertEqual(node3.parents, set())
# Create a few nodes.
node1 = Node('f')
node2 = Node('x')
node3 = Node('f')
node4 = Node('f')
node5 = Node('a')
node1.add_argument(node2)
node3.add_argument(node1)
node4.add_argument(node5)
self.assertEqual(node1.arguments, [node2])
self.assertEqual(node2.arguments, [])
self.assertEqual(node3.arguments, [node1])
self.assertEqual(node4.arguments, [node5])
self.assertEqual(node5.arguments, [])
self.assertEqual(node1.parents, {node3})
self.assertEqual(node2.parents, {node1})
self.assertEqual(node3.parents, set())
self.assertEqual(node4.parents, set())
self.assertEqual(node5.parents, {node4})
def test_equality(self):
# Create a few nodes.
node1 = Node('f')
node2 = Node('f')
node3 = Node('g')
node4 = Node('f')
node4.add_argument(node1)
node4.add_argument(node3)
node5 = Node('f')
node5.add_argument(node1)
node5.add_argument(node2)
# Nodes 1 and 2 should be equal to each other.
self.assertEqual(node1, node2)
# Nodes 1 and 3 are not equal because their function names differ.
self.assertNotEqual(node1, node3)
# Nodes 1 and 4 are not equal because they do not have the same number
# of arguments.
self.assertNotEqual(node1, node4)
# Nodes 4 and 5 are not equal because their arguments are not congruent.
self.assertNotEqual(node4, node5)
def test_merge(self):
# Create a few nodes.
node1 = Node('f')
node2 = Node('x')
node3 = Node('f')
node4 = Node('f')
node5 = Node('a')
node1.add_argument(node2)
node3.add_argument(node1)
node4.add_argument(node5)
# Merge nodes 1 and 2 and nodes 5 and 3.
self.assertTrue(node1.merge(node2))
self.assertTrue(node5.merge(node3))
# Now node 2 should be the representative.
self.assertEqual(node1.parents, set())
self.assertEqual(node1.find, node2)
self.assertEqual(node2.parents, {node1, node3, node4})
self.assertEqual(node2.find, node2)
self.assertEqual(node3.parents, set())
self.assertEqual(node3.find, node2)
self.assertEqual(node4.parents, set())
self.assertEqual(node4.find, node2)
self.assertEqual(node5.parents, set())
self.assertEqual(node5.find, node2)
# Trying to merge nodes 1 and 2 should return false as they are already
# congruent to each other.
self.assertFalse(node1.merge(node2))
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
node11.find = node3
node12.find = node5
# Create the lists to check.
merge_list = [
(node1, node2),
(node4, node6)
]
inequality_list = [
(node3, node5)
]
atom_list = []
'''
# Create the nodes.
node1 = Node('x')
node2 = Node('cons')
node3 = Node('x1')
node4 = Node('x2')
node5 = Node('y')
node6 = Node('cons')
node7 = Node('cdr')
node8 = Node('car')
node9 = Node('z')
node10 = Node('cons')
node11 = Node('cdr')
node12 = Node('car')
node13 = Node('car')
node14 = Node('cdr')
node15 = Node('car')
node16 = Node('cdr')
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
def test_get_class_parents(self):
node1 = Node('f')
node2 = Node('f', [node1])
node3 = Node('f')
node4 = Node('f', [node3])
# Without setting anything else, there are no parents.
self.assertEqual(node1.get_class_parents(), {node2})
self.assertEqual(node2.get_class_parents(), set())
self.assertEqual(node3.get_class_parents(), {node4})
self.assertEqual(node4.get_class_parents(), set())
# Attention: Changing the find value will not change the parents sets!
node2.find = node1
node4.find = node3
self.assertEqual(node1.get_class_parents(), {node2})
self.assertEqual(node2.get_class_parents(), {node2})
self.assertEqual(node3.get_class_parents(), {node4})
self.assertEqual(node4.get_class_parents(), {node4})
node3.find = node1
self.assertEqual(node1.get_class_parents(), {node2})
self.assertEqual(node2.get_class_parents(), {node2})
self.assertEqual(node3.get_class_parents(), {node2})
self.assertEqual(node4.get_class_parents(), {node2})
def test_get_class_representative(self):
node1 = Node('f')
node2 = Node('f')
node3 = Node('f')
node4 = Node('f')
# Without setting anything else, all nodes are their own
# representatives.
self.assertEqual(node1.get_class_representative(), node1)
self.assertEqual(node2.get_class_representative(), node2)
self.assertEqual(node3.get_class_representative(), node3)
self.assertEqual(node4.get_class_representative(), node4)
node1.find = node2
self.assertEqual(node1.get_class_representative(), node2)
self.assertEqual(node2.get_class_representative(), node2)
self.assertEqual(node3.get_class_representative(), node3)
self.assertEqual(node4.get_class_representative(), node4)
node3.find = node4
self.assertEqual(node1.get_class_representative(), node2)
self.assertEqual(node2.get_class_representative(), node2)
self.assertEqual(node3.get_class_representative(), node4)
self.assertEqual(node4.get_class_representative(), node4)
node2.find = node3
self.assertEqual(node1.get_class_representative(), node4)
self.assertEqual(node2.get_class_representative(), node4)
self.assertEqual(node3.get_class_representative(), node4)
self.assertEqual(node4.get_class_representative(), node4)
def test_union(self):
# Create a few nodes.
node1 = Node('f')
node2 = Node('x')
node3 = Node('f')
node4 = Node('f')
node5 = Node('a')
node1.add_argument(node2)
node3.add_argument(node1)
node4.add_argument(node5)
# Create the union of nodes 1 and 2, 3 and 1, 5 and 3, and 4 and 1.
node1.union(node2)
node3.union(node1)
node5.union(node3)
node4.union(node1)
self.assertEqual(node1.parents, set())
self.assertEqual(node2.parents, {node1, node3, node4})
self.assertEqual(node3.parents, set())
self.assertEqual(node4.parents, set())
self.assertEqual(node5.parents, set())
self.assertEqual(node1.find, node2)
self.assertEqual(node2.find, node2)
self.assertEqual(node3.find, node2)
self.assertEqual(node4.find, node2)
self.assertEqual(node5.find, node2)
def test_merge(self):
# Create a few nodes.
node1 = Node('f')
node2 = Node('x')
node3 = Node('f')
node4 = Node('f')
node5 = Node('a')
node1.add_argument(node2)
node3.add_argument(node1)
node4.add_argument(node5)
# Merge nodes 1 and 2 and nodes 5 and 3.
self.assertTrue(node1.merge(node2))
self.assertTrue(node5.merge(node3))
# Now node 2 should be the representative.
self.assertEqual(node1.parents, set())
self.assertEqual(node1.find, node2)
self.assertEqual(node2.parents, {node1, node3, node4})
self.assertEqual(node2.find, node2)
self.assertEqual(node3.parents, set())
self.assertEqual(node3.find, node2)
self.assertEqual(node4.parents, set())
self.assertEqual(node4.find, node2)
self.assertEqual(node5.parents, set())
self.assertEqual(node5.find, node2)
# Trying to merge nodes 1 and 2 should return false as they are already
# congruent to each other.
self.assertFalse(node1.merge(node2))
def test_add_argument(self):
# A few nodes.
node1 = Node('f')
node2 = Node('g')
node3 = Node('h')
# The functions have the following forms: g(f, f), h(f)
node2.add_argument(node1)
node2.add_argument(node1)
node3.add_argument(node1)
# Now node 2 should have an argument list with two elements, both being
# node 1 and node 3 should have node 1 as its only argument. Node 1
# should have nodes 2 and 3 as its parents, but both only once.
self.assertEqual(node1.arguments, [])
self.assertEqual(node1.parents, {node2, node3})
self.assertEqual(node2.arguments, [node1, node1])
self.assertEqual(node2.parents, set())
self.assertEqual(node3.arguments, [node1])
self.assertEqual(node3.parents, set())
# Create a few nodes.
node1 = Node('f')
node2 = Node('x')
node3 = Node('f')
node4 = Node('f')
node5 = Node('a')
node1.add_argument(node2)
node3.add_argument(node1)
node4.add_argument(node5)
self.assertEqual(node1.arguments, [node2])
self.assertEqual(node2.arguments, [])
self.assertEqual(node3.arguments, [node1])
self.assertEqual(node4.arguments, [node5])
self.assertEqual(node5.arguments, [])
self.assertEqual(node1.parents, {node3})
self.assertEqual(node2.parents, {node1})
self.assertEqual(node3.parents, set())
self.assertEqual(node4.parents, set())
self.assertEqual(node5.parents, {node4})
node12.find = node5
# Create the lists to check.
merge_list = [
(node1, node2),
(node4, node6)
]
inequality_list = [
(node3, node5)
]
atom_list = []
'''
# Create the nodes.
node1 = Node('x')
node2 = Node('cons')
node3 = Node('x1')
node4 = Node('x2')
node5 = Node('y')
node6 = Node('cons')
node7 = Node('cdr')
node8 = Node('car')
node9 = Node('z')
node10 = Node('cons')
node11 = Node('cdr')
node12 = Node('car')
node13 = Node('car')
node14 = Node('cdr')
node15 = Node('car')
node16 = Node('cdr')
node17 = Node('car')
def test_union(self):
# Create a few nodes.
node1 = Node('f')
node2 = Node('x')
node3 = Node('f')
node4 = Node('f')
node5 = Node('a')
node1.add_argument(node2)
node3.add_argument(node1)
node4.add_argument(node5)
# Create the union of nodes 1 and 2, 3 and 1, 5 and 3, and 4 and 1.
node1.union(node2)
node3.union(node1)
node5.union(node3)
node4.union(node1)
self.assertEqual(node1.parents, set())
self.assertEqual(node2.parents, {node1, node3, node4})
self.assertEqual(node3.parents, set())
self.assertEqual(node4.parents, set())
self.assertEqual(node5.parents, set())
self.assertEqual(node1.find, node2)
self.assertEqual(node2.find, node2)
self.assertEqual(node3.find, node2)
self.assertEqual(node4.find, node2)
self.assertEqual(node5.find, node2)
