diff options
Diffstat (limited to 'SA_POP/SANet/SANode.cpp')
| -rw-r--r-- | SA_POP/SANet/SANode.cpp | 1020 |
1 files changed, 1020 insertions, 0 deletions
diff --git a/SA_POP/SANet/SANode.cpp b/SA_POP/SANet/SANode.cpp new file mode 100644 index 00000000000..4ce882526d0 --- /dev/null +++ b/SA_POP/SANet/SANode.cpp @@ -0,0 +1,1020 @@ +// -*- C++ -*- + +//============================================================================= +/** + * @file SANode.cpp + * + * This file contains the Node, CondNode, & TaskNode class + * implementations for spreading activation network nodes. + * + * @author John S. Kinnebrew <john.s.kinnebrew@vanderbilt.edu> + */ +//============================================================================= + +#include "SANode.h" +#include "SANet_Exceptions.h" +#include <utility> +#include <iostream> + +using namespace SANet; + +Node::Node (NodeID ID, std::string name, MultFactor atten_factor) +: ID_ (ID), + name_ (name), + atten_factor_ (atten_factor), + step_ (0), + prob_changed_ (false), + util_changed_ (false) +{ + // Initialize probability and utility info. + pos_util_.utility = 0; + pos_util_.common.clear (); + neg_util_.utility = 0; + neg_util_.common.clear (); + true_prob_.probability = 0; + true_prob_.common.clear (); + false_prob_.probability = 0; + false_prob_.common.clear (); + + // Initialize (empty) pre and post node maps and links. + pre_nodes_.clear (); + pre_links_.clear (); + post_nodes_.clear (); + post_links_.clear (); +}; + +Node::~Node () +{ + // Nothing to do. +}; + +Utility_Info Node::get_reward (int step) +{ + // Check to ensure step is the current step, or else throw exception + if (step != step_) { + throw Invalid_Step (); + } + + // Return positive utility info. + return pos_util_; +}; + +Probability_Info Node::get_prob (int step, bool value) +{ + // Check to ensure step is the current step, or else throw exception + if (step != step_) { + throw Invalid_Step (); + } + + // Return probability for true if value is true. + if (value) { + return true_prob_; + } + // Otherwise return probability for false. + return false_prob_; +}; + +bool Node::prob_changed (void) +{ + return prob_changed_; +}; + +bool Node::util_changed (void) +{ + return util_changed_; +}; + +std::string Node::get_name (void) +{ + return name_; +}; + +NodeID Node::get_ID (void) +{ + return ID_; +}; + +// Get pre-links (nodes with links to this node). +const LinkMap& Node::get_pre (void) +{ + return this->pre_links_; +}; + +// Get post-links (nodes with links from this node). +const LinkMap& Node::get_post (void) +{ + return this->post_links_; +}; + +TaskNode::TaskNode (NodeID ID, std::string name, MultFactor atten_factor, + TaskCost cost, Probability prior_prob) +: Node (ID, name, atten_factor), + cost_ (cost), + prior_prob_ (prior_prob) +{ + // Nothing to do. +}; + +TaskNode::~TaskNode (void) +{ + // Nothing to do. +}; + +bool TaskNode::update (void) +{ + // Reset change flags. + prob_changed_ = false; + util_changed_ = false; + + //****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP + // Probability flag is not being set correctly, so run to max steps... + prob_changed_ = true; + util_changed_ = true; + //****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP + + // Flag for detection of loops. + bool is_loop = false; + + // Variables for current node and link in updates. + CondNode *cur_node; + CondID cur_ID; + LinkMap::iterator link_iter; + LinkWeight cur_weight; + MultFactor cur_mult; + + // Variables for current probability value. + Probability_Info cur_prob; + ProbabilityMap::iterator true_prob_iter; + ProbabilityMap::iterator false_prob_iter; + ProbabilityMap::iterator prev_prob_iter; + + // Reset probability info. + true_prob_.probability = prior_prob_; + //****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP + // clearing common probabilities set removes self (if "loop" found where + // this node is receiving utility from same goal on multiple paths). so + // will never realize that nothing has changed. + // and (possible) common probabilities are never removed from the set once + // detected so this does not need to be cleared as long as any + // changed probabilities are appropriately changed in common set... right??? + true_prob_.common.clear (); + + // Update probability. + for (NodeMap::iterator node_iter = pre_nodes_.begin (); + node_iter != pre_nodes_.end (); node_iter++) + { + // Get current node info. + cur_ID = node_iter->first; + cur_node = (CondNode *) node_iter->second; + + // Get current link info. + link_iter = pre_links_.find(cur_ID); + if (link_iter == pre_links_.end ()) { + throw Update_Error (); + } + cur_weight = link_iter->second; + + // Get current probability info. + try { + if (cur_weight >= 0) { + cur_prob = cur_node->get_prob (step_, true); + } else { + cur_prob = cur_node->get_prob (step_, false); + } + } catch (Invalid_Step e) { + std::cerr << "Error in task node update: Invalid step value."; + std::cerr << std::endl; + return false; + } catch (...) { + std::cerr << "Unexpected exception thrown in task node update."; + std::cerr << std::endl; + return false; + } + + // Get conditional probability of success for true precondition. + true_prob_iter = pre_true_probs_.find (cur_ID); + if (true_prob_iter == pre_true_probs_.end ()) { + throw Update_Error (); + } + + // Get conditional probability of success for false precondition. + false_prob_iter = pre_false_probs_.find (cur_ID); + if (false_prob_iter == pre_false_probs_.end ()) { + throw Update_Error (); + } + + // Update true probability depending on sign of link weight. + if (cur_weight >= 0) { + // Update conditional probability of success given this node. + cur_prob.probability = ((cur_prob.probability * true_prob_iter->second) + + ((1 - cur_prob.probability) * false_prob_iter->second)); + + // Update probability. + true_prob_.probability = true_prob_.probability * cur_prob.probability / + prior_prob_; + } else { + // Update conditional probability of success given this node. + cur_prob.probability = ((cur_prob.probability * false_prob_iter->second) + + ((1 - cur_prob.probability) * true_prob_iter->second)); + + // Update probability. + true_prob_.probability = true_prob_.probability * cur_prob.probability / + prior_prob_; + } + + // Update prob_changed_ flag if necessary. + if (cur_node->prob_changed ()) { + prob_changed_ = true; + } + + // Update common probabilities and handle duplicates. + for (ProbabilityMap::iterator prob_iter = cur_prob.common.begin (); + prob_iter != cur_prob.common.end (); prob_iter++) + { + prev_prob_iter = true_prob_.common.find (prob_iter->first); + + // If a common probability is already in the probability map, + // divide from probability, otherwise add it to the probability map. + if (prev_prob_iter != true_prob_.common.end ()) + { +#if defined (SANET_DEBUG) + std::cout << std::endl << "DEBUG in TaskNode::update()... (" << ID_ << ") Duplicate common probability found:" << std::endl; + std::cout << "Probability from " << prev_prob_iter->first << " = " << prev_prob_iter->second << std::endl << std::endl; +#endif /* SANET_DEBUG */ + true_prob_.probability = true_prob_.probability / + prev_prob_iter->second; + } else { + true_prob_.common.insert (std::make_pair (prob_iter->first, + prob_iter->second)); + } + } + } + + // Variables for current utility updates. + Utility_Info cur_util; + UtilityMap::iterator prev_util_iter; + + // Reset utility info. + pos_util_.utility = 0; + pos_util_.common.clear (); + neg_util_.utility = 0; + neg_util_.common.clear (); + + // Update utility. + for (NodeMap::iterator node_iter = post_nodes_.begin (); + node_iter != post_nodes_.end (); node_iter++) + { + // Get current node info. + cur_ID = node_iter->first; + cur_node = (CondNode *) node_iter->second; + + // Update util_changed_ flag if necessary. + if (cur_node->util_changed ()) { + util_changed_ = true; + } + + // Get current link info. + link_iter = post_links_.find(cur_ID); + if (link_iter == post_links_.end ()) { + throw Update_Error (); + } + cur_weight = link_iter->second; + + // Set multiplier as attenuation factor times link weight, probability + // that task will succeed, and probability that effect node has the + // opposite value from what task would change it to. + if (cur_weight >= 0) { + cur_mult = atten_factor_ * cur_weight * true_prob_.probability * + cur_node->get_prob (step_, false).probability; + } else { + cur_mult = atten_factor_ * cur_weight * true_prob_.probability * + cur_node->get_prob (step_, true).probability; + } + + // Get current utility info. + try { + cur_util = cur_node->get_reward (step_); + } catch (Invalid_Step e) { + std::cerr << "Error in task node update: Invalid step value."; + std::cerr << std::endl; + return false; + } catch (...) { + std::cerr << "Unexpected exception thrown in task node update."; + std::cerr << std::endl; + return false; + } + + // Updated utility based on current multiplier. + cur_util.utility = cur_mult * cur_util.utility; + + // Update positive or negative utilities depending on sign of link weight. + if (cur_weight >= 0) { + // Add utility to expected utility value. + pos_util_.utility += cur_util.utility; + + // Update all component utilities based on link weight, and handle + // utilities from common goals. + for (UtilityMap::iterator util_iter = cur_util.common.begin (); + util_iter != cur_util.common.end (); util_iter++) + { + util_iter->second = cur_mult * util_iter->second; + prev_util_iter = pos_util_.common.find (util_iter->first); + + // If a utility from this goal is already in the utility map, + // delete smaller utility value, otherwise add the utility from this + // goal to the utility map. + if (prev_util_iter != pos_util_.common.end ()) + { +#if defined (SANET_DEBUG) + std::cout << std::endl << "DEBUG in TaskNode::update()... (" << ID_ << ") Duplicate utility from goal found:" << std::endl; + std::cout << "Utility from " << prev_util_iter->first << " = " << prev_util_iter->second << std::endl << std::endl; +#endif /* SANET_DEBUG */ + if (prev_util_iter->second < util_iter->second) { + pos_util_.utility -= prev_util_iter->second; + } else { + pos_util_.utility -= util_iter->second; + } + // Set loop flag. + is_loop = true; + } else { + pos_util_.common.insert (std::make_pair (util_iter->first, + util_iter->second)); + } + } + } else { + // Add utility to expected utility value. + neg_util_.utility += cur_util.utility; + + // Update all component utilities based on link weight, and handle + // utilities from common goals. + for (UtilityMap::iterator util_iter = cur_util.common.begin (); + util_iter != cur_util.common.end (); util_iter++) + { + util_iter->second = cur_mult * util_iter->second; + prev_util_iter = neg_util_.common.find (util_iter->first); + + // If a utility from this goal is already in the utility map, + // delete larger (less negative) utility value, otherwise add the + // utility from this goal to the utility map. + if (prev_util_iter != neg_util_.common.end ()) + { + if (prev_util_iter->second > util_iter->second) { + neg_util_.utility -= prev_util_iter->second; + } else { + neg_util_.utility -= util_iter->second; + } + // Set loop flag. + is_loop = true; + } else { + neg_util_.common.insert (std::make_pair (util_iter->first, + util_iter->second)); + } + } + } + } + +//*****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG**** + // Include cost in current expected utility. +// pos_util_.utility -= cost_; +//*****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG****DEBUG**** + + // If a loop was detected, add current probability to common probabilities. + if (is_loop) { +#if defined (SANET_DEBUG) + if (DEBUG) { + std::cout << std::endl << "DEBUG in TaskNode::update()... (" << ID_ << ") Loop detected, adding this node to common probabilities" << std::endl << std::endl; +#endif /* SANET_DEBUG */ + // Update common probabilities for true probability. + ProbabilityMap::iterator prob_iter = true_prob_.common.find (ID_); + if (prob_iter != true_prob_.common.end ()) { + if (prob_iter->second != true_prob_.probability) { + prob_iter->second = true_prob_.probability; + prob_changed_ = true; + } + } else { + true_prob_.common.insert (std::make_pair (ID_, + true_prob_.probability)); + // Update prob_changed_ flag. + prob_changed_ = true; + } + } + + // Set false probability (i.e. probability of task failure) to 1 minus + // probability of success. + false_prob_.probability = 1 - true_prob_.probability; + + // Update step (at end of step for task node). + step_++; + + // Return boolean changed value based on change flags. + return (prob_changed_ || util_changed_); + +}; + +Utility TaskNode::get_utility (int step) +{ + // Check to ensure step is the current step, or else throw exception + if (step != step_) { + throw Invalid_Step (); + } + + // Variable for summing expected utility. + Utility utility = 0; + + // Add positive expected utility, negative expected utility values, and + // subtract cost. + utility += pos_util_.utility + neg_util_.utility - cost_; + + return utility; +}; + +void TaskNode::print (std::basic_ostream<char, std::char_traits<char> >& + strm, bool verbose) +{ + // Print node name and ID. + strm << "Task Node \"" << name_ << "\":" << std::endl; + strm << " ID: " << ID_ << std::endl; + + // Print cost and expected utility. + strm << " Cost: " << cost_ << std::endl; + strm << " Expected Utility: " << get_utility (step_) << std::endl; + + // If verbose, print positive and negative component utilities from goals. + if (verbose) { + // Print positive utility and its component utilities. + strm << " Positive utility received: " << pos_util_.utility << std::endl; + for (UtilityMap::iterator iter = pos_util_.common.begin (); + iter != pos_util_.common.end (); iter++) + { + strm << " Component expected utility from goal " << iter->first; + strm << " = " << iter->second; + strm << std::endl; + } + + // Print negative utility and its component utilities. + strm << " Negative utility received: " << neg_util_.utility << std::endl; + for (UtilityMap::iterator iter = neg_util_.common.begin (); + iter != neg_util_.common.end (); iter++) + { + strm << " Component expected utility from goal " << iter->first; + strm << " = " << iter->second; + strm << std::endl; + } + } + + // Print probability of task success. + strm << " Probability (success): " << true_prob_.probability << std::endl; + + // If verbose, print common utility + if (verbose) { + for (ProbabilityMap::iterator iter = true_prob_.common.begin (); + iter != true_prob_.common.end (); iter++) + { + strm << " Component probability (possible loops only) from "; + strm << iter->first << " = " << iter->second; + strm << std::endl; + } + } + + // If verbose, print probability of task failure. + if (verbose) { + strm << " Probability (failure): " << false_prob_.probability; + strm << std::endl; + } + + // Current link map iterator input and output link printing loops. + LinkMap::iterator cur_link_iter; + + // Print node input links. + strm << " In Links (preconditions):" << std::endl; + for (NodeMap::iterator node_iter = pre_nodes_.begin (); + node_iter != pre_nodes_.end (); node_iter++) + { + // Print input node name and ID. + strm << " " << node_iter->first << " "; + strm << "(" << node_iter->second->get_name () << "); "; + + // Find link for this node. + cur_link_iter = pre_links_.find (node_iter->first); + + // If this node couldn't be found in pre_links_ print error, + // otherwise print link weight. + if (cur_link_iter == pre_links_.end ()) { + strm << "ERROR: weight not found"; + } else { + strm << "weight = " << cur_link_iter->second; + } + strm << std::endl; + + // If verbose, print conditional probabilities of success for precondition. + if (verbose) { + strm << " Conditional probability if true: "; + strm << pre_true_probs_.find (node_iter->first)->second << std::endl; + strm << " Conditional probability if false: "; + strm << pre_false_probs_.find (node_iter->first)->second << std::endl; + } + } + + // Print node output links. + strm << " Out Links (effects):" << std::endl; + for (NodeMap::iterator node_iter = post_nodes_.begin (); + node_iter != post_nodes_.end (); node_iter++) + { + // Print output node name and ID. + strm << " " << node_iter->first << " "; + strm << "(" << node_iter->second->get_name () << "); "; + + // Find link for this node. + cur_link_iter = post_links_.find (node_iter->first); + + // If this node couldn't be found in post_links_ print error, + // otherwise print link weight. + if (cur_link_iter == post_links_.end ()) { + strm << "ERROR: weight not found"; + } else { + strm << "weight = " << cur_link_iter->second; + } + strm << std::endl; + } +}; + +void TaskNode::add_precond (CondID ID, CondNode *node, Probability true_prob, + Probability false_prob) +{ + // Add node to pre-nodes. + pre_nodes_.insert (std::make_pair (ID, node)); + + // Add probabilities. + pre_true_probs_.insert (std::make_pair (ID, true_prob)); + pre_false_probs_.insert (std::make_pair (ID, false_prob)); + + // Add link weight. + LinkWeight weight = (true_prob - false_prob)/(true_prob + false_prob); + pre_links_.insert (std::make_pair (ID, weight)); + + // Add link for precondition node. + node->add_post_link (ID_, this, weight); +}; + +void TaskNode::add_effect (CondID ID, CondNode *node, LinkWeight weight) +{ + // Add node to post-nodes. + post_nodes_.insert (std::make_pair (ID, node)); + + // Add link weight. + post_links_.insert (std::make_pair (ID, weight)); + + // Add link for precondition node. + node->add_pre_link (ID_, this, weight); +}; + +CondNode::CondNode (CondID ID, std::string name, MultFactor atten_factor, + Probability true_prob, Probability false_prob, Utility goal_util, CondKind condkind) +: Node (ID, name, atten_factor), + goal_util_ (goal_util), + init_true_prob_ (true_prob), + true_prob_from_ (ID), + false_prob_from_ (ID), + cond_kind_ (condkind) +{ + // Set initial probabilities. + true_prob_.probability = true_prob; + false_prob_.probability = false_prob; + + // Set prob_changed_ flag. + prob_changed_ = true; + + // If this node is a goal, set util_changed_ flag and add goal utility + // to positive utilities. + if (goal_util > 0) { + util_changed_ = true; + pos_util_.utility = goal_util; + pos_util_.common.insert (std::make_pair (ID, goal_util)); + } +}; + +CondNode::~CondNode (void) +{ + // Nothing to do. +}; + +// Get initial/current probability. +Probability CondNode::get_init_prob (bool value) +{ + if (value) + return this->init_true_prob_; + + return (1.0 - this->init_true_prob_); +}; + +// Update initial/current probability. +void CondNode::set_init_prob (Probability init_true_prob) +{ + this->init_true_prob_ = init_true_prob; + this->prob_changed_ = true; +}; + +void CondNode::print (std::basic_ostream<char, std::char_traits<char> >& + strm, bool verbose) +{ + // Print node name and ID. + strm << "Condition Node \"" << name_ << "\":" << std::endl; + strm << " ID: " << ID_ << std::endl; + + // Print goal utility. + strm << " Goal utility: " << goal_util_ << std::endl; + + // Print positive utility. + strm << " Expected utility for true value: " << pos_util_.utility; + strm << std::endl; + + // If verbose, print component utilities from all goals. + if (verbose) { + for (UtilityMap::iterator iter = pos_util_.common.begin (); + iter != pos_util_.common.end (); iter++) + { + strm << " Component expected utility from goal " << iter->first; + strm << " = " << iter->second; + strm << std::endl; + } + } + + // Print negative utility. + strm << " Expected utility for false value: " << neg_util_.utility; + strm << std::endl; + + // If verbose, print component utilities from all goals. + if (verbose) { + for (UtilityMap::iterator iter = neg_util_.common.begin (); + iter != neg_util_.common.end (); iter++) + { + strm << " Component expected utility from goal " << iter->first; + strm << " = " << iter->second; + strm << std::endl; + } + } + + // Print probability (maximum) that condition is true. + strm << " Probability (true): " << true_prob_.probability << std::endl; + + // If verbose, print component probabilities for loops. + if (verbose) { + for (ProbabilityMap::iterator iter = true_prob_.common.begin (); + iter != true_prob_.common.end (); iter++) + { + strm << " Component probability (possible loops only) from "; + strm << iter->first << " = " << iter->second; + strm << std::endl; + } + } + + // Print probability (maximum) that condition is false. + strm << " Probability (false): " << false_prob_.probability << std::endl; + + // If verbose, print component probabilities for loops. + if (verbose) { + for (ProbabilityMap::iterator iter = false_prob_.common.begin (); + iter != false_prob_.common.end (); iter++) + { + strm << " Component probability (possible loops only) from "; + strm << iter->first << " = " << iter->second; + strm << std::endl; + } + } + + // Current link map iterator input and output link printing loops. + LinkMap::iterator cur_link_iter; + + // Print node input links. + strm << " In Links (tasks that change this condition):" << std::endl; + for (NodeMap::iterator node_iter = pre_nodes_.begin (); + node_iter != pre_nodes_.end (); node_iter++) + { + // Print input node name and ID. + strm << " " << node_iter->first << " "; + strm << "(" << node_iter->second->get_name () << "); "; + + // Find link for this node. + cur_link_iter = pre_links_.find (node_iter->first); + + // If this node couldn't be found in pre_links_ print error, + // otherwise print link weight. + if (cur_link_iter == pre_links_.end ()) { + strm << "ERROR: weight not found"; + } else { + strm << "weight = " << cur_link_iter->second; + } + strm << std::endl; + } + + // Print node output links. + strm << " Out Links (tasks that have this precondition):" << std::endl; + for (NodeMap::iterator node_iter = post_nodes_.begin (); + node_iter != post_nodes_.end (); node_iter++) + { + // Print output node name and ID. + strm << " " << node_iter->first << " "; + strm << "(" << node_iter->second->get_name () << "); "; + + // Find link for this node. + cur_link_iter = post_links_.find (node_iter->first); + + // If this node couldn't be found in post_links_ print error, + // otherwise print link weight. + if (cur_link_iter == post_links_.end ()) { + strm << "ERROR: weight not found"; + } else { + strm << "weight = " << cur_link_iter->second; + } + strm << std::endl; + } +}; + +bool CondNode::update (void) +{ + // Reset change flags. + prob_changed_ = false; + util_changed_ = false; + + // Update step (at beginning of step for condition node). + step_++; + + // Flag for detection of loops. + bool is_loop = false; + + // Variables for current node and link in updates. + TaskNode *cur_node; + TaskID cur_ID; + LinkMap::iterator link_iter; + LinkWeight cur_weight; + MultFactor cur_mult; + + // Reset utility info. + pos_util_.utility = 0; + pos_util_.common.clear (); + neg_util_.utility = 0; + neg_util_.common.clear (); + // If this node is a goal, set add goal utility to positive utilities. + if (goal_util_ > 0) { + pos_util_.utility = goal_util_; + pos_util_.common.insert (std::make_pair (ID_, goal_util_)); + }; + + // Variables for current utility updates. + Utility_Info cur_util; + UtilityMap::iterator prev_util_iter; + + // Update utility. + for (NodeMap::iterator node_iter = post_nodes_.begin (); + node_iter != post_nodes_.end (); node_iter++) + { + // Get current node info. + cur_ID = node_iter->first; + cur_node = (TaskNode *) node_iter->second; + + // Update util_changed_ flag if necessary. + if (cur_node->util_changed ()) { + util_changed_ = true; + } + + // Get current link info. + link_iter = post_links_.find(cur_ID); + if (link_iter == post_links_.end ()) { + throw Update_Error (); + } + cur_weight = link_iter->second; + + // Set multiplier as attenuation factor times link weight. + cur_mult = atten_factor_ * cur_weight; + + // Get current utility info. + try { + cur_util = cur_node->get_reward (step_); + } catch (Invalid_Step e) { + std::cerr << "Error in condition node update: Invalid step value."; + std::cerr << std::endl; + return false; + } catch (...) { + std::cerr << "Unexpected exception thrown in condition node update."; + std::cerr << std::endl; + return false; + } + + // Updated utility based on current multiplier. + cur_util.utility = cur_mult * cur_util.utility; + + // Update positive or negative utilities depending on sign of link weight. + if (cur_weight >= 0) { + // Add utility to expected utility value. + pos_util_.utility += cur_util.utility; + + // Update all component utilities based on link weight, and handle + // utilities from common goals. + for (UtilityMap::iterator util_iter = cur_util.common.begin (); + util_iter != cur_util.common.end (); util_iter++) + { + util_iter->second = cur_mult * util_iter->second; + prev_util_iter = pos_util_.common.find (util_iter->first); + + // If a utility from this goal is already in the utility map, + // delete smaller utility value, otherwise add the utility from this + // goal to the utility map. + if (prev_util_iter != pos_util_.common.end ()) + { +#if defined (SANET_DEBUG) + std::cout << std::endl << "DEBUG in CondNode::update()... (" << ID_ << ") Duplicate utility from goal found:" << std::endl; + std::cout << "Utility from " << prev_util_iter->first << " = " << prev_util_iter->second << std::endl << std::endl; +#endif /* SANET_DEBUG */ + if (prev_util_iter->second < util_iter->second) { + pos_util_.utility -= prev_util_iter->second; + } else { + pos_util_.utility -= util_iter->second; + } + // Set loop flag. + is_loop = true; + } else { + pos_util_.common.insert (std::make_pair (util_iter->first, + util_iter->second)); + } + } + } else { + // Add utility to expected utility value. + neg_util_.utility += cur_util.utility; + + // Update all component utilities based on link weight, and handle + // utilities from common goals. + for (UtilityMap::iterator util_iter = cur_util.common.begin (); + util_iter != cur_util.common.end (); util_iter++) + { + util_iter->second = cur_mult * util_iter->second; + prev_util_iter = neg_util_.common.find (util_iter->first); + + // If a utility from this goal is already in the utility map, + // delete larger (less negative) utility value, otherwise add the + // utility from this goal to the utility map. + if (prev_util_iter != neg_util_.common.end ()) + { + if (prev_util_iter->second > util_iter->second) { + neg_util_.utility -= prev_util_iter->second; + } else { + neg_util_.utility -= util_iter->second; + } + // Set loop flag. + is_loop = true; + } else { + neg_util_.common.insert (std::make_pair (util_iter->first, + util_iter->second)); + } + } + } + } + + // Variables for current probability value. + Probability_Info cur_prob; + ProbabilityMap::iterator prob_iter; + + // Update probability. + for (NodeMap::iterator node_iter = pre_nodes_.begin (); + node_iter != pre_nodes_.end (); node_iter++) + { + // Get current node info. + cur_ID = node_iter->first; + cur_node = (TaskNode *) node_iter->second; + + // Get current link info. + link_iter = pre_links_.find(cur_ID); + if (link_iter == pre_links_.end ()) { + throw Update_Error (); + } + cur_weight = link_iter->second; + + // Set multiplier as absolute value of link weight. + if (cur_weight >= 0) { + cur_mult = cur_weight; + } else { + cur_mult = -1 * cur_weight; + } + + // Get current probability info. + try { + cur_prob = cur_node->get_prob (step_); + } catch (Invalid_Step e) { + std::cerr << "Error in condition node update: Invalid step value."; + std::cerr << std::endl; + return false; + } catch (...) { + std::cerr << "Unexpected exception thrown in condition node update."; + std::cerr << std::endl; + return false; + } + + // Update probability based on current multiplier. + cur_prob.probability = cur_mult * cur_prob.probability; + + // Update true or false probability depending on sign of link weight. + if (cur_weight >= 0) { + // Change true probability if this probability is higher than previous. + if (cur_prob.probability > true_prob_.probability) { + true_prob_ = cur_prob; + true_prob_from_ = cur_ID; + + // Update prob_changed_ flag. + prob_changed_ = true; + } + // Otherwise check for newly detected common probabilities if current + // probability is from this node. + else if (true_prob_from_ == cur_ID) { + for (prob_iter = cur_prob.common.begin (); + prob_iter != cur_prob.common.end (); prob_iter++) + { + // If this common probability is not in current common probability + // map, add it. + if (true_prob_.common.find (prob_iter->first) == + true_prob_.common.end ()) + { + // Add common probability. + true_prob_.common.insert (std::make_pair (prob_iter->first, + prob_iter->second)); + + // Update prob_changed_ flag. + prob_changed_ = true; + } + } + } + } else { + // Change false probability if this probability is higher than previous. + if (cur_prob.probability > false_prob_.probability) { + false_prob_ = cur_prob; + false_prob_from_ = cur_ID; + + // Update prob_changed_ flag. + prob_changed_ = true; + } + // Otherwise check for newly detected common probabilities if current + // probability is from this node. + else if (false_prob_from_ == cur_ID) { + for (prob_iter = cur_prob.common.begin (); + prob_iter != cur_prob.common.end (); prob_iter++) + { + // If this common probability is not in current common probability + // map, add it. + if (false_prob_.common.find (prob_iter->first) == + false_prob_.common.end ()) + { + // Add common probability. + false_prob_.common.insert (std::make_pair (prob_iter->first, + prob_iter->second)); + + // Update prob_changed_ flag. + prob_changed_ = true; + } + } + } + } + } + + // If a loop was detected, add current probability to common probabilities. + if (is_loop) { +#if defined (SANET_DEBUG) + std::cout << std::endl << "DEBUG in CondNode::update()... (" << ID_ << ") Loop detected, adding this node to common probabilities" << std::endl << std::endl; +#endif /* SANET_DEBUG */ + // Update common probabilities for true probability. + prob_iter = true_prob_.common.find (ID_); + if (prob_iter != true_prob_.common.end ()) { + prob_iter->second = true_prob_.probability; + } else { + true_prob_.common.insert (std::make_pair (ID_, + true_prob_.probability)); + // Update prob_changed_ flag. + prob_changed_ = true; + } + } + + // Return boolean changed value based on change flags. + return (prob_changed_ || util_changed_); +}; + +void CondNode::add_pre_link (TaskID ID, TaskNode *node, LinkWeight weight) +{ + // Add node to pre-nodes. + pre_nodes_.insert (std::make_pair (ID, node)); + + // Add link weight. + pre_links_.insert (std::make_pair (ID, weight)); +}; + +void CondNode::add_post_link (TaskID ID, TaskNode *node, LinkWeight weight) +{ + // Add node to post-nodes. + post_nodes_.insert (std::make_pair (ID, node)); + + // Add link weight. + post_links_.insert (std::make_pair (ID, weight)); +}; + +/// Get the kind +CondKind CondNode::get_cond_kind() +{ + return this->cond_kind_; +} + |