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 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
00194
00195
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) {
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)
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
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
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
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
00300
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;
00314 else
00315 obj_comp_link=obj_comp_link/.975;
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
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341