00001
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
00017 class StareAtPawBehavior2 : public BehaviorBase {
00018 public:
00019
00020 StareAtPawBehavior2()
00021 : BehaviorBase("StareAtPawBehavior2"), lastLeg(LFrLegOrder), pointID(MotionManager::invalid_MC_ID)
00022 { }
00023
00024 virtual void DoStart() {
00025 BehaviorBase::DoStart();
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();
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
00065 const char * ipname=(lastLeg==LFrLegOrder?"ToeLFrPaw":"ToeRFrPaw");
00066 NEWMAT::ColumnVector Pobj=kine->getFrameInterestPoint(BaseFrameOffset,ipname);
00067
00068
00069 PostureEngine pose;
00070 NEWMAT::ColumnVector Plink=Kinematics::pack(0,0,1,0);
00071
00072 pose.solveLinkVector(Pobj,CameraFrameOffset,Plink);
00073
00074
00075 MMAccessor<HeadPointerMC>(pointID)->setJoints(pose(HeadOffset+0).value,pose(HeadOffset+1).value,pose(HeadOffset+2).value);
00076 } else {
00077 serr->printf("StareAtPawBehavior2: 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 };
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100 #endif