Homepage Demos Overview Downloads Tutorials Reference
Credits

KinematicSampleBehavior.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_KinematicSampleBehavior_h_
00003 #define INCLUDED_KinematicSampleBehavior_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 mirror leg positions (note that it mirrors the paw position - not necessarily the joint angles used to get there!)
00016 class KinematicSampleBehavior : public BehaviorBase {
00017 public:
00018   //! constructor
00019   KinematicSampleBehavior()
00020     : BehaviorBase("KinematicSampleBehavior"), 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       //I'll use the pack and unpack functions here just to illustrate how to avoid 
00066       //needing to use NEWMAT data structures - but if you're going to be doing
00067       //any significant math, you'll eventually want to get comfortable with NEWMAT...
00068 
00069       // (Actually, I think the NEWMAT version would be more readable...)
00070 
00071       /******** Determine location of target position ********/
00072       float link_x=60,link_y=0,link_z=0; // 6cm along x axis of selected joint
00073       float obj_x=0, obj_y=0, obj_z=0;   // these will hold the objective position
00074       //this next line computes the link position, and stores result into obj_*
00075       Kinematics::unpack(kine->linkToBase(getIndex(lastLeg))*Kinematics::pack(link_x,link_y,link_z),
00076                          obj_x,obj_y,obj_z);
00077 
00078       /******** Solve each leg for the point ********/
00079       MMAccessor<PostureMC> pose_acc(poseID); //
00080       for(unsigned int i=0; i<NumLegs; i++)
00081         if(i!=(unsigned int)lastLeg) {
00082           float m_x=((i<2)==((unsigned int)lastLeg<2))?obj_x:-obj_x;
00083           float m_y=((i%2)==((unsigned int)lastLeg%2))?obj_y:-obj_y;
00084           pose_acc->solveLinkPosition(Kinematics::pack(m_x,m_y,obj_z),
00085                                       getIndex((LegOrder_t)i),
00086                                       Kinematics::pack(link_x,link_y,link_z));
00087         }
00088       
00089       //If you would like to verify the positiions of the back toes... (relative to body center)
00090       //cout << "L: " << kine->getJointInterestPoint(BaseFrameOffset,"ToeLBkPaw").t();
00091       //cout << "R: " << kine->getJointInterestPoint(BaseFrameOffset,"ToeRBkPaw").t();
00092       
00093     } else {
00094       serr->printf("KinematicSampleBehavior: Unhandled event %s\n",e.getName().c_str());
00095     }
00096   }
00097 
00098   static std::string getClassDescription() { return "Uses kinematics to mirror leg positions (note that it mirrors the paw position - not necessarily the joint angles used to get there!)"; }
00099   virtual std::string getDescription() const { return getClassDescription(); }
00100   
00101 protected:
00102   //! returns the index of the knee for the requested @a leg
00103   unsigned int getIndex(LegOrder_t leg) {
00104     //or try: return PawFrameOffset+leg;
00105     return LegOffset+leg*JointsPerLeg+KneeOffset;
00106   }
00107   LegOrder_t lastLeg; //!< the last leg to have its button pressed, i.e. the "source"
00108   MotionManager::MC_ID poseID; //!< the PostureMC which does all the computation
00109 };
00110 
00111 /*! @file
00112  * @brief Defines KinematicSampleBehavior, which uses kinematics to mirror leg positions (note that it mirrors the paw position - not necessarily the joint angles used to get there!)
00113  * @author ejt (Creator)
00114  *
00115  * $Author: ejt $
00116  * $Name: tekkotsu-2_2_2 $
00117  * $Revision: 1.9 $
00118  * $State: Exp $
00119  * $Date: 2004/12/23 01:47:06 $
00120  */
00121 
00122 #endif

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