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 CMPack kinematics to track the paw which last received a button press with the camera
00014 /*! This is the "old" way of doing things, using CMPack kinematics... compare to StareAtPawBehavior2 for new-style kinematics */
00015 class StareAtPawBehavior : public BehaviorBase {
00016 public:
00017   //! constructor
00018   StareAtPawBehavior()
00019     : BehaviorBase("StareAtPawBehavior"), lastLeg(LFrLegOrder), pointID(MotionManager::invalid_MC_ID)
00020   { }
00021 
00022   virtual void DoStart() {
00023     BehaviorBase::DoStart(); // do this first
00024     for(unsigned int i=0; i<NumHeadJoints; i++)
00025       head_angles[i]=state->outputs[HeadOffset+i];
00026     pointID=motman->addPersistentMotion(SharedObject<HeadPointerMC>());
00027     erouter->addListener(this,EventBase::sensorEGID);
00028     erouter->addListener(this,EventBase::buttonEGID);
00029   }
00030 
00031   virtual void DoStop() {
00032     motman->removeMotion(pointID);
00033     pointID=MotionManager::invalid_MC_ID;
00034     erouter->removeListener(this);
00035     BehaviorBase::DoStop(); // do this last
00036   }
00037 
00038   virtual void processEvent(const EventBase& e) {
00039     if(e.getGeneratorID()==EventBase::buttonEGID) {
00040 
00041       if(e.getSourceID()==LFrPawOffset)
00042         lastLeg=LFrLegOrder;
00043       else if(e.getSourceID()==RFrPawOffset)
00044         lastLeg=RFrLegOrder;
00045       else
00046         return;
00047       if(e.getTypeID()==EventBase::activateETID) {
00048         unsigned int lastlegoff=LegOffset+lastLeg*JointsPerLeg;
00049         SharedObject<PIDMC> relaxLeg(lastlegoff,lastlegoff+JointsPerLeg,0);
00050         motman->addPrunableMotion(relaxLeg);
00051       } else if(e.getTypeID()==EventBase::deactivateETID) {
00052         unsigned int lastlegoff=LegOffset+lastLeg*JointsPerLeg;
00053         SharedObject<PIDMC> tightLeg(lastlegoff,lastlegoff+JointsPerLeg,1);
00054         motman->addPrunableMotion(tightLeg);
00055       }
00056 
00057     } else if(e.getGeneratorID()==EventBase::sensorEGID) {
00058 
00059       double leg_angles[JointsPerLeg];
00060       for(unsigned int i=0; i<JointsPerLeg; i++)
00061         leg_angles[i]=state->outputs[LegOffset+lastLeg*JointsPerLeg+i];
00062       vector3d paw;
00063       GetLegPosition(paw,leg_angles,lastLeg);
00064       paw-=body_to_neck; //target should be neck-relative
00065       paw.z+=body_to_neck.z; // but body-height relative (#@%&$)
00066       GetHeadAngles(head_angles,paw,0,0);
00067       /*{
00068         double tmp[JointsPerLeg];
00069         GetLegAngles(tmp,paw,lastLeg);
00070         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]);
00071         vector3d tmp2;
00072         tmp2=RunForwardModel(head_angles,0,0,vector3d(0,0,0));
00073         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]);
00074         }*/
00075       MMAccessor<HeadPointerMC>(pointID)->setJoints(head_angles[0],head_angles[1],head_angles[2]);
00076       
00077     } else {
00078       serr->printf("StareAtPawBehavior: Unhandled event %s\n",e.getName().c_str());
00079     }
00080   }
00081 
00082   static std::string getClassDescription() { return "Uses kinematics to track the paw which last received a button press with the camera"; }
00083   virtual std::string getDescription() const { return getClassDescription(); }
00084   
00085 protected:
00086   LegOrder_t lastLeg; //!< last leg to have it's button pressed, i.e. the one we are looking at
00087   MotionManager::MC_ID pointID; //!< the HeadPointerMC we are using to do the looking
00088   double head_angles[JointsPerLeg]; //!< temporary storage of the head angles we desire
00089 };
00090 
00091 /*! @file
00092  * @brief Defines StareAtPawBehavior, which uses CMPack 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_2 $
00097  * $Revision: 1.7 $
00098  * $State: Exp $
00099  * $Date: 2004/12/23 01:47:07 $
00100  */
00101 
00102 #endif

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