00001
00002 #ifndef INCLUDED_KinematicSampleBehavior2_h_
00003 #define INCLUDED_KinematicSampleBehavior2_h_
00004
00005 #include "Behaviors/BehaviorBase.h"
00006 #include "Motion/PIDMC.h"
00007 #include "Motion/PostureMC.h"
00008 #include "Motion/MotionManager.h"
00009 #include "Shared/SharedObject.h"
00010 #include "Motion/roboop/robot.h"
00011 #include "Shared/Config.h"
00012 #include "Motion/Kinematics.h"
00013
00014
00015
00016 class KinematicSampleBehavior2 : public BehaviorBase {
00017 public:
00018
00019 KinematicSampleBehavior2()
00020 : BehaviorBase("KinematicSampleBehavior2"), lastLeg(LFrLegOrder), poseID(MotionManager::invalid_MC_ID)
00021 { }
00022
00023 virtual void DoStart() {
00024 BehaviorBase::DoStart();
00025 poseID=motman->addPersistentMotion(SharedObject<PostureMC>());
00026 erouter->addListener(this,EventBase::sensorEGID);
00027 erouter->addListener(this,EventBase::buttonEGID);
00028 }
00029
00030 virtual void DoStop() {
00031 motman->removeMotion(poseID);
00032 poseID=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 switch(e.getSourceID()) {
00040 case LFrPawOffset:
00041 lastLeg=LFrLegOrder; break;
00042 case RFrPawOffset:
00043 lastLeg=RFrLegOrder; break;
00044 case LBkPawOffset:
00045 lastLeg=LBkLegOrder; break;
00046 case RBkPawOffset:
00047 lastLeg=RBkLegOrder; break;
00048 default:
00049 return;
00050 }
00051 if(e.getTypeID()==EventBase::activateETID) {
00052 unsigned int lastlegoff=LegOffset+lastLeg*JointsPerLeg;
00053 SharedObject<PIDMC> relaxLeg(lastlegoff,lastlegoff+JointsPerLeg,0);
00054 motman->addPrunableMotion(relaxLeg);
00055 MMAccessor<PostureMC> pose_acc(poseID);
00056 for(unsigned int i=0; i<JointsPerLeg; i++)
00057 pose_acc->setOutputCmd(lastlegoff+i,OutputCmd::unused);
00058 } else if(e.getTypeID()==EventBase::deactivateETID) {
00059 unsigned int lastlegoff=LegOffset+lastLeg*JointsPerLeg;
00060 SharedObject<PIDMC> tightLeg(lastlegoff,lastlegoff+JointsPerLeg,1);
00061 motman->addPrunableMotion(tightLeg);
00062 }
00063
00064 } else if(e.getGeneratorID()==EventBase::sensorEGID) {
00065
00066 NEWMAT::ColumnVector obj(4);
00067 switch(lastLeg) {
00068 case LFrLegOrder:
00069 obj=kine->getFrameInterestPoint(BaseFrameOffset,"LowerInnerBackLFrThigh,LowerOuterBackLFrThigh"); break;
00070 case RFrLegOrder:
00071 obj=kine->getFrameInterestPoint(BaseFrameOffset,"LowerInnerBackRFrThigh,LowerOuterBackRFrThigh"); break;
00072 case LBkLegOrder:
00073 obj=kine->getFrameInterestPoint(BaseFrameOffset,"LowerInnerFrontLBkThigh"); break;
00074 case RBkLegOrder:
00075 obj=kine->getFrameInterestPoint(BaseFrameOffset,"LowerInnerFrontRBkThigh"); break;
00076 }
00077 if(obj(4)!=1)
00078 return;
00079
00080 unsigned int solveLink=PawFrameOffset+((lastLeg+2)%NumLegs);
00081 NEWMAT::ColumnVector link(4);
00082 switch(lastLeg) {
00083 case LFrLegOrder:
00084 link=kine->getLinkInterestPoint(solveLink,"ToeLBkPaw"); break;
00085 case RFrLegOrder:
00086 link=kine->getLinkInterestPoint(solveLink,"ToeRBkPaw"); break;
00087 case LBkLegOrder:
00088 link=kine->getLinkInterestPoint(solveLink,"LowerInnerBackLFrShin"); break;
00089 case RBkLegOrder:
00090 link=kine->getLinkInterestPoint(solveLink,"LowerInnerBackRFrShin"); break;
00091 }
00092 if(link(4)!=1)
00093 return;
00094
00095
00096 float dist=state->outputs[LegOffset+lastLeg*JointsPerLeg+KneeOffset];
00097 dist*=30/outputRanges[LegOffset+lastLeg*JointsPerLeg+KneeOffset][MaxRange];
00098 cout << "Distance is " << dist/10 << "cm" << endl;
00099 float curlen=sqrt(link.SubMatrix(1,3,1,1).SumSquare());
00100
00101 if(lastLeg==LFrLegOrder || lastLeg==LBkLegOrder)
00102 link.SubMatrix(1,3,1,1)*=(dist+curlen)/curlen;
00103 else
00104 link(4)=curlen/(dist+curlen);
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138 MMAccessor<PostureMC> pose_acc(poseID);
00139 pose_acc->solveLinkPosition(obj,solveLink,link);
00140
00141
00142
00143
00144
00145
00146
00147
00148 } else {
00149 serr->printf("KinematicSampleBehavior2: Unhandled event %s\n",e.getName().c_str());
00150 }
00151 }
00152
00153 static std::string getClassDescription() { return "Uses kinematics to make the back toe (Toe{LR}BkPaw) touch the lower thigh (Lower{LeftBackL,RightBackR}FrThigh)"; }
00154 virtual std::string getDescription() const { return getClassDescription(); }
00155
00156 protected:
00157 LegOrder_t lastLeg;
00158 MotionManager::MC_ID poseID;
00159 };
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172 #endif