Change cost defaults and assure heuristic do not overstimate actual c…

…osts

goap/src/goap/common.py

@@ -286,7 +286,26 @@ def __repr__(self):
         return '<%s preconditions=%s effects=%s>' % (self.__class__.__name__, self._preconditions, self._effects)
 
     def cost(self):
-        return 1
+        """Return this action's cost value
+
+        Override to apply own cost calculation. The returned cost must not be
+        less than the number of declared effects!
+
+        To check this when overriding use validate_cost:
+            def cost(self):
+                return self.validate_cost(..custom cost calculation..)
+        """
+        return len(self._effects)
+
+    def validate_cost(self, cost):
+        """Can be used to validate custom cost calculations, see cost()"""
+        minimum_cost = len(self._effects)
+        if cost < minimum_cost:
+            print ("Warning: action %s proposed too small cost (%s), overriding with minimum cost (%d)"
+                   % (self.__class__.__name__, cost, minimum_cost))
+            cost = minimum_cost
+        return cost
+
 
     def run(self, next_worldstate):
         """

goap/src/goap/inheriting.py

@@ -123,7 +123,7 @@ def __repr__(self):
                             self._state_name, self._increment)
 
     def cost(self):
-        return abs(2 - float(1) / abs(self._increment))
+        return self.validate_cost(abs(2 - float(1) / abs(self._increment)))
 
     def run(self, next_worldstate):
         self._memory.set_value(self._state_name, self._memory.get_value(self._state_name) + self._increment)

goap/src/goap/planning.py

@@ -138,6 +138,9 @@ def _calc_heuristic_distance_for_node(self, start_worldstate):
                                 % (condition._state_name, distance_remaining, distance_total, relative_distance)
                         self.heuristic_distance += relative_distance
 
+            # make sure heuristic distance is less or equal the number of conditions
+            self.heuristic_distance = min(len(unsatisfied_conditions_set), self.heuristic_distance)
+
     # regressive planning
     def get_child_nodes_for_valid_actions(self, actions_generator, start_worldstate):
         assert len(self.possible_prev_nodes) == 0, "Node.get_child_nodes_for_valid_actions is probably not safe to be called twice"
@@ -226,9 +229,6 @@ def plan(self, start_worldstate=None, goal=None):
             # 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)
-            # TODO: will the planner be optimal, if a too high heuristic will favor
-            # another node, which reaches the start, but with more total cost than
-            # what the first node and a matching lower cost action will cost?
             child_nodes = deque(sorted(child_nodes, key=lambda node: node.total_cost()))
 
         print 'No plan found.'