Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

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

Tekkotsu v1.4
Generated Sat Jul 19 00:06:29 2003 by Doxygen 1.3.2