- Main Page
- Namespaces
- Classes
- Files
- Related Pages

# /Users/fpy/titan-1-5-MBARI/Mers/Estimation/Discrete/Trigger.cpp

    00001 //==============================================================================
    00002 // Copyright (c) 2004           Model-based Embedded and Robotic Systems Group
    00003 // All Rights Reserved          Massachusetts Institute of Technology
    00004 //
    00005 // This software is provided as is. Model-based Embedded and Robotic Systems
    00006 // (MERS) group does not assume any responsibility.
    00007 //==============================================================================
    00008
    00009 #include "Trigger.h"
    00010
    00011 #ifdef _MSC_VER
    00012 #pragma warning( disable : 4786 )
    00013 #endif
    00014
    00015 #include <Mers/Models/Base/BaseModelVariable.h>
    00016 #include <Mers/Models/Base/BaseModelVariableArray.h>
    00017 #include <Mers/Models/Base/BaseModelAssignment.h>
    00018 #include <Mers/Models/Base/BaseModelAssignmentArray.h>
    00019 #include <Mers/Models/Base/BaseModelDomain.h>
    00020
    00021 #include "Dissent.h"
    00022 #include "CompiledTransition.h"
    00023 #include "CompiledObject.h"
    00024 #include "Trajectories.h"
    00025 #include "Estimate.h"
    00026
    00027 #include <cassert>
    00028
    00029 using namespace Mers::Estimation::Discrete;
    00030 using Mers::Models::Base::BaseModelVariable;
    00031 using Mers::Models::Base::BaseModelVariableArray;
    00032 using Mers::Models::Base::BaseModelAssignment;
    00033 using Mers::Models::Base::BaseModelAssignmentArray;
    00034 using Mers::Models::Base::BaseModelDomain;
    00035 using Mers::Utils::Containers::IntegerArray;
    00036 using std::vector;
    00037 using std::ostream;
    00038 using std::cerr;
    00039 using std::cout;
    00040 using std::endl;
    00041
    00042 const int Trigger::UNDEFINED = -1;
    00043
    00044         //-----------------------------------------------
    00045         //      CONSTRUCTOR
    00046         //-----------------------------------------------
    00047
    00048
    00049 Trigger::Trigger(const BaseModelVariableArray * stateVarArray, const IntegerArray * pMap,
    00050                 const unsigned int totalNumVars, vector<const Dissent *> diss,
    00051                 vector<const CompiledTransition *> compT, const Trajectories * trajs,
    00052                 unsigned short db)
    00053 : dissents(diss), compiledTransitions(compT), assignmentsHaveChanged(true),
    00054   allStateVars(stateVarArray), numVariablesInModel(totalNumVars),
    00055   numComponentsInModel(stateVarArray->getLength()), probabilityMap(pMap), debug(db)
    00056 {
    00057         unsigned int i,j;
    00058         unsigned int var, val;
    00059         unsigned int compObjNum = 0;
    00060         unsigned int antNum = 0;
    00061
    00062         // Initialize arrays
    00063         for(i=0; i<numVariablesInModel; i++)
    00064                 for(j=0; j<MAX_DOMAIN_SIZE; j++)
    00065                         lookUpTable[i][j] = NULL;
    00066
    00067
    00068         // Put dissent entries in lookUpTable
    00069         vector<const Dissent *>::iterator dissentIter;
    00070         for(dissentIter = dissents.begin(); dissentIter != dissents.end(); dissentIter++)
    00071         {
    00072                 for(antNum=0; antNum < (*dissentIter)->getAntecedentLength(); antNum++)
    00073                 {
    00074                         var = (*dissentIter)->getAntecedentVariableUID(antNum);
    00075                         val = (*dissentIter)->getAntecedentValueUID(antNum);
    00076                         assert( var < MAX_NUM_VARIABLES );
    00077                         assert( val < MAX_DOMAIN_SIZE );
    00078                         if(lookUpTable[var][val] == NULL)
    00079                                 lookUpTable[var][val] = new IntegerArray();
    00080
    00081                         // Add entry into integer array
    00082                         lookUpTable[var][val]->pushBack(compObjNum);
    00083
    00084                         if(debug > 1)
    00085                                 cout << "Adding " << compObjNum << " to lookUpTable[" << var << "][" << val << "]" << endl;
    00086                 }
    00087                 compObjNum++;
    00088         }
    00089
    00090         // Put compiled transitions into the lookUpTable
    00091         vector<const CompiledTransition *>::iterator compTransIter;
    00092         for(compTransIter = compiledTransitions.begin(); compTransIter != compiledTransitions.end(); compTransIter++)
    00093         {
    00094                 for(antNum=0; antNum < (*compTransIter)->getAntecedentLength(); antNum++)
    00095                 {
    00096                         var = (*compTransIter)->getAntecedentVariableUID(antNum);
    00097                         val = (*compTransIter)->getAntecedentValueUID(antNum);
    00098                         assert( var < MAX_NUM_VARIABLES );
    00099                         assert( val < MAX_DOMAIN_SIZE );
    00100                         if(lookUpTable[var][val] == NULL)
    00101                                 lookUpTable[var][val] = new IntegerArray();
    00102
    00103                         // Add entry into integer array
    00104                         lookUpTable[var][val]->pushBack(compObjNum);
    00105
    00106                         if(debug > 1)
    00107                                 cout << "Adding " << compObjNum << " to lookUpTable[" << var << "][" << val << "]" << endl;
    00108                 }
    00109                 compObjNum++;
    00110         }
    00111
    00112         // Place dissents and compiled transitions into the compiled object list in
    00113         //  their proper locations
    00114         changeTrajectories(trajs);
    00115 }
    00116
    00117
    00118         //-----------------------------------------------
    00119         //      DESTRUCTOR
    00120         //-----------------------------------------------
    00121
    00122
    00123 Trigger::~Trigger()
    00124 {
    00125         // delete all dissents
    00126         while(!dissents.empty())
    00127         {
    00128                 const Dissent * dis = dissents.back();
    00129                 delete dis;
    00130                 dissents.pop_back();
    00131
    00132         }
    00133         // delete all compiled transitions
    00134         while(!compiledTransitions.empty())
    00135         {
    00136                 const CompiledTransition * ct = compiledTransitions.back();
    00137                 delete ct;
    00138                 compiledTransitions.pop_back();
    00139         }
    00140         // delete all compiled objects
    00141         while(!allCompiledObjects.empty())
    00142         {
    00143                 CompiledObject * co = allCompiledObjects.back();
    00144                 delete co;
    00145                 allCompiledObjects.pop_back();
    00146         }
    00147
    00148         // delete all integer arrays in lookUpTable
    00149         for(unsigned int i=0; i<numVariablesInModel; i++)
    00150                 for(unsigned int j=0; j<MAX_DOMAIN_SIZE; j++)
    00151                         if(lookUpTable[i][j] != NULL)
    00152                                 delete lookUpTable[i][j];
    00153
    00154 }
    00155
    00156
    00157
    00158         //-----------------------------------------------
    00159         //      PUBLIC METHODS
    00160         //-----------------------------------------------
    00161
    00162 bool
    00163 Trigger::insertAssignment(const BaseModelAssignment * assign)
    00164 {
    00165         bool successFlag = true;
    00166
    00167         if(debug > 0)
    00168                 cout << "Inserting assignment into trigger..." << endl;
    00169
    00170         if(assign->getVariable()->getVariableType() == BaseModelVariable::STATE)
    00171         {
    00172                 cerr << "<Trigger> ERROR: You can only assign values to observables and commands." << endl;
    00173                 return false;
    00174         }
    00175         else if(assign->getVariable()->getVariableType() == BaseModelVariable::CONTROL ||
    00176                 assign->getVariable()->getVariableType() == BaseModelVariable::OBSERVABLE)
    00177         {
    00178                 for(unsigned int i=0; i < currentTrajectories->getNumberEstimates(); i++)
    00179                         if(!insertAssignment(assign, i))
    00180                                 successFlag = false;
    00181         }
    00182         else
    00183         {
    00184                 cerr << "<Trigger> ERROR: Unknown variable type!" << endl;
    00185                 return false;
    00186         }
    00187
    00188         return successFlag;
    00189 }
    00190
    00191
    00192 bool
    00193 Trigger::insertAssignment(const BaseModelAssignment * assign, unsigned int estNum)
    00194 {
    00195         if(assign->getVariable()->getVariableType() != BaseModelVariable::CONTROL &&
    00196                 assign->getVariable()->getVariableType() != BaseModelVariable::OBSERVABLE &&
    00197                 assign->getVariable()->getVariableType() != BaseModelVariable::STATE)
    00198         {
    00199                 cerr << "<Trigger> ERROR: You can only assign values to mode, command, and observation variables." << endl;
    00200                 return false;
    00201         }
    00202
    00203         bool successFlag = true;
    00204         unsigned int varUID = assign->getVariable()->getUID();
    00205         unsigned int valUID = assign->getValue()->getUID();
    00206
    00207         if(varUID >= numVariablesInModel)
    00208         {
    00209                 cerr << "<Trigger> ERROR: Assignment variable is not within the bounds of the model!"
    00210                         << endl;
    00211                 return false;
    00212         }
    00213         if(valUID >= MAX_DOMAIN_SIZE)
    00214         {
    00215                 cerr << "<Trigger> ERROR: Assignment value exceeds boundaries!" << endl;
    00216                 return false;
    00217         }
    00218
    00219         if(debug > 1)
    00220                 cout << "<Trigger> Attempting to insert (" << varUID << "," << valUID
    00221                                 << ") assignment from Estimate #" << estNum << "..." << endl;
    00222
    00223         // Modify compiled objects that are listed in the lookup table
    00224         if(lookUpTable[varUID][valUID] != NULL)
    00225         {
    00226                 // Check to see if variable has already been assigned a value
    00227                 if(currentAssignmentsVector[estNum][varUID] != UNDEFINED)
    00228                         if(!processCompiledObjects(lookUpTable[varUID][currentAssignmentsVector[estNum][varUID]], false, estNum))
    00229                                 successFlag = false;
    00230                 if(!processCompiledObjects(lookUpTable[varUID][valUID], true, estNum))
    00231                         successFlag = false;
    00232         }
    00233         else
    00234                 if(debug > 2)
    00235                         cout << "<Trigger> Nothing in lookup table for (" << varUID << "," << valUID << ")" << endl;
    00236
    00237         if(successFlag)
    00238                 // Add into current assignment list
    00239                 currentAssignmentsVector[estNum][assign->getVariable()->getUID()] = assign->getValue()->getUID();
    00240
    00241         // Record that assignments have changed
    00242         assignmentsHaveChanged = true;
    00243
    00244         return successFlag;
    00245 }
    00246
    00247
    00248 bool
    00249 Trigger::changeTrajectories(const Trajectories * trajs)
    00250 {
    00251         bool flag;
    00252
    00253         currentTrajectories = trajs;
    00254
    00255         if(debug > 0)
    00256                 cout << "Changing trajectories in trigger..." << endl;
    00257
    00258         // Reset all the compiled objects / counters
    00259         clearCompiledObjects();
    00260
    00261         // Initialize all compiled objects
    00262         flag = initCompiledObjectList(trajs->getNumberEstimates());
    00263
    00264         // Insert current mode asignments
    00265         for(unsigned int i=0; i < trajs->getNumberEstimates(); i++)
    00266                 for(unsigned int j=0; j < numComponentsInModel; j++)
    00267                         if(!insertAssignment(trajs->getEstimate(i)->getAssignment(j), i))
    00268                                 flag = false;
    00269
    00270         return flag;
    00271 }
    00272
    00273
    00274 vector<double>
    00275 Trigger::getNormalizedEnabledTransitionProbabilities(void)
    00276 {
    00277         if(assignmentsHaveChanged)
    00278                 findConflictsAndModeAssignments();
    00279
    00280         return modeAssignmentProbabilities;
    00281 }
    00282
    00283 vector<const BaseModelAssignmentArray *>
    00284 Trigger::getConflicts(void)
    00285 {
    00286         if(assignmentsHaveChanged)
    00287                 findConflictsAndModeAssignments();
    00288
    00289         return conflicts;
    00290 }
    00291
    00292 int
    00293 Trigger::getCurrentCommandObservationValueUID(unsigned int varUID) const
    00294 {
    00295         if(varUID >= currentAssignmentsVector[0].getLength())
    00296                 return UNDEFINED;
    00297         else
    00298                 return currentAssignmentsVector[0][varUID];
    00299 }
    00300
    00301 void
    00302 Trigger::showDissents(ostream &out) const
    00303 {
    00304         unsigned int count = 0;
    00305         size_t limit = dissents.size();
    00306         out << "Dissents: " << endl;
    00307         for(count=0; count < limit; count++)
    00308                 out << *dissents[count] << endl;
    00309         out << endl;
    00310 }
    00311
    00312 void
    00313 Trigger::showCompiledTransitions(ostream &out) const
    00314 {
    00315         unsigned int count = 0;
    00316         size_t limit = compiledTransitions.size();
    00317         out << "Compiled Transitions: " << endl;
    00318         for(count=0; count < limit; count++)
    00319                 out << *compiledTransitions[count] << endl;
    00320         out << endl;
    00321 }
    00322
    00323 void
    00324 Trigger::showEnabled(ostream &out) const
    00325 {
    00326         unsigned int count = 0;
    00327         size_t limit = allCompiledObjects.size();
    00328         for(count = 0; count < limit; count++)
    00329         {
    00330                 // Display header if begining of list
    00331                 if(count==0 && dissents.size() != 0)
    00332                         out << "Enabled Dissents:" << endl;
    00333                 if((count > 0 || dissents.size() == 0) && (count-dissents.size())%compiledTransitions.size() == 0)
    00334                         out << endl << "Enabled Compiled Transitions for Estimate " << (count-dissents.size())/compiledTransitions.size() << ":" << endl;
    00335
    00336                 if(allCompiledObjects[count]->isEnabled())
    00337                         out << *allCompiledObjects[count] << endl;
    00338         }
    00339 }
    00340
    00341 void
    00342 Trigger::setDebug(const unsigned short debugVal)
    00343 // Sets debug to debugVal
    00344 {
    00345         debug = debugVal;
    00346         return;
    00347 }
    00348
    00349
    00350 ostream&
    00351 Mers::Estimation::Discrete::operator<<(ostream& os, Trigger const & trig)
    00352 {
    00353         unsigned int count;
    00354         size_t limit = trig.dissents.size() + trig.compiledTransitions.size();
    00355
    00356         if(trig.debug > 1)
    00357                 limit = trig.allCompiledObjects.size();
    00358
    00359         for(count = 0; count < limit; count++)
    00360         {
    00361                 // Display header if begining of list
    00362                 if(count==0)
    00363                         os << "Dissents:" << endl;
    00364                 if(count>0 && (count-trig.dissents.size())%trig.compiledTransitions.size() == 0)
    00365                         os << endl << "Compiled Transitions for Estimate #" << (count-trig.dissents.size())/trig.compiledTransitions.size() << endl;
    00366
    00367                 // Add comments for debug > 0
    00368                 if(trig.debug > 0)
    00369                 {
    00370                         if(trig.allCompiledObjects[count]->isEnabled())
    00371                                 os << "ENABLED:  ";
    00372                         else
    00373                                 os << "DISABLED: ";
    00374                 }
    00375                 os << *trig.allCompiledObjects[count] << endl;
    00376         }
    00377
    00378         return os;
    00379 }
    00380         //-----------------------------------------------
    00381         //      PRIVATE METHODS
    00382         //-----------------------------------------------
    00383
    00384
    00385 bool
    00386 Trigger::findConflictsAndModeAssignments(void)
    00387 {
    00388         unsigned int i, j, k;
    00389         double outgoingProbSum = 0.0;
    00390
    00391         if(debug > 1)
    00392                 cout << "<Trigger> Finding conflicts and mode assignment probabilities..." << endl;
    00393
    00394         if(!assignmentsHaveChanged)
    00395                 cout << "<Trigger> WARNING: Computing conflicts and mode assignments may not be necessary." << endl;
    00396
    00397         // Clear old conflicts and probabilities
    00398         while(conflicts.size() > 0)
    00399                 conflicts.pop_back();
    00400
    00401         while(modeAssignmentProbabilities.size() > 0)
    00402                 modeAssignmentProbabilities.pop_back();
    00403
    00404
    00405         // Initialize new probability list
    00406         for(k=0; k<currentTrajectories->getNumberEstimates(); k++)      // For each estimate
    00407                 for(i=0; i<allStateVars->getLength(); i++)              // For all state vars
    00408                         for(j=0; j<(*allStateVars)[i]->getDomain()->getLength(); j++)   // For all possible component assigns
    00409                                 modeAssignmentProbabilities.push_back(0.0);
    00410
    00411         // Find all enabled dissents and create conflict list
    00412         for(i=0; i<dissents.size(); i++)
    00413                 if(allCompiledObjects[i]->isEnabled())
    00414                         conflicts.push_back(allCompiledObjects[i]->getDissent()->getConflict());
    00415
    00416         // Find all enabled compiled transitions and create probability list
    00417         for( ; i<allCompiledObjects.size(); i++)
    00418         {
    00419                 if(allCompiledObjects[i]->isEnabled())
    00420                 {
    00421                         const BaseModelAssignment * target = allCompiledObjects[i]->getCompiledTransition()->getTargetMode();
    00422                         // Find target in mode list
    00423                         for(j=0; j<allStateVars->getLength() && *(target->getVariable()) != *((*allStateVars)[j]); j++)
    00424                         {;}
    00425                         if(j == allStateVars->getLength()) // Couldn't find variable match
    00426                         {
    00427                                 cerr << "<Trigger> ERROR: Couldn't find variable match! ("
    00428                                         << target->getVariable()->getUID() << "," << target->getValue()->getUID() << ")" << endl;
    00429                                 return false;
    00430                         }
    00431                         // For each enabled compiled transition, put the
    00432                         // transition probability into next mode assignment
    00433                         // probably array.
    00434                         modeAssignmentProbabilities[(modeAssignmentProbabilities.size()/currentTrajectories->getNumberEstimates()) *
    00435                                 ((i-dissents.size())/compiledTransitions.size()) + (*probabilityMap)[j] + target->getValue()->getUID()] +=
    00436                                 allCompiledObjects[i]->getCompiledTransition()->getTransitionProbability();
    00437                 }
    00438         }
    00439
    00440         // Normalize the outgoing transitions probabilities
    00441         //  (sum of transitions should be 1.0)
    00442         for(k=0; k<currentTrajectories->getNumberEstimates(); k++)      // For each estimate
    00443                 for(i=0; i<allStateVars->getLength(); i++)              // For all state vars
    00444                 {
    00445                         outgoingProbSum = 0.0;
    00446                         // Add up all outgoing transition probabilities
    00447                         for(j=0; j<(*allStateVars)[i]->getDomain()->getLength(); j++)   // For all possible component assigns
    00448                                 outgoingProbSum += modeAssignmentProbabilities[k*modeAssignmentProbabilities.size()/currentTrajectories->getNumberEstimates()
    00449                                         + (*probabilityMap)[i] + j];
    00450                         if(outgoingProbSum <= 0.0)
    00451                                 cerr << "<Trigger> ERROR: No reachable mode assignment for " << (*allStateVars)[i]->getName() << endl;
    00452                         // Divide by the sum of the outgoing transition probabilities
    00453                         for(j=0; j<(*allStateVars)[i]->getDomain()->getLength(); j++)   // For all possible component assigns
    00454                                 modeAssignmentProbabilities[k*modeAssignmentProbabilities.size()/currentTrajectories->getNumberEstimates()
    00455                                         + (*probabilityMap)[i] + j] /= outgoingProbSum;
    00456                 }
    00457
    00458         assignmentsHaveChanged = false;
    00459         return true;
    00460 }
    00461
    00462
    00463 void
    00464 Trigger::clearCompiledObjects(void)
    00465 {
    00466         CompiledObject * compObj;
    00467
    00468         while(allCompiledObjects.size() != 0)
    00469         {
    00470                 compObj = allCompiledObjects.back();
    00471                 allCompiledObjects.pop_back();
    00472                 delete compObj;
    00473         }
    00474
    00475         return;
    00476 }
    00477
    00478
    00479
    00480 bool
    00481 Trigger::initCompiledObjectList(const unsigned int numEstimates)
    00482 {
    00483         unsigned int i, j;
    00484         size_t compObjNum = allCompiledObjects.size();
    00485         if(compObjNum != 0)
    00486         {
    00487                 cerr << "<Trigger> ERROR: Compiled Object list must be cleared before initialized." << endl;
    00488                 return false;
    00489         }
    00490
    00491         // Place one set of dissents into the compiled object list
    00492         for(compObjNum = 0; compObjNum < dissents.size(); compObjNum++)
    00493                 allCompiledObjects.push_back(new CompiledObject(dissents[compObjNum], CompiledObject::oDISSENT, debug));
    00494
    00495         // Place one set of the compiled transition list for each mode estimate in the
    00496         //  trajectories
    00497         for(i=0; i<numEstimates; i++)
    00498                 for(compObjNum = 0; compObjNum < compiledTransitions.size(); compObjNum++)
    00499                         allCompiledObjects.push_back(new CompiledObject(compiledTransitions[compObjNum], CompiledObject::oCOMPILEDTRANSITION, debug));
    00500
    00501         // Reset all the current assignments to be undefined
    00502         currentAssignmentsVector.resize(numEstimates);
    00503         for(i=0; i<numEstimates; i++)
    00504         {
    00505                 currentAssignmentsVector[i].resize(numVariablesInModel);
    00506                 for(j=0; j<numVariablesInModel; j++)
    00507                         currentAssignmentsVector[i][j] = UNDEFINED;
    00508         }
    00509
    00510         return true;
    00511 }
    00512
    00513 bool
    00514 Trigger::processCompiledObjects(const IntegerArray * compObjIndices, const bool add,
    00515                                                                 const unsigned int estNum)
    00516 {
    00517         bool successFlag = true;
    00518         unsigned int index;
    00519
    00520         if(debug > 2)
    00521                 cout << "<Trigger> Processing compiled objects in the integer list..." << endl;
    00522
    00523         // Process all comp objects
    00524         for(unsigned int i=0; i<compObjIndices->getLength(); i++)
    00525         {
    00526                 index = (*compObjIndices)[i];
    00527                 if(!(index < dissents.size() && estNum > 0))            // Change counts on dissents only when estNum = 0
    00528                 {
    00529                         if(index >= dissents.size())            // Compiled transition. Need to consider estimate #
    00530                                 index += estNum*compiledTransitions.size();
    00531                         if(debug > 3)
    00532                                 cout << "<Trigger> Changing count on: " << *allCompiledObjects[index] << endl;
    00533                         if(!allCompiledObjects[index]->implicantElementSatisfied(add))
    00534                                 successFlag = false;
    00535                 }
    00536         }
    00537
    00538         return successFlag;
    00539 }
    00540