Homepage
Demos
Overview
Downloads
Dev. Resources
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 "IPC/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 "Sound/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("BanditMachine","BanditMachine"), 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)
00031     : StateNode("BanditMachine",n), 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   virtual std::string getDescription() const { return getClassDescription(); }
00043 
00044   virtual void setup() {
00045     StateNode *wait=start=addNode(new WaitNode("Wait",bandit));
00046     StateNode *left=addNode(new PressNode("Left",LFrLegOffset+KneeOffset));
00047     StateNode *right=addNode(new PressNode("Right",RFrLegOffset+KneeOffset));
00048     StateNode *decide=addNode(new DecideNode("Decide",bandit,left,right));
00049     StateNode *recoverl=addNode(new OutputNode("\nBadPressLeft",std::cout,wait));
00050     StateNode *recoverr=addNode(new OutputNode("\nBadPressRight",std::cout,wait));
00051     left->addTransition(new SmoothCompareTrans<float>(wait,&state->pidduties[LFrLegOffset+RotatorOffset],CompareTrans<float>::LT,-.07,EventBase(EventBase::sensorEGID,SensorSourceID::UpdatedSID,EventBase::statusETID),.7));
00052     right->addTransition(new SmoothCompareTrans<float>(wait,&state->pidduties[RFrLegOffset+RotatorOffset],CompareTrans<float>::LT,-.07,EventBase(EventBase::sensorEGID,SensorSourceID::UpdatedSID,EventBase::statusETID),.7));
00053     wait->addTransition(new TimeOutTrans(decide,2000));
00054     left->addTransition(new TimeOutTrans(recoverl,1500));
00055     right->addTransition(new TimeOutTrans(recoverr,1500));
00056     //    recover->addTransition(new TimeOutTrans(decide,500));
00057     StateNode::setup();
00058   }
00059 
00060   virtual void DoStart() {
00061     StateNode::DoStart();
00062     stare->DoStart();
00063     start->DoStart();
00064     SharedObject<PostureMC> lie("liedown.pos");
00065     lie->setOutputCmd(LFrLegOffset+RotatorOffset,.77);
00066     lie->setOutputCmd(RFrLegOffset+RotatorOffset,.73);
00067     lie->setOutputCmd(LFrLegOffset+KneeOffset,.6);
00068     lie->setOutputCmd(RFrLegOffset+KneeOffset,.6);
00069     liedown=motman->addPrunableMotion(lie);
00070   }
00071 
00072   virtual void DoStop() {
00073     motman->removeMotion(liedown);
00074     stare->DoStop();
00075     StateNode::DoStop();
00076   }
00077 
00078 protected:
00079   //! This node is used to move a paw down using a MotionSequenceMC
00080   class PressNode : public StateNode {
00081   public:
00082     //! constructor
00083     /*! @param n name of the node
00084      *  @param idx the joint index of the paw to move
00085      */
00086     PressNode(const char* n, unsigned int idx) : StateNode("PressNode",n), press_id(MotionManager::invalid_MC_ID), index(idx) {
00087       SharedObject<SmallMotionSequenceMC> press;
00088       press->setTime(0);
00089       press->setOutputCmd(idx,.6);
00090       press->setTime(1);
00091       press->setOutputCmd(idx,.6);
00092       press->setTime(200);
00093       press->setOutputCmd(idx,.3);
00094       press->setTime(1500);
00095       press->setOutputCmd(idx,outputRanges[idx][MinRange]);
00096       press_id=motman->addPersistentMotion(press,MotionManager::kStdPriority+1);
00097     }
00098     //!destructor
00099     virtual ~PressNode() {
00100       motman->removeMotion(press_id);
00101     }
00102     virtual void DoStart() {
00103       StateNode::DoStart();
00104       MMAccessor<SmallMotionSequenceMC> press(press_id);
00105       press->play();
00106       press->setOutputCmd(index,.6);
00107       //      press->setSpeed(1);
00108     }
00109     virtual void DoStop() {
00110       MMAccessor<SmallMotionSequenceMC> press(press_id);
00111       //      press->setSpeed(-1);
00112       press->pause();
00113       press->setTime(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 bandito the decision making algorithm to use (look in karmedbandit.h)
00127      *  @param left the PressNode to go to if the left paw is chosen
00128      *  @param right the PressNode to go to if the right paw is chosen
00129      */
00130     DecideNode(const char* n, karmedbanditExp3_1& bandito, StateNode* left, StateNode* right)
00131       : StateNode("DecideNode",n), b(bandito), l(left), r(right)
00132     {}
00133     virtual void DoStart() {
00134       StateNode::DoStart();
00135       AddReference();
00136       DoStop();
00137       if(b.decide()==0) {
00138         std::cout << "Left... " << std::flush;
00139         l->DoStart();
00140       } else {
00141         std::cout << "Right... " << std::flush;
00142         r->DoStart();
00143       }
00144       RemoveReference();
00145     }
00146   protected:
00147     karmedbanditExp3_1& b; //!< the class implementing the k-armed bandit algorithm
00148     StateNode* l; //!< the node to go to if the left paw is chosen
00149     StateNode* r; //!< the node to go to if the right paw is chosen
00150   private:
00151     DecideNode(const DecideNode& node); //!< don't call this
00152     DecideNode operator=(const DecideNode& node); //!< don't call this
00153   };
00154   
00155   //! Waits to see if a reward is received, lights up LEDs to let the user know
00156   class WaitNode : public StateNode {
00157   public:
00158     //! constructor
00159     /* @param n name to use for the node
00160      * @param bandito the class to pass the reward to (if it comes)
00161      */
00162     WaitNode(const char* n, karmedbanditExp3_1& bandito)
00163       : StateNode("WaitNode",n), b(bandito), reward(false), leds_id(MotionManager::invalid_MC_ID)
00164     {
00165       leds_id=motman->addPersistentMotion(SharedObject<LedMC>());
00166     }
00167     //! destructor
00168     virtual ~WaitNode() {
00169       motman->removeMotion(leds_id);
00170     }
00171     virtual void DoStart() {
00172       StateNode::DoStart();
00173       erouter->addListener(this,EventBase::visObjEGID,ProjectInterface::visPinkBallSID);
00174       erouter->addTimer(this,0,1000,false);
00175       MMAccessor<LedMC> leds(leds_id);
00176       leds->cflash(BotLLEDMask+BotRLEDMask,1,1000);
00177     }
00178     virtual void DoStop() {
00179       erouter->removeListener(this);
00180       b.reward(reward);
00181       cout << endl;
00182       reward=false;
00183       StateNode::DoStop();
00184     }
00185     virtual void processEvent(const EventBase& event) {
00186       if(event.getGeneratorID()==EventBase::timerEGID) {
00187         sndman->PlayFile("whimper.wav");
00188       } else {
00189         sndman->PlayFile("yipper.wav");
00190         reward=true;
00191         MMAccessor<LedMC> leds(leds_id);
00192         leds->cflash(MidLLEDMask+MidRLEDMask,1,100);
00193       }
00194       erouter->removeListener(this);
00195     }
00196   protected:
00197     karmedbanditExp3_1& b; //!< the class implimenting a k-armed bandit algorithm to pass the reward back to
00198     bool reward; //!< true if a reward was received
00199     MotionManager::MC_ID leds_id; //!< MC_ID of a LedMC
00200   };
00201 
00202   StareAtBallBehavior* stare; //!< active as long as we're in this state so it keeps an eye on the ball
00203   StateNode* start; //!< used to start off by lying down before we start pressing buttons
00204   MotionManager::MC_ID liedown; //!< a MotionSequence which will move the dog into a lying down posture
00205   karmedbanditExp3_1 bandit; //!< algorithm to use in the k-armed bandit problem
00206 
00207 private:
00208   BanditMachine(const BanditMachine& node); //!< don't call this
00209   BanditMachine operator=(const BanditMachine& node); //!< don't call this
00210 };
00211 
00212 /*! @file
00213  * @brief Defines BanditMachine, A state machine for playing k-armed bandit
00214  * @author ejt (Creator)
00215  *
00216  * $Author: ejt $
00217  * $Name: tekkotsu-2_4_1 $
00218  * $Revision: 1.25 $
00219  * $State: Exp $
00220  * $Date: 2005/06/01 05:47:45 $
00221  */
00222 
00223 #endif

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:46 2005 by Doxygen 1.4.4