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

Tekkotsu v2.2.1
Generated Tue Nov 23 16:36:39 2004 by Doxygen 1.3.9.1