Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

PostureEngine.cc

Go to the documentation of this file.
00001 #include "PostureEngine.h"
00002 #include "Shared/WorldState.h"
00003 #include "Motion/roboop/robot.h"
00004 #include "Shared/Config.h"
00005 #include <stdio.h>
00006 
00007 PostureEngine::~PostureEngine() {}
00008 
00009 void PostureEngine::takeSnapshot() {
00010   for(unsigned int i=0; i<NumOutputs; i++)
00011     cmds[i].value=state->outputs[i];
00012 }
00013 
00014 void PostureEngine::takeSnapshot(const WorldState* st) {
00015   for(unsigned int i=0; i<NumOutputs; i++)
00016     cmds[i].value=st->outputs[i];
00017 }
00018 
00019 void PostureEngine::setWeights(float w, unsigned int lowjoint, unsigned int highjoint) {
00020   for(unsigned int i=lowjoint; i<highjoint; i++)
00021     cmds[i].weight=w;
00022 }
00023 
00024 void PostureEngine::clear() {
00025   for(unsigned int i=0; i<NumOutputs; i++)
00026     cmds[i].unset();
00027 }
00028 
00029 PostureEngine& PostureEngine::setOverlay(const PostureEngine& pe) {
00030   for(unsigned int i=0; i<NumOutputs; i++)
00031     if(pe.cmds[i].weight>0)
00032       cmds[i]=pe.cmds[i];
00033   return *this;
00034 }
00035 PostureEngine PostureEngine::createOverlay(const PostureEngine& pe) const {
00036   PostureEngine tmp(*this);
00037   return tmp.setOverlay(pe);
00038 }
00039 PostureEngine& PostureEngine::setUnderlay(const PostureEngine& pe) {
00040   for(unsigned int i=0; i<NumOutputs; i++)
00041     if(cmds[i].weight<=0)
00042       cmds[i]=pe.cmds[i];
00043   return *this;
00044 }
00045 PostureEngine PostureEngine::createUnderlay(const PostureEngine& pe) const {
00046   PostureEngine tmp(*this);
00047   return tmp.setUnderlay(pe);
00048 }
00049 /*! joints being averaged with unused joints have their weights averaged, but not their values (so an output can crossfade properly)\n
00050  *  @param pe the other PostureEngine
00051  *  @param w amount to weight towards @a pe
00052  *  - if @a w < .001, nothing is done
00053  *  - if @a w > .999, a straight copy of @a pe occurs (sets joints to unused properly at end of fade)
00054  *  - .001 and .999 is used instead of 0 and 1 to allow for slight addition errors in a loop (if
00055  *    using repeated additions of a delta value instead of repeated divisions)
00056  *  @return @c *this, stores results into this */
00057 PostureEngine& PostureEngine::setAverage(const PostureEngine& pe, float w) {
00058   if(w<0.001)
00059     return *this;
00060   if(w>0.999)
00061     return (*this=pe);
00062   float wp=1-w;
00063   for(unsigned int i=0; i<NumOutputs; i++)
00064     if(cmds[i].weight>0) {
00065       if(pe.cmds[i].weight>0)
00066         cmds[i].set(cmds[i].value*wp+pe.cmds[i].value*w,cmds[i].weight*wp+pe.cmds[i].weight*w);
00067       else
00068         cmds[i].weight*=wp;
00069     } else
00070       cmds[i].set(pe.cmds[i].value,pe.cmds[i].weight*w);
00071   return *this;
00072 }
00073 /*! joints being averaged with weight<=0 have their weights averaged, but not their values (so an output can crossfade properly)\n
00074  *  @param pe the other PostureEngine
00075  *  @param w amount to weight towards @a pe
00076  *  - if @a w < .001, nothing is done
00077  *  - if @a w > .999, a straight copy of @a pe occurs (sets joints to unused properly at end of fade)
00078  *  - .001 and .999 is used instead of 0 and 1 to allow for slight addition errors in a loop (if
00079  *    using repeated additions of a delta value instead of repeated divisions)
00080  *  @return a new posture containing the results */
00081 PostureEngine PostureEngine::createAverage(const PostureEngine& pe, float w) const {
00082   PostureEngine tmp(*this);
00083   return tmp.setAverage(pe,w);
00084 }
00085 PostureEngine& PostureEngine::setCombine(const PostureEngine& pe) {
00086   for(unsigned int i=0; i<NumOutputs; i++) {
00087     float total=cmds[i].weight+pe.cmds[i].weight;
00088     cmds[i].set((cmds[i].value*cmds[i].weight+pe.cmds[i].value*pe.cmds[i].weight)/total,total);
00089   }
00090   return *this;
00091 }
00092 PostureEngine PostureEngine::createCombine(const PostureEngine& pe) const {
00093   PostureEngine tmp(*this);
00094   return tmp.setCombine(pe);
00095 }
00096 
00097 float PostureEngine::diff(const PostureEngine& pe) const {
00098   float ans=0;
00099   for(unsigned int i=0; i<NumOutputs; i++)
00100     if(cmds[i].weight>0 && pe.cmds[i].weight>0) {
00101       float dif=cmds[i].value-pe.cmds[i].value;
00102       ans+=dif*dif;
00103     }
00104   return ans;
00105 }
00106 
00107 float PostureEngine::avgdiff(const PostureEngine& pe) const {
00108   float ans=0;
00109   unsigned int cnt=0;
00110   for(unsigned int i=0; i<NumOutputs; i++)
00111     if(cmds[i].weight>0 && pe.cmds[i].weight>0) {
00112       float dif=cmds[i].value-pe.cmds[i].value;
00113       ans+=dif*dif;
00114       cnt++;
00115     }
00116   return ans/cnt;
00117 }
00118 
00119 float PostureEngine::maxdiff(const PostureEngine& pe) const {
00120   float max=0;
00121   for(unsigned int i=0; i<NumOutputs; i++)
00122     if(cmds[i].weight>0 && pe.cmds[i].weight>0) {
00123       float dif=cmds[i].value-pe.cmds[i].value;
00124       if(dif>max)
00125         max=dif;
00126     }
00127   return max;
00128 }
00129 
00130 unsigned int PostureEngine::getBinSize() const {
00131   unsigned int len=11;
00132   char buf[255];
00133   for(unsigned int i=0; i<NumOutputs; i++)
00134     if(cmds[i].weight>0)
00135       len+=snprintf(buf,len,"%s\t% .4f\t% .4f\n",outputNames[i],cmds[i].value,cmds[i].weight);
00136   return len;
00137 }
00138 
00139 unsigned int PostureEngine::LoadBuffer(const char buf[], unsigned int len) {
00140   unsigned int origlen=len;
00141   clear();
00142   if(strncmp("#POS",buf,4)!=0) {
00143     // we don't want to display an error here because we may be only testing file type,
00144     // so it's up to the caller to decide if it's necessarily an error if the file isn't
00145     // a posture
00146     //std::cout << "ERROR PostureEngine load corrupted - expected #POS header" << std::endl;
00147     return 0;
00148   }
00149   char formatstring[64];
00150   snprintf(formatstring,64,"%%%dc %%f %%f%%n",outputNameLen); //std::cout << "Format: " << formatstring << std::endl;
00151   unsigned int idx=0;
00152   unsigned int linenum=1;
00153   char jname[outputNameLen+1];
00154   jname[outputNameLen]='\0';
00155   while(len<=origlen && len>0) {
00156     float fval, fwht=1;
00157     int written;
00158     //    printf("%d %.9s\n",linenum+1,buf);
00159     if(buf[0]=='\r') {
00160       buf++; len--;
00161       if(buf[0]=='\n') {
00162         buf++; len--;
00163       }
00164       linenum++;
00165       continue;
00166     }
00167     if(buf[0]=='\n') {
00168       buf++; len--;
00169       linenum++;
00170       continue;
00171     }
00172     if(buf[0]=='#') {
00173       if(strncmp("#END\n",buf,5)==0 || strncmp("#END\r",buf,5)==0) {
00174         return origlen-len+5;
00175       } else if(strncmp("#END\r\n",buf,6)==0) {
00176         return origlen-len+6;
00177       } else {
00178         while(len>0 && *buf!='\n' && *buf!='\r') {len--;buf++;}
00179         if(*buf=='\n') { //in case of \r\n
00180           buf++;
00181           len--;
00182         }
00183         linenum++;
00184         continue;
00185       }
00186     }
00187     written=-1;
00188     jname[0]='\0';
00189     sscanf(buf,formatstring,jname,&fval,&fwht,&written);
00190     if(!ChkAdvance(written,&buf,&len,"*** ERROR PostureEngine load corrupted - line %d\n",linenum)) return 0;
00191     while(*buf!='\n' && *buf!='\r')
00192       buf++;
00193     if(buf[0]=='\r' && buf[1]=='\n')
00194       buf+=2;
00195     else
00196       buf++;
00197     linenum++;
00198     //std::cout << '"' << jname << "\"\t" << (float)fval << '\t' << (float)fwht << std::endl;
00199     // we continue the search in order from where we left off - these are often
00200     // going to go in order, so might as well save a little time
00201     unsigned int startidx=idx;
00202     for(;idx<NumOutputs;idx++)
00203       if(strcmp(jname,outputNames[idx])==0) {
00204         cmds[idx].set(fval,fwht);
00205         break;
00206       }
00207     if(idx==NumOutputs) { //not found following startidx, look from beginning
00208       for(idx=0;idx<startidx;idx++)
00209         if(strcmp(jname,outputNames[idx])==0) {
00210           cmds[idx].set(fval,fwht);
00211           break;
00212       }
00213       if(idx==startidx && strcmp(jname,outputNames[idx])!=0) //not found at all
00214         std::cout << "*** WARNING '" << jname << "' is not a valid joint on this model." << std::endl;
00215     }
00216   }
00217   std::cout << "*** WARNING PostureEngine load missing #END" << std::endl;
00218   return origlen-len;
00219 }
00220 
00221 //Why do i need a cast to const char**??? It doesn't work without, should be automatic... grrrr
00222 unsigned int PostureEngine::SaveBuffer(char buf[], unsigned int len) const {
00223   unsigned int origlen=len;
00224   int written=snprintf(buf,len,"#POS\n");
00225   if(!ChkAdvance(written,(const char**)&buf,&len,"*** ERROR PostureEngine save failed on header\n")) return 0;
00226   if(len==0 || len>origlen) {
00227     std::cout << "*** ERROR PostureEngine save overflow on header" << std::endl;
00228     return 0;
00229   }
00230   for(unsigned int i=0; i<NumOutputs; i++)
00231     if(cmds[i].weight>0) {
00232       written=snprintf(buf,len,"%s\t% .4f\t% .4f\n",outputNames[i],cmds[i].value,cmds[i].weight);
00233       if(!ChkAdvance(written,(const char**)&buf,&len,"*** ERROR PostureEngine save failed\n")) return 0;
00234       if(len==0 || len>origlen) {
00235         std::cout << "*** ERROR PostureEngine save overflow" << std::endl;
00236         return 0;
00237       }
00238     }
00239   written=snprintf(buf,len,"#END\n");
00240   if(!ChkAdvance(written,(const char**)&buf,&len,"*** ERROR PostureEngine save failed on #END\n")) return 0;
00241   if(len==0 || len>origlen) {
00242     std::cout << "*** ERROR PostureEngine save overflow on #END" << std::endl;
00243     return 0;
00244   }
00245   return origlen-len;
00246 }
00247 
00248 unsigned int PostureEngine::LoadFile(const char filename[]) {
00249   return LoadSave::LoadFile(config->motion.makePath(filename).c_str());
00250 }
00251 unsigned int PostureEngine::SaveFile(const char filename[]) const {
00252   return LoadSave::SaveFile(config->motion.makePath(filename).c_str());
00253 }
00254 
00255 /*
00256 NEWMAT::ReturnMatrix
00257 Kinematics::getOrientation(unsigned int j) {
00258   unsigned int c=-1U,l=-1U;
00259   if(!lookup(j,c,l)) {
00260     NEWMAT::Matrix A(4,4);
00261     A<<ROBOOP::fourbyfourident;
00262     A.Release(); return A;
00263   }
00264   update(c,l);
00265   NEWMAT::Matrix R;
00266   NEWMAT::ColumnVector p;
00267   chains[c]->kine(R,p,j);
00268   R.Release(); return R;
00269 }
00270 
00271 NEWMAT::ReturnMatrix
00272 Kinematics::getPosition(unsigned int j) {
00273   unsigned int c=-1U,l=-1U;
00274   if(!lookup(j,c,l)) {
00275     NEWMAT::Matrix A(4,4);
00276     A<<ROBOOP::fourbyfourident;
00277     A.Release(); return A;
00278   }
00279   update(c,l);
00280   NEWMAT::Matrix R;
00281   NEWMAT::ColumnVector p;
00282   chains[c]->kine(R,p,j);
00283   p.Release(); return p;
00284 }
00285 */
00286 
00287 bool
00288 PostureEngine::solveLinkPosition(const NEWMAT::ColumnVector& Pobj, unsigned int j, const NEWMAT::ColumnVector& Plink) {
00289   unsigned int c=-1U,l=-1U;
00290   if(!lookup(j,c,l))
00291     return false;
00292   update(c,l);
00293   bool conv=false;
00294   NEWMAT::ColumnVector q=chains[c]->inv_kin_pos(Pobj,0,l,Plink,conv);
00295   for(unsigned int i=1; i<=l && i<=chainMaps[c].size(); i++)
00296     if(chainMaps[c][i]<NumOutputs)
00297       setOutputCmd(chainMaps[c][i],chains[c]->get_q(i));
00298   return conv;
00299 }
00300 
00301 bool
00302 PostureEngine::solveLinkVector(const NEWMAT::ColumnVector& Pobj, unsigned int j, const NEWMAT::ColumnVector& Plink) {
00303   solveLinkPosition(Pobj,j,Plink);
00304   //This method is an approximation, not entirely precise or fast as it could be
00305   //Something to work on some more down the road! :)
00306   //(this method is shared with HeadPointerMC::lookAtPoint(x,y,z))
00307   NEWMAT::ColumnVector poE=baseToLink(j)*Pobj;
00308   if(poE.nrows()>3 && poE(4)!=0) {
00309     poE/=poE(4);
00310   }
00311   poE=poE.SubMatrix(1,3,1,1);
00312   NEWMAT::ColumnVector plE=Plink.SubMatrix(1,3,1,1);
00313   if(Plink.nrows()>3 && Plink(4)!=0)
00314     plE/=Plink(4);
00315   float plE2=plE.SumSquare();
00316   float plE_len=sqrt(plE2);
00317   float obj_comp_link=NEWMAT::DotProduct(plE,poE)/plE_len;
00318   if(obj_comp_link<plE_len)
00319     obj_comp_link=obj_comp_link*.975; //.975 is a bit of fudge - accounts for joints moving Plink when adjusting
00320   else
00321     obj_comp_link=obj_comp_link/.975; //.975 is a bit of fudge - accounts for joints moving Plink when adjusting
00322   NEWMAT::ColumnVector obj_proj_link(4);
00323   obj_proj_link.SubMatrix(1,3,1,1)=obj_comp_link*plE/plE_len;
00324   obj_proj_link(4)=1;
00325   return solveLinkPosition(Pobj,j,obj_proj_link);
00326 }
00327 
00328 void
00329 PostureEngine::update(unsigned int c, unsigned int l) {
00330   for(unsigned int j=1; j<=l; j++) {
00331     unsigned int tkout=chainMaps[c][j];
00332     if(tkout<NumOutputs)
00333       chains[c]->set_q(getOutputCmd(tkout).value,j);
00334   }
00335 }
00336 
00337 /*! @file
00338  * @brief Implements PostureEngine, a base class for managing the values and weights of all the outputs
00339  * @author ejt (Creator)
00340  *
00341  * $Author: ejt $
00342  * $Name: tekkotsu-2_4_1 $
00343  * $Revision: 1.24 $
00344  * $State: Exp $
00345  * $Date: 2005/01/07 19:36:38 $
00346  */
00347 

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:48 2005 by Doxygen 1.4.4