00001 #include "PostureMC.h"
00002 #include "Shared/Config.h"
00003 #include "Shared/WorldState.h"
00004 #include "Shared/ERS7Info.h"
00005 #include "Shared/ERS210Info.h"
00006
00007 PostureMC& PostureMC::setDirty(bool d) {
00008 dirty=d;
00009 targetReached=false;
00010 for(unsigned int i=0; i<NumOutputs; i++)
00011 curPositions[i]=motman->getOutputCmd(i).value;
00012 return *this;
00013 }
00014
00015 void PostureMC::defaultMaxSpeed(float x) {
00016 for(unsigned int i=0; i<NumOutputs; i++)
00017 maxSpeeds[i]=MaxOutputSpeed[i]*FrameTime*x;
00018
00019 #ifdef TGT_IS_AIBO
00020 maxSpeeds[HeadOffset+TiltOffset]=config->motion.max_head_tilt_speed*FrameTime*x/1000;
00021 maxSpeeds[HeadOffset+PanOffset]=config->motion.max_head_pan_speed*FrameTime*x/1000;
00022 maxSpeeds[HeadOffset+RollOffset]=config->motion.max_head_roll_speed*FrameTime*x/1000;
00023 #else
00024 const char* n = ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::TiltOffset];
00025 unsigned int i = capabilities.findOutputOffset(n);
00026 if(i!=-1U)
00027 maxSpeeds[i]=config->motion.max_head_tilt_speed*FrameTime*x/1000;
00028 n = ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::PanOffset];
00029 i = capabilities.findOutputOffset(n);
00030 if(i!=-1U)
00031 maxSpeeds[i]=config->motion.max_head_pan_speed*FrameTime*x/1000;
00032 n = ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::NodOffset];
00033 i = capabilities.findOutputOffset(n);
00034 if(i!=-1U)
00035 maxSpeeds[i]=config->motion.max_head_roll_speed*FrameTime*x/1000;
00036 n = ERS210Info::outputNames[ERS210Info::HeadOffset+ERS210Info::RollOffset];
00037 i = capabilities.findOutputOffset(n);
00038 if(i!=-1U)
00039 maxSpeeds[i]=config->motion.max_head_roll_speed*FrameTime*x/1000;
00040 #endif
00041 }
00042
00043 int PostureMC::updateOutputs() {
00044 int tmp=isDirty();
00045 if(tmp || hold) {
00046 dirty=false;
00047 for(unsigned int i=0; i<NumOutputs; i++) {
00048 if(maxSpeeds[i]<=0) {
00049 curPositions[i]=cmds[i].value;
00050 motman->setOutput(this,i,cmds[i]);
00051 } else {
00052 unsigned int f=0;
00053 while(cmds[i].value>curPositions[i]+maxSpeeds[i] && f<NumFrames) {
00054 curPositions[i]+=maxSpeeds[i];
00055 motman->setOutput(this,i,OutputCmd(curPositions[i],cmds[i].weight),f);
00056 f++;
00057 }
00058 while(cmds[i].value<curPositions[i]-maxSpeeds[i] && f<NumFrames) {
00059 curPositions[i]-=maxSpeeds[i];
00060 motman->setOutput(this,i,OutputCmd(curPositions[i],cmds[i].weight),f);
00061 f++;
00062 }
00063 if(f<NumFrames) {
00064 curPositions[i]=cmds[i].value;
00065 for(;f<NumFrames;f++)
00066 motman->setOutput(this,i,cmds[i],f);
00067 } else
00068 dirty=true;
00069 }
00070 }
00071 if(!dirty && !targetReached) {
00072 postEvent(EventBase(EventBase::motmanEGID,getID(),EventBase::statusETID));
00073 targetReached=true;
00074 targetTimestamp=get_time();
00075 }
00076 }
00077 return tmp;
00078 }
00079
00080 int PostureMC::isAlive() {
00081 if(dirty || !targetReached)
00082 return true;
00083 if(targetReached && (!hold || get_time()-targetTimestamp>timeout)) {
00084 if(get_time()-targetTimestamp>timeout && getAutoPrune())
00085 serr->printf("WARNING: posture timed out - possible joint conflict or out-of-range target\n");
00086 return false;
00087 }
00088 float max=0;
00089 for(unsigned int i=0; i<NumOutputs; i++)
00090 if(cmds[i].weight>0) {
00091 float dif=cmds[i].value-state->outputs[i];
00092 if(dif>max)
00093 max=dif;
00094 }
00095 return (max>tolerance);
00096 }
00097
00098 void PostureMC::init() {
00099 defaultMaxSpeed();
00100 setDirty();
00101 }
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112