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     sscanf(buf,formatstring,jname,&fval,&fwht,&written);
00184     if(!ChkAdvance(written,&buf,&len,"*** ERROR PostureEngine load corrupted - line %d\n",linenum)) return 0;
00185     while(*buf!='\n' && *buf!='\r')
00186       buf++;
00187     if(buf[0]=='\r' && buf[1]=='\n')
00188       buf+=2;
00189     else
00190       buf++;
00191     linenum++;
00192     //std::cout << '"' << jname << "\"\t" << (float)fval << '\t' << (float)fwht << std::endl;
00193     unsigned int startidx=idx+1;
00194     for(;idx<NumOutputs;idx++)
00195       if(strcmp(jname,outputNames[idx])==0) {
00196         cmds[idx].set(fval,fwht);
00197         break;
00198       }
00199     if(idx==NumOutputs) {
00200       for(idx=0;idx<startidx;idx++)
00201         if(strcmp(jname,outputNames[idx])==0) {
00202           cmds[idx].set(fval,fwht);
00203           break;
00204       }
00205       if(idx==startidx && strcmp(jname,outputNames[idx])!=0)
00206         std::cout << "*** WARNING " << jname << " is not a valid joint on this model." << std::endl;
00207     }
00208   }
00209   std::cout << "*** WARNING PostureEngine load missing #END" << std::endl;
00210   return origlen-len;
00211 }
00212 
00213 //Why do i need a cast to const char**??? It doesn't work without, should be automatic... grrrr
00214 unsigned int PostureEngine::SaveBuffer(char buf[], unsigned int len) const {
00215   unsigned int origlen=len;
00216   int written=snprintf(buf,len,"#POS\n");
00217   if(!ChkAdvance(written,(const char**)&buf,&len,"*** ERROR PostureEngine save failed on header\n")) return 0;
00218   if(len==0 || len>origlen) {
00219     std::cout << "*** ERROR PostureEngine save overflow on header" << std::endl;
00220     return 0;
00221   }
00222   for(unsigned int i=0; i<NumOutputs; i++)
00223     if(cmds[i].weight>0) {
00224       written=snprintf(buf,len,"%s\t% .4f\t% .4f\n",outputNames[i],cmds[i].value,cmds[i].weight);
00225       if(!ChkAdvance(written,(const char**)&buf,&len,"*** ERROR PostureEngine save failed\n")) return 0;
00226       if(len==0 || len>origlen) {
00227         std::cout << "*** ERROR PostureEngine save overflow" << std::endl;
00228         return 0;
00229       }
00230     }
00231   written=snprintf(buf,len,"#END\n");
00232   if(!ChkAdvance(written,(const char**)&buf,&len,"*** ERROR PostureEngine save failed on #END\n")) return 0;
00233   if(len==0 || len>origlen) {
00234     std::cout << "*** ERROR PostureEngine save overflow on #END" << std::endl;
00235     return 0;
00236   }
00237   return origlen-len;
00238 }
00239 
00240 unsigned int PostureEngine::LoadFile(const char filename[]) {
00241   return LoadSave::LoadFile(config->motion.makePath(filename).c_str());
00242 }
00243 unsigned int PostureEngine::SaveFile(const char filename[]) const {
00244   return LoadSave::SaveFile(config->motion.makePath(filename).c_str());
00245 }
00246 
00247 /*
00248 NEWMAT::ReturnMatrix
00249 Kinematics::getOrientation(unsigned int j) {
00250   unsigned int c=-1U,l=-1U;
00251   if(!lookup(j,c,l)) {
00252     NEWMAT::Matrix A(4,4);
00253     A<<ROBOOP::fourbyfourident;
00254     A.Release(); return A;
00255   }
00256   update(c,l);
00257   NEWMAT::Matrix R;
00258   NEWMAT::ColumnVector p;
00259   chains[c]->kine(R,p,j);
00260   R.Release(); return R;
00261 }
00262 
00263 NEWMAT::ReturnMatrix
00264 Kinematics::getPosition(unsigned int j) {
00265   unsigned int c=-1U,l=-1U;
00266   if(!lookup(j,c,l)) {
00267     NEWMAT::Matrix A(4,4);
00268     A<<ROBOOP::fourbyfourident;
00269     A.Release(); return A;
00270   }
00271   update(c,l);
00272   NEWMAT::Matrix R;
00273   NEWMAT::ColumnVector p;
00274   chains[c]->kine(R,p,j);
00275   p.Release(); return p;
00276 }
00277 */
00278 
00279 bool
00280 PostureEngine::solveLinkPosition(const NEWMAT::ColumnVector& Pobj, unsigned int j, const NEWMAT::ColumnVector& Plink) {
00281   unsigned int c=-1U,l=-1U;
00282   if(!lookup(j,c,l))
00283     return false;
00284   update(c,l);
00285   bool conv=false;
00286   NEWMAT::ColumnVector q=chains[c]->inv_kin_pos(Pobj,0,l,Plink,conv);
00287   for(unsigned int i=1; i<=l && i<=chainMaps[c].size(); i++)
00288     if(chainMaps[c][i]<NumOutputs)
00289       setOutputCmd(chainMaps[c][i],chains[c]->get_q(i));
00290   return conv;
00291 }
00292 
00293 bool
00294 PostureEngine::solveLinkVector(const NEWMAT::ColumnVector& Pobj, unsigned int j, const NEWMAT::ColumnVector& Plink) {
00295   solveLinkPosition(Pobj,j,Plink);
00296   //This method is an approximation, not entirely precise or fast as it could be
00297   //Something to work on some more down the road! :)
00298   NEWMAT::ColumnVector poE=baseToLink(j)*Pobj;
00299   if(poE.nrows()>3 && poE(4)!=0) {
00300     poE/=poE(4);
00301   }
00302   poE=poE.SubMatrix(1,3,1,1);
00303   NEWMAT::ColumnVector plE=Plink.SubMatrix(1,3,1,1);
00304   if(Plink.nrows()>3 && Plink(4)!=0)
00305     plE/=Plink(4);
00306   float plE2=plE.SumSquare();
00307   float plE_len=sqrt(plE2);
00308   float obj_comp_link=NEWMAT::DotProduct(plE,poE)/plE_len;
00309   if(obj_comp_link<plE_len)
00310     obj_comp_link=obj_comp_link*.975; //.975 is a bit of fudge - accounts for joints moving Plink when adjusting
00311   else
00312     obj_comp_link=obj_comp_link/.975; //.975 is a bit of fudge - accounts for joints moving Plink when adjusting
00313   NEWMAT::ColumnVector obj_proj_link(4);
00314   obj_proj_link.SubMatrix(1,3,1,1)=obj_comp_link*plE/plE_len;
00315   obj_proj_link(4)=1;
00316   return solveLinkPosition(Pobj,j,obj_proj_link);
00317 }
00318 
00319 void
00320 PostureEngine::update(unsigned int c, unsigned int l) {
00321   for(unsigned int j=1; j<=l; j++) {
00322     unsigned int tkout=chainMaps[c][j];
00323     if(tkout<NumOutputs)
00324       chains[c]->set_q(getOutputCmd(tkout).value,j);
00325   }
00326 }
00327 
00328 /*! @file
00329  * @brief Implements PostureEngine, a base class for managing the values and weights of all the outputs
00330  * @author ejt (Creator)
00331  *
00332  * $Author: ejt $
00333  * $Name: tekkotsu-2_2 $
00334  * $Revision: 1.21 $
00335  * $State: Exp $
00336  * $Date: 2004/10/18 19:53:33 $
00337  */
00338 

Tekkotsu v2.2
Generated Tue Oct 19 14:19:15 2004 by Doxygen 1.3.9.1