Add cost() to Action and Node, now sorting nodes in Planner.plan()

goap/src/goap/goap.py

@@ -199,6 +199,9 @@ def __init__(self, preconditions, effects):
     def __repr__(self):
         return '<Action type=%s>' % self.__class__.__name__
 
+    def cost(self):
+        return 1
+
     def run(self, next_worldstate):
         """
         next_worldstate: the worldstate that this action should lead to when run

goap/src/goap/inheriting.py

@@ -115,7 +115,10 @@ def __init__(self, memory, variable, increment=1):
         self._memory.declare_variable(self._variable)
 
     def __repr__(self):
-        return '<MemoryIncrementerAction var=%s incr=%s>' % (self._condition, self._increment)
+        return '<MemoryIncrementerAction var=%s incr=%s>' % (self._variable, self._increment)
+
+    def cost(self):
+        return abs(self._increment)
 
     def run(self, next_worldstate):
         self._memory.set_value(self._variable, self._memory.get_value(self._variable) + self._increment)

goap/src/goap/planning.py

@@ -33,6 +33,8 @@
 """
 
 
+from collections import deque
+
 from goap import ActionBag, WorldState
 
 
@@ -51,24 +53,15 @@ def __init__(self, worldstate, action, parent_nodes_path_list, parent_actions_pa
         self.parent_nodes_path_list = parent_nodes_path_list
         self.parent_actions_path_list = parent_actions_path_list
 
-#        self.valid_actionbag = ActionBag()
-
     def __repr__(self):
-        return '<Node %X action=%s worldstate=%s>' % (id(self), self.action, self.worldstate)
-
-#     def _check_and_add_actions(self, actionbag):
-#         for action in actionbag.generate_matching_actions():
-#              # TODO use difference subset
-# #         for action in actionbag.get():
-# #             if action.has_matching_effects(self.worldstate):
-# #                 print 'helping action: ', action
-#             self.valid_actionbag.add(action)
-# #             else:
-# #                 print 'helpless action: ', action
+        return '<Node %X cost=%s action=%s worldstate=%s>' % \
+            (id(self), self.cost(), self.action, self.worldstate)
+
+    def cost(self):
+        return len(self.parent_nodes_path_list) + \
+                self.action.cost() if self.action is not None else 0
 
     def get_child_nodes_for_valid_actions(self, actionbag):
-#         self._check_and_add_actions(actionbag)
-#        self.valid_actionbag = actionbag
         nodes = []
         for action in actionbag:
             nodes_path_list = self.parent_nodes_path_list[:]
@@ -109,7 +102,7 @@ def plan(self):
 
 #         worldstate = self._start_worldstate
 
-        child_nodes = {goal_node}
+        child_nodes = deque([goal_node])
 
 #         while not self._goal.is_valid(worldstate):
         count = 0
@@ -121,7 +114,7 @@ def plan(self):
             print "=Doing another planning loop="
             print 'nodes: ', child_nodes
 
-            current_node = child_nodes.pop()
+            current_node = child_nodes.popleft()
             print 'popping this: ', current_node
 #             print 'nodes: ', child_nodes, len(child_nodes)
 
@@ -140,9 +133,12 @@ def plan(self):
                     self._actionbag.generate_matching_actions(self._start_worldstate, current_node.worldstate))
             print 'new child nodes: ', new_child_nodes
 
-            child_nodes.update(new_child_nodes)
-
+            # add new nodes and sort. this is stable, so old nodes stay
+            # more left in the deque than new nodes with same weight
+            child_nodes.extend(new_child_nodes)
+            child_nodes = deque(sorted(child_nodes, key=Node.cost))
 
+        print 'No plan found.'
         return None