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
00050
00051
00052
00053
00054
00055
00056
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
00074
00075
00076
00077
00078
00079
00080
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
00144
00145
00146
00147 return 0;
00148 }
00149 char formatstring[64];
00150 snprintf(formatstring,64,"%%%dc %%f %%f%%n",outputNameLen);
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
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') {
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
00199
00200
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) {
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)
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
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
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
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
00305
00306
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;
00320 else
00321 obj_comp_link=obj_comp_link/.975;
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
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347