PostureMC.ccGo to the documentation of this file.00001 #include "PostureMC.h"
00002 #include "Shared/Config.h"
00003 #include "MotionManager.h"
00004 #include "Shared/WorldState.h"
00005
00006 PostureMC& PostureMC::setDirty(bool d) {
00007 dirty=d;
00008 targetReached=false;
00009 for(unsigned int i=0; i<NumOutputs; i++)
00010 curPositions[i]=motman->getOutputCmd(i).value;
00011 return *this;
00012 }
00013
00014 void PostureMC::defaultMaxSpeed(float x) {
00015 for(unsigned int i=0; i<NumOutputs; i++)
00016 maxSpeeds[i]=MaxOutputSpeed[i]*FrameTime*x;
00017
00018 maxSpeeds[HeadOffset+TiltOffset]=config->motion.max_head_tilt_speed*FrameTime*x/1000;
00019 maxSpeeds[HeadOffset+PanOffset]=config->motion.max_head_pan_speed*FrameTime*x/1000;
00020 maxSpeeds[HeadOffset+RollOffset]=config->motion.max_head_roll_speed*FrameTime*x/1000;
00021 }
00022
00023 int PostureMC::updateOutputs() {
00024 int tmp=isDirty();
00025 if(tmp || hold) {
00026 dirty=false;
00027 for(unsigned int i=0; i<NumOutputs; i++) {
00028 if(maxSpeeds[i]<=0) {
00029 curPositions[i]=cmds[i].value;
00030 motman->setOutput(this,i,cmds[i]);
00031 } else {
00032 unsigned int f=0;
00033 while(cmds[i].value>curPositions[i]+maxSpeeds[i] && f<NumFrames) {
00034 curPositions[i]+=maxSpeeds[i];
00035 motman->setOutput(this,i,OutputCmd(curPositions[i],cmds[i].weight),f);
00036 f++;
00037 }
00038 while(cmds[i].value<curPositions[i]-maxSpeeds[i] && f<NumFrames) {
00039 curPositions[i]-=maxSpeeds[i];
00040 motman->setOutput(this,i,OutputCmd(curPositions[i],cmds[i].weight),f);
00041 f++;
00042 }
00043 if(f<NumFrames) {
00044 curPositions[i]=cmds[i].value;
00045 for(;f<NumFrames;f++)
00046 motman->setOutput(this,i,cmds[i],f);
00047 } else
00048 dirty=true;
00049 }
00050 }
00051 if(!dirty && !targetReached) {
00052 postEvent(EventBase(EventBase::motmanEGID,getID(),EventBase::statusETID));
00053 targetReached=true;
00054 targetTimestamp=get_time();
00055 }
00056 }
00057 return tmp;
00058 }
00059
00060 int PostureMC::isAlive() {
00061 if(dirty || !targetReached)
00062 return true;
00063 if(targetReached && get_time()-targetTimestamp>timeout) {
00064 if(getAutoPrune())
00065 serr->printf("WARNING: posture timed out - possible joint conflict or out-of-range target\n");
00066 return false;
00067 }
00068 PostureEngine tmp;
00069 tmp.takeSnapshot();
00070 return (maxdiff(tmp)>tolerance);
00071 }
00072
00073 void PostureMC::init() {
00074 defaultMaxSpeed();
00075 setDirty();
00076 }
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
|