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
00045
00046
00047
00048
00049
00050
00051
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
00069
00070
00071
00072
00073
00074
00075
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
00139
00140
00141
00142 return 0;
00143 }
00144 char formatstring[64];
00145 snprintf(formatstring,64,"%%%dc %%f %%f%%n",outputNameLen);
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
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') {
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
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
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
00249
00250
00251
00252
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 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
00297
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;
00311 else
00312 obj_comp_link=obj_comp_link/.975;
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
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338