00001
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
00016 class KinematicSampleBehavior : public BehaviorBase {
00017 public:
00018
00019 KinematicSampleBehavior()
00020 : BehaviorBase("KinematicSampleBehavior"), 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
00067
00068
00069
00070
00071
00072 float link_x=60,link_y=0,link_z=0;
00073 float obj_x=0, obj_y=0, obj_z=0;
00074
00075 Kinematics::unpack(kine->linkToBase(getIndex(lastLeg))*Kinematics::pack(link_x,link_y,link_z),
00076 obj_x,obj_y,obj_z);
00077
00078
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
00090
00091
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 unsigned int getIndex(LegOrder_t leg) {
00103
00104 return LegOffset+leg*JointsPerLeg+KneeOffset;
00105 }
00106 LegOrder_t lastLeg;
00107 MotionManager::MC_ID poseID;
00108 };
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121 #endif