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(), 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(4); Plink=0; Plink(3)=1; //infinite ray along z axis, maximizes distance from camera to objective
00071       //NEWMAT::ColumnVector Plink(4); Plink=0; Plink(3)=80; Plink(4)=1; //keep head 8cm away from paw
00072       //Alternatively, could also use the pack function a la: Plink=Kinematics::pack(0,0,80);
00073       pose.solveLinkVector(Pobj,CameraFrameOffset,Plink);
00074       
00075       //Set joint values
00076       MMAccessor<HeadPointerMC>(pointID)->setJoints(pose(HeadOffset+0).value,pose(HeadOffset+1).value,pose(HeadOffset+2).value);
00077     } else {
00078       serr->printf("StareAtPawBehavior2: Unhandled event %s\n",e.getName().c_str());
00079     }
00080   }
00081 
00082   virtual std::string getName() const { return "StareAtPawBehavior2"; }
00083 
00084   static std::string getClassDescription() { return "Uses kinematics to track the paw which last received a button press with the camera"; }
00085   
00086 protected:
00087   LegOrder_t lastLeg;
00088   MotionManager::MC_ID pointID;
00089 };
00090 
00091 /*! @file
00092  * @brief Defines StareAtPawBehavior2, which uses kinematics to track the paw which last received a button press with the camera
00093  * @author ejt (Creator)
00094  *
00095  * $Author: ejt $
00096  * $Name: tekkotsu-2_2 $
00097  * $Revision: 1.12 $
00098  * $State: Exp $
00099  * $Date: 2004/10/17 01:32:24 $
00100  */
00101 
00102 #endif

Tekkotsu v2.2
Generated Tue Oct 19 14:19:16 2004 by Doxygen 1.3.9.1