StareAtPawBehavior.hGo to the documentation of this file.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
00015 class StareAtPawBehavior : public BehaviorBase {
00016 public:
00017
00018 StareAtPawBehavior()
00019 : BehaviorBase("StareAtPawBehavior"), lastLeg(LFrLegOrder), pointID(MotionManager::invalid_MC_ID)
00020 { }
00021
00022 virtual void DoStart() {
00023 BehaviorBase::DoStart();
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();
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;
00065 paw.z+=body_to_neck.z;
00066 GetHeadAngles(head_angles,paw,0,0);
00067
00068
00069
00070
00071
00072
00073
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;
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
|