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 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       cout << "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" << endl;
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       //Find paw location
00065       const char * ipname=(lastLeg==LFrLegOrder?"ToeLFrPaw":"ToeRFrPaw");
00066       NEWMAT::ColumnVector Pobj=kine->getFrameInterestPoint(BaseFrameOffset,ipname);
00067 
00068       //Compute neck angles
00069       PostureEngine pose;
00070       NEWMAT::ColumnVector Plink=Kinematics::pack(0,0,1,0); //infinite ray along z axis - maximize distance from paw
00071       //NEWMAT::ColumnVector Plink=Kinematics::pack(0,0,80,1); //or, keep head 8cm away from paw
00072       pose.solveLinkVector(Pobj,CameraFrameOffset,Plink);
00073       
00074       //Set joint values
00075       MMAccessor<HeadPointerMC>(pointID)->setJoints(pose(HeadOffset+0).value,pose(HeadOffset+1).value,pose(HeadOffset+2).value);
00076     } else {
00077       serr->printf("StareAtPawBehavior2: Unhandled event %s\n",e.getName().c_str());
00078     }
00079   }
00080 
00081   static std::string getClassDescription() { return "Uses kinematics to track the paw which last received a button press with the camera"; }
00082   virtual std::string getDescription() const { return getClassDescription(); }
00083   
00084 protected:
00085   LegOrder_t lastLeg;
00086   MotionManager::MC_ID pointID;
00087 };
00088 
00089 /*! @file
00090  * @brief Defines StareAtPawBehavior2, which uses kinematics to track the paw which last received a button press with the camera
00091  * @author ejt (Creator)
00092  *
00093  * $Author: ejt $
00094  * $Name: tekkotsu-2_2_1 $
00095  * $Revision: 1.14 $
00096  * $State: Exp $
00097  * $Date: 2004/11/11 01:45:36 $
00098  */
00099 
00100 #endif

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