Homepage Demos Overview Downloads Tutorials Reference
Credits

StareAtPawBehavior2.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_StareAtPawBehavior2_h_
00003 #define INCLUDED_StareAtPawBehavior2_h_
00004 
00005 #include "Behaviors/BehaviorBase.h"
00006 #include "Motion/PIDMC.h"
00007 #include "Motion/HeadPointerMC.h"
00008 #include "Motion/PostureMC.h"
00009 #include "Motion/MotionManager.h"
00010 #include "Shared/SharedObject.h"
00011 #include "Motion/roboop/robot.h"
00012 #include "Shared/Config.h"
00013 #include "Motion/Kinematics.h"
00014 
00015 
00016 //! Uses new-style ROBOOP kinematics to track the paw which last received a button press with the camera
00017 class StareAtPawBehavior2 : public BehaviorBase {
00018 public:
00019   //! constructor
00020   StareAtPawBehavior2()
00021     : BehaviorBase("StareAtPawBehavior2"), lastLeg(LFrLegOrder), pointID(MotionManager::invalid_MC_ID)
00022   { }
00023 
00024   virtual void DoStart() {
00025     BehaviorBase::DoStart(); // do this first
00026     pointID=motman->addPersistentMotion(SharedObject<HeadPointerMC>());
00027     erouter->addListener(this,EventBase::sensorEGID);
00028     erouter->addListener(this,EventBase::buttonEGID);
00029     if(state->robotDesign == WorldState::ERS7Mask)
00030       sout->printf("NOTICE: The ERS-7 has a rather \"sticky\" nod joint\n"
00031                    "(the upper tilt joint).  This can cause it to hesitate\n"
00032                    "or altogether fail to precisely center the target position\n"
00033                    "vertically in the center of the image...\n");
00034   }
00035 
00036   virtual void DoStop() {
00037     motman->removeMotion(pointID);
00038     pointID=MotionManager::invalid_MC_ID;
00039     erouter->removeListener(this);
00040     BehaviorBase::DoStop(); // do this last
00041   }
00042 
00043   virtual void processEvent(const EventBase& e) {
00044     if(e.getGeneratorID()==EventBase::buttonEGID) {
00045 
00046       if(e.getSourceID()==LFrPawOffset) {
00047         lastLeg=LFrLegOrder;
00048       } else if(e.getSourceID()==RFrPawOffset) {
00049         lastLeg=RFrLegOrder;
00050       } else
00051         return;
00052       if(e.getTypeID()==EventBase::activateETID) {
00053         unsigned int lastlegoff=LegOffset+lastLeg*JointsPerLeg;
00054         SharedObject<PIDMC> relaxLeg(lastlegoff,lastlegoff+JointsPerLeg,0);
00055         motman->addPrunableMotion(relaxLeg);
00056       } else if(e.getTypeID()==EventBase::deactivateETID) {
00057         unsigned int lastlegoff=LegOffset+lastLeg*JointsPerLeg;
00058         SharedObject<PIDMC> tightLeg(lastlegoff,lastlegoff+JointsPerLeg,1);
00059         motman->addPrunableMotion(tightLeg);
00060       }
00061 
00062     } else if(e.getGeneratorID()==EventBase::sensorEGID) {
00063 
00064       //Which paw do we want?
00065       const char * ipname= (lastLeg==LFrLegOrder) ? "ToeLFrPaw" : "ToeRFrPaw";
00066 
00067       //Ask kinematics for current location of that paw (this is the "objective" aka target)
00068       NEWMAT::ColumnVector Pobj=kine->getJointInterestPoint(BaseFrameOffset,ipname);
00069 
00070       //Now point the head there
00071       MMAccessor<HeadPointerMC>(pointID)->lookAtPoint(Pobj(1),Pobj(2),Pobj(3)); //keep head as far away as possible
00072       //Alternative method:
00073       // MMAccessor<HeadPointerMC>(pointID)->lookAtPoint(Pobj(1),Pobj(2),Pobj(3),80); //keep head 80mm away
00074 
00075     } else {
00076       serr->printf("StareAtPawBehavior2: Unhandled event %s\n",e.getName().c_str());
00077     }
00078   }
00079 
00080   static std::string getClassDescription() { return "Uses kinematics to track the paw which last received a button press with the camera"; }
00081   virtual std::string getDescription() const { return getClassDescription(); }
00082   
00083 protected:
00084   LegOrder_t lastLeg; //!< last leg to have it's button pressed, i.e. the one we are looking at
00085   MotionManager::MC_ID pointID; //!< the HeadPointerMC we are using to do the looking
00086 };
00087 
00088 /*! @file
00089  * @brief Defines StareAtPawBehavior2, which uses new-style ROBOOP kinematics to track the paw which last received a button press with the camera
00090  * @author ejt (Creator)
00091  *
00092  * $Author: ejt $
00093  * $Name: tekkotsu-2_2_2 $
00094  * $Revision: 1.18 $
00095  * $State: Exp $
00096  * $Date: 2004/12/23 01:47:07 $
00097  */
00098 
00099 #endif

Tekkotsu v2.2.2
Generated Tue Jan 4 15:43:15 2005 by Doxygen 1.4.0