00001
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
00014 class StareAtPawBehavior : public BehaviorBase {
00015 public:
00016
00017 StareAtPawBehavior()
00018 : BehaviorBase(), lastLeg(LFrLegOrder), pointID(MotionManager::invalid_MC_ID)
00019 { }
00020
00021 virtual void DoStart() {
00022 BehaviorBase::DoStart();
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();
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;
00064 paw.z+=body_to_neck.z;
00065 GetHeadAngles(head_angles,paw,0,0);
00066
00067
00068
00069
00070
00071
00072
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 virtual std::string getName() const { return "StareAtPawBehavior"; }
00082
00083 static std::string getClassDescription() { return "Uses kinematics to track the paw which last received a button press with the camera"; }
00084
00085 protected:
00086 LegOrder_t lastLeg;
00087 MotionManager::MC_ID pointID;
00088 double head_angles[JointsPerLeg];
00089 };
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102 #endif