Homepage Demos Overview Downloads Tutorials Reference
Credits

KinematicSampleBehavior2.h

Go to the documentation of this file.
00001 //-*-c++-*-
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 //! Uses kinematics to make the back toe (Toe{LR}BkPaw) touch the lower thigh (Lower{LeftBackL,RightBackR}FrThigh)
00016 class KinematicSampleBehavior2 : public BehaviorBase {
00017 public:
00018   //! constructor
00019   KinematicSampleBehavior2()
00020     : BehaviorBase("KinematicSampleBehavior2"), lastLeg(LFrLegOrder), poseID(MotionManager::invalid_MC_ID)
00021   { }
00022 
00023   virtual void DoStart() {
00024     BehaviorBase::DoStart(); // do this first
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(); // do this last
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       //Plan A:
00066       NEWMAT::ColumnVector obj(4);
00067       switch(lastLeg) {
00068       case LFrLegOrder:
00069         obj=kine->getJointInterestPoint(BaseFrameOffset,"LowerInnerBackLFrThigh,LowerOuterBackLFrThigh"); break;
00070       case RFrLegOrder:
00071         obj=kine->getJointInterestPoint(BaseFrameOffset,"LowerInnerBackRFrThigh,LowerOuterBackRFrThigh"); break;
00072       case LBkLegOrder:
00073         obj=kine->getJointInterestPoint(BaseFrameOffset,"LowerInnerFrontLBkThigh"); break;
00074       case RBkLegOrder:
00075         obj=kine->getJointInterestPoint(BaseFrameOffset,"LowerInnerFrontRBkThigh"); break;
00076       }
00077       if(obj(4)!=1)
00078         return;
00079       
00080       unsigned int solveLink=PawFrameOffset+((lastLeg+2)%NumLegs); //swap front/back
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       //use the knee angle to assign distance from the solution point
00096       float dist=state->outputs[LegOffset+lastLeg*JointsPerLeg+KneeOffset];
00097       dist*=30/outputRanges[LegOffset+lastLeg*JointsPerLeg+KneeOffset][MaxRange]; //scale to go up to 3 cm away
00098       cout << "Distance is " << dist/10 << "cm" << endl;
00099       float curlen=sqrt(link.SubMatrix(1,3,1,1).SumSquare());
00100       //Two ways to do the same thing:
00101       if(lastLeg==LFrLegOrder || lastLeg==LBkLegOrder)
00102         link.SubMatrix(1,3,1,1)*=(dist+curlen)/curlen; //scale the vector components individually
00103       else
00104         link(4)=curlen/(dist+curlen); //scale along the vector using the homogenous coordinate
00105 
00106       //Plan B:
00107       /*
00108       NEWMAT::ColumnVector obj(4);
00109       switch(lastLeg) {
00110       case LFrLegOrder:
00111         obj=kine->getJointInterestPoint(BaseFrameOffset,"LowerLeftBackLFrShin,LowerRightBackLFrShin"); break;
00112       case RFrLegOrder:
00113         obj=kine->getJointInterestPoint(BaseFrameOffset,"LowerLeftBackRFrShin,LowerRightBackRFrShin"); break;
00114       case LBkLegOrder:
00115         obj=kine->getJointInterestPoint(BaseFrameOffset,"ToeLBkPaw"); break;
00116       case RBkLegOrder:
00117         obj=kine->getJointInterestPoint(BaseFrameOffset,"ToeRBkPaw"); break;
00118       }
00119       if(obj(4)!=1)
00120         return;
00121       
00122       unsigned int solveLink=PawFrameOffset+((lastLeg+2)%NumLegs); //swap front/back
00123       NEWMAT::ColumnVector link(4);
00124       switch(lastLeg) {
00125       case LFrLegOrder:
00126         link=kine->getLinkInterestPoint(solveLink,"ToeLBkPaw"); break;
00127       case RFrLegOrder:
00128         link=kine->getLinkInterestPoint(solveLink,"ToeRBkPaw"); break;
00129       case LBkLegOrder:
00130         link=kine->getLinkInterestPoint(solveLink,"LowerLeftBackLFrShin,LowerRightBackLFrShin"); break;
00131       case RBkLegOrder:
00132         link=kine->getLinkInterestPoint(solveLink,"LowerLeftBackRFrShin,LowerRightBackRFrShin"); break;
00133       }
00134       if(link(4)!=1)
00135         return;
00136       */
00137 
00138       MMAccessor<PostureMC> pose_acc(poseID);
00139       pose_acc->solveLinkPosition(obj,solveLink,link);
00140 
00141       //If you would like to verify the positiions of the back toes... (relative to body center)
00142       //cout << "L: " << kine->getJointInterestPoint(BaseFrameOffset,"ToeLBkPaw").t();
00143       //cout << "R: " << kine->getJointInterestPoint(BaseFrameOffset,"ToeRBkPaw").t();
00144       //cout << "Toe: " << pose_acc->getJointInterestPoint(BaseFrameOffset,"ToeLBkPaw").t();
00145       //cout << "PawA: " << pose_acc->getJointInterestPoint(BaseFrameOffset,"LBkPaw").t();
00146       //cout << "PawB: " << (pose_acc->jointToJoint(PawFrameOffset+LBkLegOrder,BaseFrameOffset)*Kinematics::pack(0,0,0)).t();
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; //!< the last leg to have its button pressed, i.e. the "source"
00158   MotionManager::MC_ID poseID; //!< the PostureMC which does all the computation
00159 };
00160 
00161 /*! @file
00162  * @brief Defines KinematicSampleBehavior2, which uses kinematics to make the back toe (Toe{LR}BkPaw) touch the lower thigh (Lower{LeftBackL,RightBackR}FrThigh)
00163  * @author ejt (Creator)
00164  *
00165  * $Author: ejt $
00166  * $Name: tekkotsu-2_2_2 $
00167  * $Revision: 1.12 $
00168  * $State: Exp $
00169  * $Date: 2004/12/23 01:47:06 $
00170  */
00171 
00172 #endif

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