Tekkotsu 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 "Shared/string_util.h"
00006 #include <stdio.h>
00007 #include <iostream>
00008 #include <regex.h>
00009 #include <map>
00010 
00011 using namespace std;
00012 
00013 PostureEngine::~PostureEngine() {}
00014 
00015 void PostureEngine::takeSnapshot() {
00016   takeSnapshot(*WorldState::getCurrent());
00017 }
00018 
00019 void PostureEngine::takeSnapshot(const WorldState& st) {
00020   for(unsigned int i=0; i<NumOutputs; i++)
00021     cmds[i].value=st.outputs[i];
00022 }
00023 
00024 void PostureEngine::setWeights(float w, unsigned int lowjoint, unsigned int highjoint) {
00025   for(unsigned int i=lowjoint; i<highjoint; i++)
00026     cmds[i].weight=w;
00027 }
00028 
00029 void PostureEngine::clear() {
00030   for(unsigned int i=0; i<NumOutputs; i++)
00031     cmds[i].unset();
00032 }
00033 
00034 PostureEngine& PostureEngine::setOverlay(const PostureEngine& pe) {
00035   for(unsigned int i=0; i<NumOutputs; i++)
00036     if(pe.cmds[i].weight>0)
00037       cmds[i]=pe.cmds[i];
00038   return *this;
00039 }
00040 PostureEngine PostureEngine::createOverlay(const PostureEngine& pe) const {
00041   PostureEngine tmp(*this);
00042   return tmp.setOverlay(pe);
00043 }
00044 PostureEngine& PostureEngine::setUnderlay(const PostureEngine& pe) {
00045   for(unsigned int i=0; i<NumOutputs; i++)
00046     if(cmds[i].weight<=0)
00047       cmds[i]=pe.cmds[i];
00048   return *this;
00049 }
00050 PostureEngine PostureEngine::createUnderlay(const PostureEngine& pe) const {
00051   PostureEngine tmp(*this);
00052   return tmp.setUnderlay(pe);
00053 }
00054 /*! joints being averaged with unused joints have their weights averaged, but not their values (so an output can crossfade properly)\n
00055  *  @param pe the other PostureEngine
00056  *  @param w amount to weight towards @a pe
00057  *  - if @a w < .001, nothing is done
00058  *  - if @a w > .999, a straight copy of @a pe occurs (sets joints to unused properly at end of fade)
00059  *  - .001 and .999 is used instead of 0 and 1 to allow for slight addition errors in a loop (if
00060  *    using repeated additions of a delta value instead of repeated divisions)
00061  *  @return @c *this, stores results into this */
00062 PostureEngine& PostureEngine::setAverage(const PostureEngine& pe, float w) {
00063   if(w<0.001)
00064     return *this;
00065   if(w>0.999)
00066     return (*this=pe);
00067   float wp=1-w;
00068   for(unsigned int i=0; i<NumOutputs; i++)
00069     if(cmds[i].weight>0) {
00070       if(pe.cmds[i].weight>0)
00071         cmds[i].set(cmds[i].value*wp+pe.cmds[i].value*w,cmds[i].weight*wp+pe.cmds[i].weight*w);
00072       else
00073         cmds[i].weight*=wp;
00074     } else
00075       cmds[i].set(pe.cmds[i].value,pe.cmds[i].weight*w);
00076   return *this;
00077 }
00078 /*! joints being averaged with weight<=0 have their weights averaged, but not their values (so an output can crossfade properly)\n
00079  *  @param pe the other PostureEngine
00080  *  @param w amount to weight towards @a pe
00081  *  - if @a w < .001, nothing is done
00082  *  - if @a w > .999, a straight copy of @a pe occurs (sets joints to unused properly at end of fade)
00083  *  - .001 and .999 is used instead of 0 and 1 to allow for slight addition errors in a loop (if
00084  *    using repeated additions of a delta value instead of repeated divisions)
00085  *  @return a new posture containing the results */
00086 PostureEngine PostureEngine::createAverage(const PostureEngine& pe, float w) const {
00087   PostureEngine tmp(*this);
00088   return tmp.setAverage(pe,w);
00089 }
00090 PostureEngine& PostureEngine::setCombine(const PostureEngine& pe) {
00091   for(unsigned int i=0; i<NumOutputs; i++) {
00092     float total=cmds[i].weight+pe.cmds[i].weight;
00093     cmds[i].set((cmds[i].value*cmds[i].weight+pe.cmds[i].value*pe.cmds[i].weight)/total,total);
00094   }
00095   return *this;
00096 }
00097 PostureEngine PostureEngine::createCombine(const PostureEngine& pe) const {
00098   PostureEngine tmp(*this);
00099   return tmp.setCombine(pe);
00100 }
00101 
00102 float PostureEngine::diff(const PostureEngine& pe) const {
00103   float ans=0;
00104   for(unsigned int i=0; i<NumOutputs; i++)
00105     if(cmds[i].weight>0 && pe.cmds[i].weight>0) {
00106       float dif=cmds[i].value-pe.cmds[i].value;
00107       ans+=dif*dif;
00108     }
00109   return ans;
00110 }
00111 
00112 float PostureEngine::avgdiff(const PostureEngine& pe) const {
00113   float ans=0;
00114   unsigned int cnt=0;
00115   for(unsigned int i=0; i<NumOutputs; i++)
00116     if(cmds[i].weight>0 && pe.cmds[i].weight>0) {
00117       float dif=cmds[i].value-pe.cmds[i].value;
00118       ans+=dif*dif;
00119       cnt++;
00120     }
00121   return ans/cnt;
00122 }
00123 
00124 float PostureEngine::maxdiff(const PostureEngine& pe) const {
00125   float max=0;
00126   for(unsigned int i=0; i<NumOutputs; i++)
00127     if(cmds[i].weight>0 && pe.cmds[i].weight>0) {
00128       float dif=cmds[i].value-pe.cmds[i].value;
00129       if(dif>max)
00130         max=dif;
00131     }
00132   return max;
00133 }
00134 
00135 void PostureEngine::setSaveFormat(bool condensed, WorldState* ws) {
00136   saveFormatCondensed=condensed;
00137   loadSaveSensors=ws;
00138 }
00139 
00140 unsigned int PostureEngine::getBinSize() const {
00141   unsigned int written=11; //accounts for initial #POS\n and final #END\n, plus 1
00142   const unsigned int len=0;
00143   char buf[len];
00144   if(saveFormatCondensed) {
00145 #ifndef PLATFORM_APERIOS
00146     written+=snprintf(buf,len,"condensed %s\n",RobotName);
00147 #else
00148     written+=snprintf(buf,len,"condensed %s\n",RobotName.c_str());
00149 #endif
00150     if(loadSaveSensors!=NULL)
00151       written+=snprintf(buf,len,"meta-info = %u %u\n",loadSaveSensors->lastSensorUpdateTime,loadSaveSensors->frameNumber);
00152     bool weightsAllEqual=true;
00153     float weightsVal=cmds[0].weight;
00154     for(unsigned int i=1; i<NumOutputs && weightsAllEqual; i++)
00155       weightsAllEqual=(cmds[i].weight==weightsVal);
00156     if(!weightsAllEqual || weightsVal!=0) { //if they're all 0, skip outputs and weights
00157       written+=snprintf(buf,len,"outputs =");
00158       for(unsigned int i=0; i<NumOutputs; i++) {
00159         written+=snprintf(buf,len," %g",cmds[i].value);
00160       }
00161       if(!weightsAllEqual || weightsVal!=1) { //if they're all 1, skip weights
00162         written+=snprintf(buf,len,"\nweights =");
00163         for(unsigned int i=0; i<NumOutputs; i++) {
00164           written+=snprintf(buf,len," %g",cmds[i].weight);
00165         }
00166       }
00167       written+=snprintf(buf,len,"\n");
00168     }
00169     if(loadSaveSensors!=NULL) {
00170       written+=snprintf(buf,len,"buttons =");
00171       for(unsigned int i=0; i<NumButtons; i++) {
00172         written+=snprintf(buf,len," %g",loadSaveSensors->buttons[i]);
00173       }
00174       written+=snprintf(buf,len,"\nsensors =");
00175       for(unsigned int i=0; i<NumSensors; i++) {
00176         written+=snprintf(buf,len," %g",loadSaveSensors->sensors[i]);
00177       }
00178       written+=snprintf(buf,len,"\npidduties =");
00179       for(unsigned int i=0; i<NumPIDJoints; i++) {
00180         written+=snprintf(buf,len," %g",loadSaveSensors->pidduties[i]);
00181       }
00182       written+=snprintf(buf,len,"\n");
00183     }   
00184   } else {
00185     if(loadSaveSensors!=NULL)
00186       written+=snprintf(buf,len,"<meta-info>\n  timestamp\t%u\n  framenumber\t%u\n</meta-info>\n",loadSaveSensors->lastSensorUpdateTime,loadSaveSensors->frameNumber);
00187     for(unsigned int i=0; i<NumOutputs; i++)
00188       if(cmds[i].weight>0) {
00189         if(cmds[i].weight==1)
00190           written+=snprintf(buf,len,"%s\t% .4f\n",outputNames[i],cmds[i].value);
00191         else
00192           written+=snprintf(buf,len,"%s\t% .4f\t% .4f\n",outputNames[i],cmds[i].value,cmds[i].weight);
00193       }
00194     if(loadSaveSensors!=NULL) {
00195       written+=snprintf(buf,len,"<buttons>\n");
00196       for(unsigned int i=0; i<NumButtons; i++) {
00197         written+=snprintf(buf,len,"  %s\t% .4f\t\n",buttonNames[i],loadSaveSensors->buttons[i]);
00198       }
00199       written+=snprintf(buf,len,"</buttons><sensors>\n");
00200       for(unsigned int i=0; i<NumSensors; i++) {
00201         written+=snprintf(buf,len,"  %s\t% .4f\t\n",sensorNames[i],loadSaveSensors->sensors[i]);
00202       }
00203       written+=snprintf(buf,len,"</sensors><pidduties>\n");
00204       for(unsigned int i=0; i<NumPIDJoints; i++) {
00205         written+=snprintf(buf,len,"  duty-%s\t% .4f\t\n",outputNames[i],loadSaveSensors->pidduties[i]);
00206       }
00207       written+=snprintf(buf,len,"</pidduties>\n");
00208     }
00209   }
00210   return written;
00211 }
00212 
00213 unsigned int PostureEngine::loadBuffer(const char buf[], unsigned int len) {
00214   unsigned int origlen=len;
00215   clear();
00216   if(buf==NULL || len==0) {
00217     cerr << "*** ERROR: PostureEngine::loadBuffer(" << static_cast<const void*>(buf) << ',' << len << ")" << endl;
00218     return 0;
00219   }
00220   if(strncmp("#POS",buf,4)!=0) {
00221     // we don't want to display an error here because we may be only testing file type,
00222     // so it's up to the caller to decide if it's necessarily an error if the file isn't
00223     // a posture
00224     //std::cout << "ERROR PostureEngine load corrupted - expected #POS header" << std::endl;
00225     return 0;
00226   }
00227   saveFormatCondensed=false;
00228   Capabilities* caps = &capabilities;
00229   unsigned int linenum=0;
00230   bool filtered=false; // true if in a specialization section for a different model
00231   section_t curSection=SECTION_OUTPUTS;
00232   map<string,section_t> sectionMap;
00233   sectionMap["meta-info"]=SECTION_METAINFO;
00234   sectionMap["outputs"]=SECTION_OUTPUTS;
00235   sectionMap["buttons"]=SECTION_BUTTONS;
00236   sectionMap["sensors"]=SECTION_SENSORS;
00237   sectionMap["pidduties"]=SECTION_PIDDUTIES;
00238   
00239   
00240   while(len<=origlen && len>0) {
00241     // extract a line from the buffer (line endings could be \r, \n, or \r\n )
00242     string line;
00243     for(unsigned int lineend=0; lineend<len; ++lineend) {
00244       bool isreturn = (buf[lineend]=='\r');
00245       if(isreturn || buf[lineend]=='\n') {
00246         line.assign(buf,lineend);
00247         ++linenum;
00248         ++lineend;
00249         buf+=lineend;
00250         len-=lineend;
00251         if(len==0) // end of buffer, don't check for \n
00252           break;
00253         if(isreturn && buf[0]=='\n') { // indicates an \r\n
00254           ++buf;
00255           --len;
00256         }
00257         break;
00258       }
00259     }
00260     
00261     // strip comment
00262     string::size_type commentPos = line.find('#');
00263     if(commentPos!=string::npos) {
00264       if(line.substr(commentPos)=="#END")
00265         return origlen-len;
00266       line = line.substr(0,commentPos);
00267     }
00268     
00269     // tokenize
00270     // whitespace and '=' are used as delimiters
00271     vector<string> words;
00272     for(unsigned int i=0; i<line.size(); ++i) {
00273       if(isspace(line[i]) || line[i]=='=')
00274         continue;
00275       unsigned int j=i+1;
00276       while(j<line.size() && !isspace(line[j]) && line[j]!='=')
00277         ++j;
00278       words.push_back(line.substr(i,j-i));
00279       i=j;
00280     }
00281     if(words.size()==0)
00282       continue;
00283     
00284     // debugging output
00285     /*cout << "LINE " << linenum << endl;
00286     for(unsigned int i=0; i<words.size(); ++i)
00287       cout << '\t' << words[i] << endl;*/
00288     
00289     // process the line!
00290     if(!loadLine(linenum,sectionMap,words,curSection,caps,filtered))
00291       return 0;
00292     
00293   } // end of process-line loop 'while'
00294     
00295   // shouldn't get here, proper file ends with '#END', which will trigger a 'return' within the loop
00296   std::cout << "*** WARNING PostureEngine load missing #END" << std::endl;
00297   return origlen-len;
00298 }
00299 
00300 bool PostureEngine::loadLine(unsigned int linenum, const std::map<std::string,section_t>& sectionMap, std::vector<std::string>& words, section_t& curSection, Capabilities*& caps, bool& filtered) {
00301   if(words[0]=="specialize") {
00302     if(words.size()>2)
00303       cerr << "*** Warning line " << linenum << ", extra arguments to 'specialize'" << endl;
00304     filtered = false;
00305     if(words.size()>1) try {
00306       filtered = !string_util::reMatch(RobotName,words[1],REG_EXTENDED);
00307     } catch(const std::string& err) {
00308       cerr << "*** ERROR line " << linenum << ", bad regular expression: " << err << endl;
00309       return false;
00310     }
00311     
00312   } else if(words[0]=="condensed") {
00313     if(words.size()>2)
00314       cerr << "*** Warning line " << linenum << ", extra arguments to 'condensed'" << endl;
00315     if(words.size()==1) {
00316       cerr << "*** ERROR line " << linenum << ", missing model name for 'condensed'" << endl;
00317       return false;
00318     } else {
00319       caps = getCapabilities(words[1]);
00320       if(caps==NULL) {
00321         cerr << "*** ERROR line " << linenum << ", unknown model for 'condensed': " << words[1] << endl;
00322         return false;
00323       }
00324       saveFormatCondensed=true;
00325     }
00326     
00327   } else if(words[0]=="verbose") {
00328     if(words.size()>1)
00329       cerr << "*** Warning line " << linenum << ", extra arguments to 'verbose'" << endl;
00330     saveFormatCondensed=false;
00331     
00332   } else if(words[0][0]=='<' && words[0][words[0].size()-1]=='>') {
00333     if(words.size()>1)
00334       cerr << "*** Warning line " << linenum << ", extra arguments to section tag (this isn't a real XML format, tag must be on line by itself)" << endl;
00335     bool isend = (words[0][1]=='/');
00336     const string name = words[0].substr(isend?1:2, words[0].size()-(isend?2:3));
00337     map<string,section_t>::const_iterator it = sectionMap.find(name);
00338     if(it==sectionMap.end()) {
00339       cerr << "*** ERROR line " << linenum << ", unknown section '" << name << "'" << endl;
00340       return false;
00341     }
00342     section_t section = it->second;
00343     if(isend) {
00344       if(curSection!=section)
00345         cerr << "*** Warning line " << linenum << ", mismatched close tag " << name << endl;
00346       curSection=SECTION_OUTPUTS;
00347     } else {
00348       if(curSection!=SECTION_OUTPUTS)
00349         cerr << "*** Warning line " << linenum << ", nested tags not supported, or missing close tag" << endl;
00350       curSection=section;
00351     }
00352     
00353   } else if(filtered) {
00354     // no-op
00355     
00356   } else {
00357     // either a section name (condensed form) or an output/button/sensor name (verbose form)
00358     map<string,section_t>::const_iterator it = sectionMap.find(words[0]);
00359     if(it==sectionMap.end()) {
00360       // not an entry in sectionMap...
00361       if(words[0]=="weights") { // weights is a "condensed-only" section, handled a little specially since it's not in sectionMap...
00362         if(caps==&capabilities) {
00363           // we're using the "native" target... make things a bit quicker
00364           for(unsigned int i=1; i<words.size(); ++i)
00365             cmds[i-1].weight = atof(words[i].c_str());
00366         } else {
00367           // we're using another target, map to the host
00368           for(unsigned int i=1; i<words.size(); ++i) {
00369             const char * name = caps->getOutputName(i-1);
00370             unsigned int off = capabilities.findOutputOffset(name);
00371             if(off==-1U) {
00372               cerr << "*** Warning line " << linenum << ", output '" << name << "' from robot " << caps->getRobotName() << " cannot be mapped to host " << RobotName << endl;
00373             } else {
00374               cmds[off].weight = atof(words[i].c_str());
00375             }
00376           }
00377         }
00378       } else {
00379         // line is not a section declaration -- is it an 'verbose' output/sensor/button/etc. name within the current section??
00380         if(words.size()<2) {
00381           cerr << "*** ERROR line " << linenum << ", no value supplied for " << words[0] << endl;
00382           return false;
00383         }
00384         switch(curSection) {
00385           case SECTION_METAINFO: {
00386             if(words[0]=="timestamp") {
00387               if(loadSaveSensors!=NULL)
00388                 loadSaveSensors->lastSensorUpdateTime = atoi(words[1].c_str());
00389             } else if(words[0]=="framenumber") {
00390               if(loadSaveSensors!=NULL)
00391                 loadSaveSensors->frameNumber = atoi(words[1].c_str());
00392             } else {
00393               cerr << "*** Warning line " << linenum << ", '" << words[0] << "' is not a valid meta-info" << endl;
00394             }
00395           } break;
00396           case SECTION_OUTPUTS: {
00397             stripTildes(words[0]);
00398             unsigned int off = capabilities.findOutputOffset(words[0].c_str()), loff=-1U;
00399             if(off==-1U) {
00400               // try again with L/R prefixes
00401               loff = capabilities.findOutputOffset(('L'+words[0]).c_str());
00402               if(loff!=-1U)
00403                 off = capabilities.findOutputOffset(('R'+words[0]).c_str());
00404             }
00405             if(off==-1U)
00406               cerr << "*** Warning line " << linenum << ", '" << words[0] << "' is not a valid output on this model (" << RobotName << ")" << endl;
00407             else {
00408               float value = atof(words[1].c_str());
00409               float weight=1;
00410               if(words.size()>2)
00411                 weight = atof(words[2].c_str());
00412               cmds[off].set(value,weight);
00413               if(loff!=-1U)
00414                 cmds[loff].set(value,weight);
00415             }
00416           } break;
00417           case SECTION_BUTTONS: {
00418             unsigned int off = capabilities.findButtonOffset(words[0].c_str());
00419             if(off==-1U) {
00420               cerr << "*** Warning line " << linenum << ", '" << words[0] << "' is not a valid button on this model (" << RobotName << ")" << endl;
00421             } else if(loadSaveSensors!=NULL) {
00422               loadSaveSensors->buttons[off] = atof(words[1].c_str());
00423             }
00424           } break;
00425           case SECTION_SENSORS: {
00426             unsigned int off = capabilities.findSensorOffset(words[0].c_str());
00427             if(off==-1U) {
00428               cerr << "*** Warning line " << linenum << ", '" << words[0] << "' is not a valid sensor on this model (" << RobotName << ")" << endl;
00429             } else if(loadSaveSensors!=NULL) {
00430               loadSaveSensors->sensors[off] = atof(words[1].c_str());
00431             }
00432           } break;
00433           case SECTION_PIDDUTIES: {
00434             stripTildes(words[0]);
00435             unsigned int off = capabilities.findOutputOffset(words[0].c_str());
00436             if(off==-1U) {
00437               cerr << "*** Warning line " << linenum << ", '" << words[0] << "' is not a valid output on this model (" << RobotName << ")" << endl;
00438             } else if(off<PIDJointOffset || off>=PIDJointOffset+NumPIDJoints) {
00439               cerr << "*** Warning line " << linenum << ", output '" << words[0] << "' from robot " << caps->getRobotName() << " does not map to a PID joint on the local host " << RobotName << endl;
00440             } else if(loadSaveSensors!=NULL) {
00441               loadSaveSensors->pidduties[off-PIDJointOffset] = atof(words[1].c_str());
00442             }
00443           } break;
00444         } // end of section 'switch'
00445       } // end of condensed 'weight' section vs. verbose entry 'else'
00446       
00447     } else {
00448       // is a section name (aka condensed entry)
00449       switch(it->second) {
00450         case SECTION_METAINFO: {
00451           if(loadSaveSensors==NULL)
00452             break;
00453           if(words.size()>1)
00454             loadSaveSensors->lastSensorUpdateTime = atoi(words[1].c_str());
00455           if(words.size()>1)
00456             loadSaveSensors->frameNumber = atoi(words[2].c_str());
00457         } break;
00458         case SECTION_OUTPUTS: {
00459           unsigned int size=words.size();
00460           if(size-1!=caps->getNumOutputs()) {
00461             cerr << "*** ERROR line " << linenum << ", " << caps->getRobotName() << " expected " << caps->getNumOutputs() << " values, got " << (size-1) << endl;
00462             return 0;
00463           }
00464           if(caps==&capabilities) {
00465             // we're using the "native" target... make things a bit quicker
00466             for(unsigned int i=1; i<size; ++i)
00467               cmds[i-1].set(atof(words[i].c_str()));
00468           } else {
00469             // we're using another target, map to the host
00470             for(unsigned int i=1; i<size; ++i) {
00471               const char * name = caps->getOutputName(i-1);
00472               unsigned int off = capabilities.findOutputOffset(name);
00473               if(off==-1U) {
00474                 cerr << "*** Warning line " << linenum << ", output '" << name << "' from robot " << caps->getRobotName() << " cannot be mapped to host " << RobotName << endl;
00475               } else {
00476                 cmds[off].set(atof(words[i].c_str()));
00477               }
00478             }
00479           }
00480         } break;
00481         case SECTION_BUTTONS: {
00482           if(loadSaveSensors==NULL)
00483             break;
00484           unsigned int size=words.size();
00485           if(size-1!=caps->getNumButtons()) {
00486             cerr << "*** ERROR line " << linenum << ", " << caps->getRobotName() << " expected " << caps->getNumButtons() << " values, got " << (size-1) << endl;
00487             return 0;
00488           }
00489           if(caps==&capabilities) {
00490             // we're using the "native" target... make things a bit quicker
00491             for(unsigned int i=1; i<size; ++i)
00492               loadSaveSensors->buttons[i-1] = atof(words[i].c_str());
00493           } else {
00494             // we're using another target, map to the host
00495             for(unsigned int i=1; i<size; ++i) {
00496               const char * name = caps->getButtonName(i-1);
00497               unsigned int off = capabilities.findButtonOffset(name);
00498               if(off==-1U) {
00499                 cerr << "*** Warning line " << linenum << ", button '" << name << "' from robot " << caps->getRobotName() << " cannot be mapped to host " << RobotName << endl;
00500               } else {
00501                 loadSaveSensors->buttons[off] = atof(words[i].c_str());
00502               }
00503             }
00504           }
00505         } break;
00506         case SECTION_SENSORS: {
00507           if(loadSaveSensors==NULL)
00508             break;
00509           unsigned int size=words.size();
00510           if(size-1!=caps->getNumSensors()) {
00511             cerr << "*** ERROR line " << linenum << ", " << caps->getRobotName() << " expected " << caps->getNumSensors() << " values, got " << (size-1) << endl;
00512             return 0;
00513           }
00514           if(caps==&capabilities) {
00515             // we're using the "native" target... make things a bit quicker
00516             for(unsigned int i=1; i<size; ++i)
00517               loadSaveSensors->sensors[i-1] = atof(words[i].c_str());
00518           } else {
00519             // we're using another target, map to the host
00520             for(unsigned int i=1; i<size; ++i) {
00521               const char * name = caps->getSensorName(i-1);
00522               unsigned int off = capabilities.findSensorOffset(name);
00523               if(off==-1U) {
00524                 cerr << "*** Warning line " << linenum << ", sensor '" << name << "' from robot " << caps->getRobotName() << " cannot be mapped to host " << RobotName << endl;
00525               } else {
00526                 loadSaveSensors->sensors[off] = atof(words[i].c_str());
00527               }
00528             }
00529           }
00530         } break;
00531         case SECTION_PIDDUTIES: {
00532           if(loadSaveSensors==NULL)
00533             break;
00534           unsigned int size=words.size();
00535           if(size-1!=caps->getNumPIDJoints()) {
00536             cerr << "*** ERROR line " << linenum << ", " << caps->getRobotName() << " expected " << caps->getNumPIDJoints() << " values, got " << (size-1) << endl;
00537             return 0;
00538           }
00539           if(caps==&capabilities) {
00540             for(unsigned int i=1; i<size; ++i)
00541               loadSaveSensors->pidduties[i-1] = atof(words[i].c_str());
00542           } else {
00543             // we're using another target, map to the host
00544             unsigned int pidoff = caps->getPIDJointOffset();
00545             for(unsigned int i=1; i<size; ++i) {
00546               const char * name = caps->getOutputName(pidoff+i-1);
00547               unsigned int off = capabilities.findOutputOffset(name);
00548               if(off==-1U) {
00549                 cerr << "*** Warning line " << linenum << ", output '" << name << "' from robot " << caps->getRobotName() << " cannot be mapped to host " << RobotName << endl;
00550               } else if(off<PIDJointOffset || off>=PIDJointOffset+NumPIDJoints) {
00551                 cerr << "*** Warning line " << linenum << ", output '" << name << "' from robot " << caps->getRobotName() << " does not map to a PID joint on the local host " << RobotName << endl;
00552               } else {
00553                 loadSaveSensors->pidduties[off-PIDJointOffset] = atof(words[i].c_str());
00554               }
00555             }
00556           }
00557         } break;
00558       } // end of section name 'switch'
00559     } // end of section name found 'else'
00560   } // end of not-a-keyword 'else'
00561   return true;
00562 }
00563       
00564 //Why do i need a cast to const char**??? It doesn't work without, should be automatic... grrrr
00565 unsigned int PostureEngine::saveBuffer(char buf[], unsigned int len) const {
00566   unsigned int origlen=len;
00567   int written=snprintf(buf,len,"#POS\n");
00568   if(!checkInc(written,buf,len,"*** ERROR PostureEngine save failed on header\n")) return 0;
00569   if(saveFormatCondensed) {
00570 #ifndef PLATFORM_APERIOS
00571     written=snprintf(buf,len,"condensed %s\n",RobotName);
00572 #else
00573     written=snprintf(buf,len,"condensed %s\n",RobotName.c_str());
00574 #endif
00575     if(!checkInc(written,buf,len,"*** ERROR PostureEngine save condensed header failed\n")) return 0;
00576     if(loadSaveSensors!=NULL) {
00577       written=snprintf(buf,len,"meta-info = %u %u\n",loadSaveSensors->lastSensorUpdateTime,loadSaveSensors->frameNumber);
00578       if(!checkInc(written,buf,len,"*** ERROR PostureEngine save pidduty failed\n")) return 0;
00579     }
00580     bool weightsAllEqual=true;
00581     float weightsVal=cmds[0].weight;
00582     for(unsigned int i=1; i<NumOutputs && weightsAllEqual; i++)
00583       weightsAllEqual=(cmds[i].weight==weightsVal);
00584     if(!weightsAllEqual || weightsVal!=0) { //if they're all 0, skip outputs and weights
00585       written=snprintf(buf,len,"outputs =");
00586       if(!checkInc(written,buf,len,"*** ERROR PostureEngine save outputs header failed\n")) return 0;
00587       for(unsigned int i=0; i<NumOutputs; i++) {
00588         written=snprintf(buf,len," %g",cmds[i].value);
00589         if(!checkInc(written,buf,len,"*** ERROR PostureEngine save output failed\n")) return 0;
00590       }
00591       if(!weightsAllEqual || weightsVal!=1) { //if they're all 1, skip weights
00592         written=snprintf(buf,len,"\nweights =");
00593         if(!checkInc(written,buf,len,"*** ERROR PostureEngine save weights header failed\n")) return 0;
00594         for(unsigned int i=0; i<NumOutputs; i++) {
00595           written=snprintf(buf,len," %g",cmds[i].weight);
00596           if(!checkInc(written,buf,len,"*** ERROR PostureEngine save weight failed\n")) return 0;
00597         }
00598       }
00599       written=snprintf(buf,len,"\n");
00600       if(!checkInc(written,buf,len,"*** ERROR PostureEngine save final newline failed\n")) return 0;
00601     }
00602     if(loadSaveSensors!=NULL) {
00603       written=snprintf(buf,len,"buttons =");
00604       if(!checkInc(written,buf,len,"*** ERROR PostureEngine save buttons header failed\n")) return 0;
00605       for(unsigned int i=0; i<NumButtons; i++) {
00606         written=snprintf(buf,len," %g",loadSaveSensors->buttons[i]);
00607         if(!checkInc(written,buf,len,"*** ERROR PostureEngine save button failed\n")) return 0;
00608       }
00609       written=snprintf(buf,len,"\nsensors =");
00610       if(!checkInc(written,buf,len,"*** ERROR PostureEngine save sensors header failed\n")) return 0;
00611       for(unsigned int i=0; i<NumSensors; i++) {
00612         written=snprintf(buf,len," %g",loadSaveSensors->sensors[i]);
00613         if(!checkInc(written,buf,len,"*** ERROR PostureEngine save sensor failed\n")) return 0;
00614       }
00615       written=snprintf(buf,len,"\npidduties =");
00616       if(!checkInc(written,buf,len,"*** ERROR PostureEngine save pidduties header failed\n")) return 0;
00617       for(unsigned int i=0; i<NumPIDJoints; i++) {
00618         written=snprintf(buf,len," %g",loadSaveSensors->pidduties[i]);
00619         if(!checkInc(written,buf,len,"*** ERROR PostureEngine save pidduty failed\n")) return 0;
00620       }
00621       written=snprintf(buf,len,"\n");
00622       if(!checkInc(written,buf,len,"*** ERROR PostureEngine save final newline failed\n")) return 0;
00623     }   
00624   } else {
00625     if(loadSaveSensors!=NULL) {
00626       written=snprintf(buf,len,"<meta-info>\n  timestamp\t%u\n  framenumber\t%u\n</meta-info>\n",loadSaveSensors->lastSensorUpdateTime,loadSaveSensors->frameNumber);
00627       if(!checkInc(written,buf,len,"*** ERROR PostureEngine sensor begin save failed\n")) return 0;
00628     }
00629     for(unsigned int i=0; i<NumOutputs; i++) {
00630       if(cmds[i].weight>0) {
00631         if(cmds[i].weight==1)
00632           written=snprintf(buf,len,"%s\t% .4f\n",outputNames[i],cmds[i].value);
00633         else
00634           written=snprintf(buf,len,"%s\t% .4f\t% .4f\n",outputNames[i],cmds[i].value,cmds[i].weight);
00635         if(!checkInc(written,buf,len,"*** ERROR PostureEngine save failed\n")) return 0;
00636       }
00637     }
00638     if(loadSaveSensors!=NULL) {
00639       written=snprintf(buf,len,"<buttons>\n");
00640       if(!checkInc(written,buf,len,"*** ERROR PostureEngine sensor begin save failed\n")) return 0;
00641       for(unsigned int i=0; i<NumButtons; i++) {
00642         written=snprintf(buf,len,"  %s\t% .4f\t\n",buttonNames[i],loadSaveSensors->buttons[i]);
00643         if(!checkInc(written,buf,len,"*** ERROR PostureEngine save button failed\n")) return 0;
00644       }
00645       written=snprintf(buf,len,"</buttons><sensors>\n");
00646       if(!checkInc(written,buf,len,"*** ERROR PostureEngine sensor end save failed\n")) return 0;
00647       for(unsigned int i=0; i<NumSensors; i++) {
00648         written=snprintf(buf,len,"  %s\t% .4f\t\n",sensorNames[i],loadSaveSensors->sensors[i]);
00649         if(!checkInc(written,buf,len,"*** ERROR PostureEngine save sensor failed\n")) return 0;
00650       }
00651       written=snprintf(buf,len,"</sensors><pidduties>\n");
00652       if(!checkInc(written,buf,len,"*** ERROR PostureEngine sensor end save failed\n")) return 0;
00653       for(unsigned int i=0; i<NumPIDJoints; i++) {
00654         written=snprintf(buf,len,"  %s\t% .4f\t\n",outputNames[i],loadSaveSensors->pidduties[i]);
00655         if(!checkInc(written,buf,len,"*** ERROR PostureEngine save pidduties failed\n")) return 0;
00656       }
00657       written=snprintf(buf,len,"</pidduties>\n");
00658       if(!checkInc(written,buf,len,"*** ERROR PostureEngine sensor end save failed\n")) return 0;
00659     }
00660   }
00661   written=snprintf(buf,len,"#END\n");
00662   if(!checkInc(written,buf,len,"*** ERROR PostureEngine save failed on #END\n")) return 0;
00663   return origlen-len;
00664 }
00665         
00666 unsigned int PostureEngine::loadFile(const char filename[]) {
00667   return LoadSave::loadFile(config->motion.makePath(filename).c_str());
00668 }
00669 unsigned int PostureEngine::saveFile(const char filename[]) const {
00670   return LoadSave::saveFile(config->motion.makePath(filename).c_str());
00671 }
00672 
00673 /*
00674  NEWMAT::ReturnMatrix
00675  Kinematics::getOrientation(unsigned int j) {
00676    unsigned int c=-1U,l=-1U;
00677    if(!lookup(j,c,l)) {
00678      NEWMAT::Matrix A(4,4);
00679      A<<ROBOOP::fourbyfourident;
00680      A.Release(); return A;
00681    }
00682    update(c,l);
00683    NEWMAT::Matrix R;
00684    NEWMAT::ColumnVector p;
00685    chains[c]->kine(R,p,j);
00686    R.Release(); return R;
00687  }
00688  
00689  NEWMAT::ReturnMatrix
00690  Kinematics::getPosition(unsigned int j) {
00691    unsigned int c=-1U,l=-1U;
00692    if(!lookup(j,c,l)) {
00693      NEWMAT::Matrix A(4,4);
00694      A<<ROBOOP::fourbyfourident;
00695      A.Release(); return A;
00696    }
00697    update(c,l);
00698    NEWMAT::Matrix R;
00699    NEWMAT::ColumnVector p;
00700    chains[c]->kine(R,p,j);
00701    p.Release(); return p;
00702  }
00703  */
00704         
00705 bool
00706 PostureEngine::solveLinkPosition(const NEWMAT::ColumnVector& Pobj, unsigned int j, const NEWMAT::ColumnVector& Plink) {
00707   unsigned int c=-1U,l=-1U;
00708   if(!lookup(j,c,l))
00709     return false;
00710   update(c,l);
00711   bool conv=false;
00712   NEWMAT::ColumnVector q=chains[c]->inv_kin_pos(Pobj,0,l,Plink,conv);
00713   for(unsigned int i=1; i<=l && i<=chainMaps[c].size(); i++)
00714     if(chainMaps[c][i]<NumOutputs)
00715       setOutputCmd(chainMaps[c][i],chains[c]->get_q(i));
00716   return conv;
00717 }
00718 
00719 bool
00720 PostureEngine::solveLinkVector(const NEWMAT::ColumnVector& Pobj, unsigned int j, const NEWMAT::ColumnVector& Plink) {
00721   solveLinkPosition(Pobj,j,Plink);
00722   //This method is an approximation, not entirely precise or fast as it could be
00723   //Something to work on some more down the road! :)
00724   //(this method is shared with HeadPointerMC::lookAtPoint(x,y,z))
00725   NEWMAT::ColumnVector poE=baseToLink(j)*Pobj;
00726   if(poE.nrows()>3 && poE(4)!=0) {
00727     poE/=poE(4);
00728   }
00729   poE=poE.SubMatrix(1,3,1,1);
00730   NEWMAT::ColumnVector plE=Plink.SubMatrix(1,3,1,1);
00731   if(Plink.nrows()>3 && Plink(4)!=0)
00732     plE/=Plink(4);
00733   float plE2=plE.SumSquare();
00734   float plE_len=sqrt(plE2);
00735   float obj_comp_link=NEWMAT::DotProduct(plE,poE)/plE_len;
00736   if(obj_comp_link<plE_len)
00737     obj_comp_link=obj_comp_link*.975; //.975 is a bit of fudge - accounts for joints moving Plink when adjusting
00738   else
00739     obj_comp_link=obj_comp_link/.975; //.975 is a bit of fudge - accounts for joints moving Plink when adjusting
00740   NEWMAT::ColumnVector obj_proj_link(4);
00741   obj_proj_link.SubMatrix(1,3,1,1)=obj_comp_link*plE/plE_len;
00742   obj_proj_link(4)=1;
00743   return solveLinkPosition(Pobj,j,obj_proj_link);
00744 }
00745 
00746 void
00747 PostureEngine::update(unsigned int c, unsigned int l) {
00748   for(unsigned int j=1; j<=l; j++) {
00749     unsigned int tkout=chainMaps[c][j];
00750     if(tkout<NumOutputs)
00751       chains[c]->set_q(getOutputCmd(tkout).value,j);
00752   }
00753 }
00754 
00755 /*! @file
00756  * @brief Implements PostureEngine, a base class for managing the values and weights of all the outputs
00757  * @author ejt (Creator)
00758  *
00759  * $Author: ejt $
00760  * $Name: tekkotsu-4_0 $
00761  * $Revision: 1.41 $
00762  * $State: Exp $
00763  * $Date: 2007/11/11 23:57:23 $
00764  */
00765 

Tekkotsu v4.0
Generated Thu Nov 22 00:54:55 2007 by Doxygen 1.5.4