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("BanditMachine","BanditMachine",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   virtual std::string getDescription() const { return getClassDescription(); }
00043 
00044   virtual void setup() {
00045     StateNode *wait=start=addNode(new WaitNode("Wait",this,bandit));
00046     StateNode *left=addNode(new PressNode("Left",this,LFrLegOffset+KneeOffset));
00047     StateNode *right=addNode(new PressNode("Right",this,RFrLegOffset+KneeOffset));
00048     StateNode *decide=addNode(new DecideNode("Decide",this,bandit,left,right));
00049     StateNode *recoverl=addNode(new OutputNode("\nBadPressLeft",this,std::cout,wait));
00050     StateNode *recoverr=addNode(new OutputNode("\nBadPressRight",this,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("/ms/data/motion/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 p the parent node
00085      *  @param idx the joint index of the paw to move
00086      */
00087     PressNode(const char* n, StateNode* p, unsigned int idx) : StateNode("PressNode",n,p), press_id(MotionManager::invalid_MC_ID), index(idx) {
00088       SharedObject<MotionSequenceMC<MotionSequence::SizeSmall> > press;
00089       press->setPlayTime(0);
00090       press->setOutputCmd(idx,.6);
00091       press->setPlayTime(1);
00092       press->setOutputCmd(idx,.6);
00093       press->setPlayTime(200);
00094       press->setOutputCmd(idx,.3);
00095       press->setPlayTime(1500);
00096       press->setOutputCmd(idx,outputRanges[idx][MinRange]);
00097       press_id=motman->addPersistentMotion(press,MotionManager::kStdPriority+1);
00098     }
00099     //!destructor
00100     virtual ~PressNode() {
00101       motman->removeMotion(press_id);
00102     }
00103     virtual void DoStart() {
00104       StateNode::DoStart();
00105       MMAccessor<MotionSequenceMC<MotionSequence::SizeSmall> > press(press_id);
00106       press->play();
00107       press->setOutputCmd(index,.6);
00108       //      press->setPlaySpeed(1);
00109     }
00110     virtual void DoStop() {
00111       MMAccessor<MotionSequenceMC<MotionSequence::SizeSmall> > press(press_id);
00112       //      press->setPlaySpeed(-1);
00113       press->pause();
00114       press->setPlayTime(0);
00115       StateNode::DoStop();
00116     }
00117   protected:
00118     MotionManager::MC_ID press_id; //!< the MC_ID of the MotionSequenceMC being used to do the press
00119     unsigned int index; //!< the joint index of the paw to move
00120   };
00121 
00122   //! uses one of the algorithms in karmedbandit.h to decide which paw to press next
00123   class DecideNode : public StateNode {
00124   public:
00125     //! constructor
00126     /*! @param n name of the node
00127      *  @param p the parent node
00128      *  @param bandito the decision making algorithm to use (look in karmedbandit.h)
00129      *  @param left the PressNode to go to if the left paw is chosen
00130      *  @param right the PressNode to go to if the right paw is chosen
00131      */
00132     DecideNode(const char* n, StateNode* p, karmedbanditExp3_1& bandito, StateNode* left, StateNode* right)
00133       : StateNode("DecideNode",n,p), b(bandito), l(left), r(right)
00134     {}
00135     virtual void DoStart() {
00136       StateNode::DoStart();
00137       AddReference();
00138       DoStop();
00139       if(b.decide()==0) {
00140         std::cout << "Left... " << std::flush;
00141         l->DoStart();
00142       } else {
00143         std::cout << "Right... " << std::flush;
00144         r->DoStart();
00145       }
00146       RemoveReference();
00147     }
00148   protected:
00149     karmedbanditExp3_1& b; //!< the class implementing the k-armed bandit algorithm
00150     StateNode* l; //!< the node to go to if the left paw is chosen
00151     StateNode* r; //!< the node to go to if the right paw is chosen
00152   private:
00153     DecideNode(const DecideNode& node); //!< don't call this
00154     DecideNode operator=(const DecideNode& node); //!< don't call this
00155   };
00156   
00157   //! Waits to see if a reward is received, lights up LEDs to let the user know
00158   class WaitNode : public StateNode {
00159   public:
00160     //! constructor
00161     /* @param n name to use for the node
00162      * @param p parent node
00163      * @param bandito the class to pass the reward to (if it comes)
00164      */
00165     WaitNode(const char* n, StateNode* p, karmedbanditExp3_1& bandito)
00166       : StateNode("WaitNode",n,p), b(bandito), reward(false), leds_id(MotionManager::invalid_MC_ID)
00167     {
00168       leds_id=motman->addPersistentMotion(SharedObject<LedMC>());
00169     }
00170     //! destructor
00171     virtual ~WaitNode() {
00172       motman->removeMotion(leds_id);
00173     }
00174     virtual void DoStart() {
00175       StateNode::DoStart();
00176       erouter->addListener(this,EventBase::visObjEGID,ProjectInterface::visPinkBallSID);
00177       erouter->addTimer(this,0,1000,false);
00178       MMAccessor<LedMC> leds(leds_id);
00179       leds->cflash(BotLLEDMask+BotRLEDMask,1,1000);
00180     }
00181     virtual void DoStop() {
00182       erouter->removeListener(this);
00183       b.reward(reward);
00184       cout << endl;
00185       reward=false;
00186       StateNode::DoStop();
00187     }
00188     virtual void processEvent(const EventBase& event) {
00189       if(event.getGeneratorID()==EventBase::timerEGID) {
00190         sndman->PlayFile("whimper.wav");
00191       } else {
00192         sndman->PlayFile("yipper.wav");
00193         reward=true;
00194         MMAccessor<LedMC> leds(leds_id);
00195         leds->cflash(MidLLEDMask+MidRLEDMask,1,100);
00196       }
00197       erouter->removeListener(this);
00198     }
00199   protected:
00200     karmedbanditExp3_1& b; //!< the class implimenting a k-armed bandit algorithm to pass the reward back to
00201     bool reward; //!< true if a reward was received
00202     MotionManager::MC_ID leds_id; //!< MC_ID of a LedMC
00203   };
00204 
00205   StareAtBallBehavior* stare; //!< active as long as we're in this state so it keeps an eye on the ball
00206   StateNode* start; //!< used to start off by lying down before we start pressing buttons
00207   MotionManager::MC_ID liedown; //!< a MotionSequence which will move the dog into a lying down posture
00208   karmedbanditExp3_1 bandit; //!< algorithm to use in the k-armed bandit problem
00209 
00210 private:
00211   BanditMachine(const BanditMachine& node); //!< don't call this
00212   BanditMachine operator=(const BanditMachine& node); //!< don't call this
00213 };
00214 
00215 /*! @file
00216  * @brief Defines BanditMachine, A state machine for playing k-armed bandit
00217  * @author ejt (Creator)
00218  *
00219  * $Author: ejt $
00220  * $Name: tekkotsu-2_2_1 $
00221  * $Revision: 1.20 $
00222  * $State: Exp $
00223  * $Date: 2004/11/11 21:32:14 $
00224  */
00225 
00226 #endif

Tekkotsu v2.2.1
Generated Tue Nov 23 16:36:37 2004 by Doxygen 1.3.9.1