Homepage Demos Overview Downloads Tutorials Reference
Credits

StareAtPawBehavior.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_StareAtPawBehavior_h_
00003 #define INCLUDED_StareAtPawBehavior_h_
00004 
00005 #include "Behaviors/BehaviorBase.h"
00006 #include "Motion/OldKinematics.h"
00007 #include "Motion/PIDMC.h"
00008 #include "Motion/HeadPointerMC.h"
00009 #include "Motion/MotionManager.h"
00010 #include "Shared/SharedObject.h"
00011 
00012 
00013 //! Uses kinematics to track the paw which last received a button press with the camera
00014 class StareAtPawBehavior : public BehaviorBase {
00015 public:
00016   //! constructor
00017   StareAtPawBehavior()
00018     : BehaviorBase("StareAtPawBehavior"), lastLeg(LFrLegOrder), pointID(MotionManager::invalid_MC_ID)
00019   { }
00020 
00021   virtual void DoStart() {
00022     BehaviorBase::DoStart(); // do this first
00023     for(unsigned int i=0; i<NumHeadJoints; i++)
00024       head_angles[i]=state->outputs[HeadOffset+i];
00025     pointID=motman->addPersistentMotion(SharedObject<HeadPointerMC>());
00026     erouter->addListener(this,EventBase::sensorEGID);
00027     erouter->addListener(this,EventBase::buttonEGID);
00028   }
00029 
00030   virtual void DoStop() {
00031     motman->removeMotion(pointID);
00032     pointID=MotionManager::invalid_MC_ID;
00033     erouter->removeListener(this);
00034     BehaviorBase::DoStop(); // do this last
00035   }
00036 
00037   virtual void processEvent(const EventBase& e) {
00038     if(e.getGeneratorID()==EventBase::buttonEGID) {
00039 
00040       if(e.getSourceID()==LFrPawOffset)
00041         lastLeg=LFrLegOrder;
00042       else if(e.getSourceID()==RFrPawOffset)
00043         lastLeg=RFrLegOrder;
00044       else
00045         return;
00046       if(e.getTypeID()==EventBase::activateETID) {
00047         unsigned int lastlegoff=LegOffset+lastLeg*JointsPerLeg;
00048         SharedObject<PIDMC> relaxLeg(lastlegoff,lastlegoff+JointsPerLeg,0);
00049         motman->addPrunableMotion(relaxLeg);
00050       } else if(e.getTypeID()==EventBase::deactivateETID) {
00051         unsigned int lastlegoff=LegOffset+lastLeg*JointsPerLeg;
00052         SharedObject<PIDMC> tightLeg(lastlegoff,lastlegoff+JointsPerLeg,1);
00053         motman->addPrunableMotion(tightLeg);
00054       }
00055 
00056     } else if(e.getGeneratorID()==EventBase::sensorEGID) {
00057 
00058       double leg_angles[JointsPerLeg];
00059       for(unsigned int i=0; i<JointsPerLeg; i++)
00060         leg_angles[i]=state->outputs[LegOffset+lastLeg*JointsPerLeg+i];
00061       vector3d paw;
00062       GetLegPosition(paw,leg_angles,lastLeg);
00063       paw-=body_to_neck; //target should be neck-relative
00064       paw.z+=body_to_neck.z; // but body-height relative (#@%&$)
00065       GetHeadAngles(head_angles,paw,0,0);
00066       /*{
00067         double tmp[JointsPerLeg];
00068         GetLegAngles(tmp,paw,lastLeg);
00069         sout->printf("Leg: (%g,%g,%g) (%g,%g,%g) (%g,%g,%g)\n",leg_angles[0],leg_angles[1],leg_angles[2],paw.x,paw.y,paw.z,tmp[0],tmp[1],tmp[2]);
00070         vector3d tmp2;
00071         tmp2=RunForwardModel(head_angles,0,0,vector3d(0,0,0));
00072         sout->printf("Head: (%g,%g,%g) (%g,%g,%g) (%g,%g,%g)\n",tmp2.x,tmp2.y,tmp2.z,head_angles[0],head_angles[1],head_angles[2],state->outputs[HeadOffset+0],state->outputs[HeadOffset+1],state->outputs[HeadOffset+2]);
00073         }*/
00074       MMAccessor<HeadPointerMC>(pointID)->setJoints(head_angles[0],head_angles[1],head_angles[2]);
00075       
00076     } else {
00077       serr->printf("StareAtPawBehavior: 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   double head_angles[JointsPerLeg];
00088 };
00089 
00090 /*! @file
00091  * @brief Defines StareAtPawBehavior, which uses kinematics to track the paw which last received a button press with the camera
00092  * @author ejt (Creator)
00093  *
00094  * $Author: ejt $
00095  * $Name: tekkotsu-2_2_1 $
00096  * $Revision: 1.6 $
00097  * $State: Exp $
00098  * $Date: 2004/11/11 01:45:36 $
00099  */
00100 
00101 #endif

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