Homepage Demos Overview Downloads Tutorials Reference
Credits

BanditMachine.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_BanditMachine_h_
00003 #define INCLUDED_BanditMachine_h_
00004 
00005 #include "Behaviors/StateNode.h"
00006 #include "Behaviors/Demos/StareAtBallBehavior.h"
00007 #include "Shared/SharedObject.h"
00008 #include "Motion/PostureMC.h"
00009 #include "Motion/MotionSequenceMC.h"
00010 #include "Motion/LedMC.h"
00011 #include "Behaviors/Transitions/TimeOutTrans.h"
00012 #include "Behaviors/Transitions/SmoothCompareTrans.h"
00013 #include "Behaviors/Nodes/OutputNode.h"
00014 #include "SoundPlay/SoundManager.h"
00015 #include "Shared/ProjectInterface.h"
00016 
00017 #include "Behaviors/Demos/karmedbandit.h"
00018 
00019 //! Plays K-armed bandit
00020 class BanditMachine : public StateNode {
00021 public:
00022   //!constructor
00023   BanditMachine()
00024     : StateNode("Bandit Machine",NULL), stare(NULL), start(NULL), liedown(MotionManager::invalid_MC_ID), bandit(2)
00025   {
00026     stare=new StareAtBallBehavior();
00027     stare->AddReference();
00028   }
00029   //!constructor
00030   BanditMachine(const char* n, StateNode* p=NULL)
00031     : StateNode(n,p), stare(), start(NULL), liedown(MotionManager::invalid_MC_ID), bandit(2)
00032   {
00033     stare=new StareAtBallBehavior();
00034     stare->AddReference();
00035   }
00036   //!destructor
00037   virtual ~BanditMachine() {
00038     stare->RemoveReference();
00039   }
00040 
00041   static std::string getClassDescription() { return "Plays k-armed bandit with a computer"; }
00042 
00043   virtual void setup() {
00044     StateNode *wait=start=addNode(new WaitNode("Wait",this,bandit));
00045     StateNode *left=addNode(new PressNode("Left",this,LFrLegOffset+KneeOffset));
00046     StateNode *right=addNode(new PressNode("Right",this,RFrLegOffset+KneeOffset));
00047     StateNode *decide=addNode(new DecideNode("Decide",this,bandit,left,right));
00048     StateNode *recoverl=addNode(new OutputNode("\nBadPressLeft",this,std::cout,wait));
00049     StateNode *recoverr=addNode(new OutputNode("\nBadPressRight",this,std::cout,wait));
00050     left->addTransition(new SmoothCompareTrans<float>(wait,&state->pidduties[LFrLegOffset+RotatorOffset],CompareTrans<float>::LT,-.07,EventBase(EventBase::sensorEGID,SensorSourceID::UpdatedSID,EventBase::statusETID),.7));
00051     right->addTransition(new SmoothCompareTrans<float>(wait,&state->pidduties[RFrLegOffset+RotatorOffset],CompareTrans<float>::LT,-.07,EventBase(EventBase::sensorEGID,SensorSourceID::UpdatedSID,EventBase::statusETID),.7));
00052     wait->addTransition(new TimeOutTrans(decide,2000));
00053     left->addTransition(new TimeOutTrans(recoverl,1500));
00054     right->addTransition(new TimeOutTrans(recoverr,1500));
00055     //    recover->addTransition(new TimeOutTrans(decide,500));
00056     StateNode::setup();
00057   }
00058 
00059   virtual void DoStart() {
00060     StateNode::DoStart();
00061     stare->DoStart();
00062     start->DoStart();
00063     SharedObject<PostureMC> lie("/ms/data/motion/liedown.pos");
00064     lie->setOutputCmd(LFrLegOffset+RotatorOffset,.77);
00065     lie->setOutputCmd(RFrLegOffset+RotatorOffset,.73);
00066     lie->setOutputCmd(LFrLegOffset+KneeOffset,.6);
00067     lie->setOutputCmd(RFrLegOffset+KneeOffset,.6);
00068     liedown=motman->addMotion(lie);
00069   }
00070 
00071   virtual void DoStop() {
00072     motman->removeMotion(liedown);
00073     stare->DoStop();
00074     StateNode::DoStop();
00075   }
00076 
00077 protected:
00078   //! This node is used to move a paw down using a MotionSequenceMC
00079   class PressNode : public StateNode {
00080   public:
00081     //! constructor
00082     /*! @param n name of the node
00083      *  @param p the parent node
00084      *  @param idx the joint index of the paw to move
00085      */
00086     PressNode(const char* n, StateNode* p, unsigned int idx) : StateNode(n,p), press_id(MotionManager::invalid_MC_ID), index(idx) {
00087       SharedObject<MotionSequenceMC<MotionSequence::SizeSmall> > press;
00088       press->setPlayTime(0);
00089       press->setOutputCmd(idx,.6);
00090       press->setPlayTime(1);
00091       press->setOutputCmd(idx,.6);
00092       press->setPlayTime(200);
00093       press->setOutputCmd(idx,.3);
00094       press->setPlayTime(1500);
00095       press->setOutputCmd(idx,outputRanges[idx][MinRange]);
00096       press_id=motman->addMotion(press,MotionManager::kStdPriority+1,false);
00097     }
00098     //!destructor
00099     virtual ~PressNode() {
00100       motman->removeMotion(press_id);
00101     }
00102     virtual void DoStart() {
00103       StateNode::DoStart();
00104       MMAccessor<MotionSequenceMC<MotionSequence::SizeSmall> > press(press_id);
00105       press->play();
00106       press->setOutputCmd(index,.6);
00107       //      press->setPlaySpeed(1);
00108     }
00109     virtual void DoStop() {
00110       MMAccessor<MotionSequenceMC<MotionSequence::SizeSmall> > press(press_id);
00111       //      press->setPlaySpeed(-1);
00112       press->pause();
00113       press->setPlayTime(0);
00114       StateNode::DoStop();
00115     }
00116   protected:
00117     MotionManager::MC_ID press_id; //!< the MC_ID of the MotionSequenceMC being used to do the press
00118     unsigned int index; //!< the joint index of the paw to move
00119   };
00120 
00121   //! uses one of the algorithms in karmedbandit.h to decide which paw to press next
00122   class DecideNode : public StateNode {
00123   public:
00124     //! constructor
00125     /*! @param n name of the node
00126      *  @param p the parent node
00127      *  @param bandito the decision making algorithm to use (look in karmedbandit.h)
00128      *  @param left the PressNode to go to if the left paw is chosen
00129      *  @param right the PressNode to go to if the right paw is chosen
00130      */
00131     DecideNode(const char* n, StateNode* p, karmedbanditExp3_1& bandito, StateNode* left, StateNode* right)
00132       : StateNode(n,p), b(bandito), l(left), r(right)
00133     {}
00134     virtual void DoStart() {
00135       StateNode::DoStart();
00136       AddReference();
00137       DoStop();
00138       if(b.decide()==0) {
00139         std::cout << "Left... " << std::flush;
00140         l->DoStart();
00141       } else {
00142         std::cout << "Right... " << std::flush;
00143         r->DoStart();
00144       }
00145       RemoveReference();
00146     }
00147   protected:
00148     karmedbanditExp3_1& b; //!< the class implementing the k-armed bandit algorithm
00149     StateNode* l; //!< the node to go to if the left paw is chosen
00150     StateNode* r; //!< the node to go to if the right paw is chosen
00151   private:
00152     DecideNode(const DecideNode& node); //!< don't call this
00153     DecideNode operator=(const DecideNode& node); //!< don't call this
00154   };
00155   
00156   //! Waits to see if a reward is received, lights up LEDs to let the user know
00157   class WaitNode : public StateNode {
00158   public:
00159     //! constructor
00160     /* @param n name to use for the node
00161      * @param p parent node
00162      * @param bandito the class to pass the reward to (if it comes)
00163      */
00164     WaitNode(const char* n, StateNode* p, karmedbanditExp3_1& bandito)
00165       : StateNode(n,p), b(bandito), reward(false), leds_id(MotionManager::invalid_MC_ID)
00166     {
00167       leds_id=motman->addMotion(SharedObject<LedMC>());
00168     }
00169     //! destructor
00170     virtual ~WaitNode() {
00171       motman->removeMotion(leds_id);
00172     }
00173     virtual void DoStart() {
00174       StateNode::DoStart();
00175       erouter->addListener(this,EventBase::visObjEGID,ProjectInterface::visPinkBallSID);
00176       erouter->addTimer(this,0,1000,false);
00177       MMAccessor<LedMC> leds(leds_id);
00178       leds->cflash(BotLLEDMask+BotRLEDMask,1,1000);
00179     }
00180     virtual void DoStop() {
00181       erouter->forgetListener(this);
00182       b.reward(reward);
00183       cout << endl;
00184       reward=false;
00185       StateNode::DoStop();
00186     }
00187     virtual void processEvent(const EventBase& event) {
00188       if(event.getGeneratorID()==EventBase::timerEGID) {
00189         sndman->PlayFile("whimper.wav");
00190       } else {
00191         sndman->PlayFile("yipper.wav");
00192         reward=true;
00193         MMAccessor<LedMC> leds(leds_id);
00194         leds->cflash(MidLLEDMask+MidRLEDMask,1,100);
00195       }
00196       erouter->forgetListener(this);
00197     }
00198   protected:
00199     karmedbanditExp3_1& b; //!< the class implimenting a k-armed bandit algorithm to pass the reward back to
00200     bool reward; //!< true if a reward was received
00201     MotionManager::MC_ID leds_id; //!< MC_ID of a LedMC
00202   };
00203 
00204   StareAtBallBehavior* stare; //!< active as long as we're in this state so it keeps an eye on the ball
00205   StateNode* start; //!< used to start off by lying down before we start pressing buttons
00206   MotionManager::MC_ID liedown; //!< a MotionSequence which will move the dog into a lying down posture
00207   karmedbanditExp3_1 bandit; //!< algorithm to use in the k-armed bandit problem
00208 
00209 private:
00210   BanditMachine(const BanditMachine& node); //!< don't call this
00211   BanditMachine operator=(const BanditMachine& node); //!< don't call this
00212 };
00213 
00214 /*! @file
00215  * @brief Defines BanditMachine, A state machine for playing k-armed bandit
00216  * @author ejt (Creator)
00217  *
00218  * $Author: ejt $
00219  * $Name: tekkotsu-2_1 $
00220  * $Revision: 1.13 $
00221  * $State: Exp $
00222  * $Date: 2003/12/08 00:20:57 $
00223  */
00224 
00225 #endif

Tekkotsu v2.1
Generated Tue Mar 16 23:19:12 2004 by Doxygen 1.3.5