Tekkotsu Homepage

Demos

Overview

Downloads

Dev. Resources

Reference

Credits

PostureEngine.cc Go to the documentation of this file. #include "PostureEngine.h" #include "Shared/WorldState.h" #include "Motion/roboop/robot.h" #include "Shared/Config.h" #include <stdio.h> #include <iostream> PostureEngine::~PostureEngine() {} void PostureEngine::takeSnapshot() { takeSnapshot(*WorldState::getCurrent()); } void PostureEngine::takeSnapshot(const WorldState& st) { for(unsigned int i=0; i<NumOutputs; i++) cmds[i].value=st.outputs[i]; } void PostureEngine::setWeights(float w, unsigned int lowjoint, unsigned int highjoint) { for(unsigned int i=lowjoint; i<highjoint; i++) cmds[i].weight=w; } void PostureEngine::clear() { for(unsigned int i=0; i<NumOutputs; i++) cmds[i].unset(); } PostureEngine& PostureEngine::setOverlay(const PostureEngine& pe) { for(unsigned int i=0; i<NumOutputs; i++) if(pe.cmds[i].weight>0) cmds[i]=pe.cmds[i]; return *this; } PostureEngine PostureEngine::createOverlay(const PostureEngine& pe) const { PostureEngine tmp(*this); return tmp.setOverlay(pe); } PostureEngine& PostureEngine::setUnderlay(const PostureEngine& pe) { for(unsigned int i=0; i<NumOutputs; i++) if(cmds[i].weight<=0) cmds[i]=pe.cmds[i]; return *this; } PostureEngine PostureEngine::createUnderlay(const PostureEngine& pe) const { PostureEngine tmp(*this); return tmp.setUnderlay(pe); } /*! joints being averaged with unused joints have their weights averaged, but not their values (so an output can crossfade properly)\n * @param pe the other PostureEngine * @param w amount to weight towards @a pe * - if @a w < .001, nothing is done * - if @a w > .999, a straight copy of @a pe occurs (sets joints to unused properly at end of fade) * - .001 and .999 is used instead of 0 and 1 to allow for slight addition errors in a loop (if * using repeated additions of a delta value instead of repeated divisions) * @return @c *this, stores results into this */ PostureEngine& PostureEngine::setAverage(const PostureEngine& pe, float w) { if(w<0.001) return *this; if(w>0.999) return (*this=pe); float wp=1-w; for(unsigned int i=0; i<NumOutputs; i++) if(cmds[i].weight>0) { if(pe.cmds[i].weight>0) cmds[i].set(cmds[i].value*wp+pe.cmds[i].value*w,cmds[i].weight*wp+pe.cmds[i].weight*w); else cmds[i].weight*=wp; } else cmds[i].set(pe.cmds[i].value,pe.cmds[i].weight*w); return *this; } /*! joints being averaged with weight<=0 have their weights averaged, but not their values (so an output can crossfade properly)\n * @param pe the other PostureEngine * @param w amount to weight towards @a pe * - if @a w < .001, nothing is done * - if @a w > .999, a straight copy of @a pe occurs (sets joints to unused properly at end of fade) * - .001 and .999 is used instead of 0 and 1 to allow for slight addition errors in a loop (if * using repeated additions of a delta value instead of repeated divisions) * @return a new posture containing the results */ PostureEngine PostureEngine::createAverage(const PostureEngine& pe, float w) const { PostureEngine tmp(*this); return tmp.setAverage(pe,w); } PostureEngine& PostureEngine::setCombine(const PostureEngine& pe) { for(unsigned int i=0; i<NumOutputs; i++) { float total=cmds[i].weight+pe.cmds[i].weight; cmds[i].set((cmds[i].value*cmds[i].weight+pe.cmds[i].value*pe.cmds[i].weight)/total,total); } return *this; } PostureEngine PostureEngine::createCombine(const PostureEngine& pe) const { PostureEngine tmp(*this); return tmp.setCombine(pe); } float PostureEngine::diff(const PostureEngine& pe) const { float ans=0; for(unsigned int i=0; i<NumOutputs; i++) if(cmds[i].weight>0 && pe.cmds[i].weight>0) { float dif=cmds[i].value-pe.cmds[i].value; ans+=dif*dif; } return ans; } float PostureEngine::avgdiff(const PostureEngine& pe) const { float ans=0; unsigned int cnt=0; for(unsigned int i=0; i<NumOutputs; i++) if(cmds[i].weight>0 && pe.cmds[i].weight>0) { float dif=cmds[i].value-pe.cmds[i].value; ans+=dif*dif; cnt++; } return ans/cnt; } float PostureEngine::maxdiff(const PostureEngine& pe) const { float max=0; for(unsigned int i=0; i<NumOutputs; i++) if(cmds[i].weight>0 && pe.cmds[i].weight>0) { float dif=cmds[i].value-pe.cmds[i].value; if(dif>max) max=dif; } return max; } void PostureEngine::setSaveFormat(bool condensed, WorldState* ws) { saveFormatCondensed=condensed; loadSaveSensors=ws; } unsigned int PostureEngine::getBinSize() const { unsigned int written=11; //accounts for initial #POS\n and final #END\n, plus 1 const unsigned int len=0; char buf[len]; if(saveFormatCondensed) { written+=snprintf(buf,len,"condensed %s\n",RobotName); if(loadSaveSensors!=NULL) written+=snprintf(buf,len,"meta-info = %u %u\n",loadSaveSensors->lastSensorUpdateTime,loadSaveSensors->frameNumber); bool weightsAllEqual=true; float weightsVal=cmds[0].weight; for(unsigned int i=1; i<NumOutputs && weightsAllEqual; i++) weightsAllEqual=(cmds[i].weight==weightsVal); if(!weightsAllEqual || weightsVal!=0) { //if they're all 0, skip outputs and weights written+=snprintf(buf,len,"outputs ="); for(unsigned int i=0; i<NumOutputs; i++) { written+=snprintf(buf,len," %g",cmds[i].value); } if(!weightsAllEqual || weightsVal!=1) { //if they're all 1, skip weights written+=snprintf(buf,len,"\nweights ="); for(unsigned int i=0; i<NumOutputs; i++) { written+=snprintf(buf,len," %g",cmds[i].weight); } } written+=snprintf(buf,len,"\n"); } if(loadSaveSensors!=NULL) { written+=snprintf(buf,len,"buttons ="); for(unsigned int i=0; i<NumButtons; i++) { written+=snprintf(buf,len," %g",loadSaveSensors->buttons[i]); } written+=snprintf(buf,len,"\nsensors ="); for(unsigned int i=0; i<NumSensors; i++) { written+=snprintf(buf,len," %g",loadSaveSensors->sensors[i]); } written+=snprintf(buf,len,"\npidduties ="); for(unsigned int i=0; i<NumPIDJoints; i++) { written+=snprintf(buf,len," %g",loadSaveSensors->pidduties[i]); } written+=snprintf(buf,len,"\n"); } } else { if(loadSaveSensors!=NULL) written+=snprintf(buf,len,"<meta-info>\n timestamp\t%u\n framenumber\t%u\n</meta-info>\n",loadSaveSensors->lastSensorUpdateTime,loadSaveSensors->frameNumber); for(unsigned int i=0; i<NumOutputs; i++) if(cmds[i].weight>0) { written+=snprintf(buf,len,"%s\t% .4f\t% .4f\n",outputNames[i],cmds[i].value,cmds[i].weight); } if(loadSaveSensors!=NULL) { written+=snprintf(buf,len,"<buttons>\n"); for(unsigned int i=0; i<NumButtons; i++) { written+=snprintf(buf,len," %s\t% .4f\t\n",buttonNames[i],loadSaveSensors->buttons[i]); } written+=snprintf(buf,len,"</buttons><sensors>\n"); for(unsigned int i=0; i<NumSensors; i++) { written+=snprintf(buf,len," %s\t% .4f\t\n",sensorNames[i],loadSaveSensors->sensors[i]); } written+=snprintf(buf,len,"</sensors><pidduties>\n"); for(unsigned int i=0; i<NumPIDJoints; i++) { written+=snprintf(buf,len," duty-%s\t% .4f\t\n",outputNames[i],loadSaveSensors->pidduties[i]); } written+=snprintf(buf,len,"</pidduties>\n"); } } return written; } unsigned int PostureEngine::loadBuffer(const char buf[], unsigned int len) { unsigned int origlen=len; clear(); if(strncmp("#POS",buf,4)!=0) { // we don't want to display an error here because we may be only testing file type, // so it's up to the caller to decide if it's necessarily an error if the file isn't // a posture //std::cout << "ERROR PostureEngine load corrupted - expected #POS header" << std::endl; return 0; } saveFormatCondensed=false; char formatstring[64]; snprintf(formatstring,64,"%%%dc %%g %%g%%n",outputNameLen); //std::cout << "Format: " << formatstring << std::endl; unsigned int idx=0; unsigned int linenum=1; enum section_t { SECTION_METAINFO, SECTION_OUTPUTS, SECTION_BUTTONS, SECTION_SENSORS, SECTION_PIDDUTIES } curSection=SECTION_OUTPUTS; char jname[outputNameLen+1]; jname[outputNameLen]='\0'; while(len<=origlen && len>0) { float fval, fwht=1; int written; // printf("%d %.9s\n",linenum+1,buf); if(buf[0]=='\r') { buf++; len--; if(buf[0]=='\n') { buf++; len--; } linenum++; continue; } if(buf[0]=='\n') { buf++; len--; linenum++; continue; } if(buf[0]=='#') { if(strncmp("#END\n",buf,5)==0 || strncmp("#END\r",buf,5)==0) { return origlen-len+5; } else if(strncmp("#END\r\n",buf,6)==0) { return origlen-len+6; } else { while(len>0 && *buf!='\n' && *buf!='\r') {len--;buf++;} if(*buf=='\n') { //in case of \r\n buf++; len--; } linenum++; continue; } } if(isspace(buf[0])) { buf++; len--; continue; } written=-1; if(strncmp(buf,"condensed ",strlen("condensed"))==0) { char model[64]; written=0; sscanf(buf,"condensed %64s%n",model,&written); if(!checkInc(written,buf,len,"*** ERROR PostureEngine load corrupted - line %d\n",linenum)) return 0; if(strncmp(model,RobotName,64)!=0) { printf("*** ERROR line %d, specified model (%.64s) for condensed mode does not match current robot (%s)\n",linenum,model,RobotName); return 0; } saveFormatCondensed=true; for(unsigned int i=0; i<NumOutputs; i++) cmds[i].unset(); /*} else if(strncmp(buf,"verbose",strlen("verbose"))==0) { written=strlen("verbose"); if(!checkInc(written,buf,len,"*** ERROR PostureEngine load corrupted - line %d\n",linenum)) return 0; saveFormatCondensed=false;*/ } else { if(saveFormatCondensed) { const unsigned int sectionLen=64; char section[sectionLen]; written=0; sscanf(buf,"%64s = %n",section,&written); if(!checkInc(written,buf,len,"*** ERROR PostureEngine load corrupted - line %d\n",linenum)) return 0; if(strncmp(section,"outputs",sectionLen)==0) { for(unsigned int i=0; i<NumOutputs; i++) { float val; written=0; sscanf(buf,"%g%n",&val,&written); if(!checkInc(written,buf,len,"*** ERROR PostureEngine load corrupted - line %d\n",linenum)) return 0; cmds[i].set(val); } } else if(strncmp(section,"weights",sectionLen)==0) { for(unsigned int i=0; i<NumOutputs; i++) { written=0; sscanf(buf,"%g%n",&cmds[i].weight,&written); if(!checkInc(written,buf,len,"*** ERROR PostureEngine load corrupted - line %d\n",linenum)) return 0; } } else if(strncmp(section,"buttons",sectionLen)==0) { for(unsigned int i=0; i<NumButtons; i++) { written=0; sscanf(buf,"%g%n",&loadSaveSensors->buttons[i],&written); if(!checkInc(written,buf,len,"*** ERROR PostureEngine load corrupted - line %d\n",linenum)) return 0; } } else if(strncmp(section,"sensors",sectionLen)==0) { for(unsigned int i=0; i<NumSensors; i++) { written=0; sscanf(buf,"%g%n",&loadSaveSensors->sensors[i],&written); if(!checkInc(written,buf,len,"*** ERROR PostureEngine load corrupted - line %d\n",linenum)) return 0; } } else if(strncmp(section,"pidduties",sectionLen)==0) { for(unsigned int i=0; i<NumPIDJoints; i++) { written=0; sscanf(buf,"%g%n",&loadSaveSensors->pidduties[i],&written); if(!checkInc(written,buf,len,"*** ERROR PostureEngine load corrupted - line %d\n",linenum)) return 0; } } else if(strncmp(section,"meta-info",sectionLen)==0) { written=0; sscanf(buf,"%u %u%n",&loadSaveSensors->lastSensorUpdateTime,&loadSaveSensors->frameNumber,&written); if(!checkInc(written,buf,len,"*** ERROR PostureEngine load corrupted - line %d\n",linenum)) return 0; } else { printf("*** Warning: line %d, unknown section '%.64s', skipping.\n",linenum,section); } } else { if(strncmp(buf,"<meta-info>",strlen("<meta-info>"))==0) { written=0; sscanf(buf,"<meta-info>%n",&written); if(!checkInc(written,buf,len,"*** ERROR PostureEngine load corrupted - line %d\n",linenum)) return 0; if(curSection!=SECTION_OUTPUTS) printf("*** WARNING PostureEngine encountered nested <meta-info> - line %d\n",linenum); curSection=SECTION_METAINFO; idx=0; //reset idx any time we switch sections, avoid illegal idx values continue; } else if(strncmp(buf,"</meta-info>",strlen("</meta-info>"))==0) { written=0; sscanf(buf,"</meta-info>%n",&written); if(!checkInc(written,buf,len,"*** ERROR PostureEngine load corrupted - line %d\n",linenum)) return 0; if(curSection!=SECTION_METAINFO) printf("*** WARNING PostureEngine encountered </meta-info> when not in sensor section - line %d\n",linenum); curSection=SECTION_OUTPUTS; idx=0; //reset idx any time we switch sections, avoid illegal idx values continue; } else if(strncmp(buf,"<buttons>",strlen("<buttons>"))==0) { written=0; sscanf(buf,"<buttons>%n",&written); if(!checkInc(written,buf,len,"*** ERROR PostureEngine load corrupted - line %d\n",linenum)) return 0; if(curSection!=SECTION_OUTPUTS) printf("*** WARNING PostureEngine encountered nested <buttons> - line %d\n",linenum); curSection=SECTION_BUTTONS; idx=0; //reset idx any time we switch sections, avoid illegal idx values continue; } else if(strncmp(buf,"</buttons>",strlen("</buttons>"))==0) { written=0; sscanf(buf,"</buttons>%n",&written); if(!checkInc(written,buf,len,"*** ERROR PostureEngine load corrupted - line %d\n",linenum)) return 0; if(curSection!=SECTION_BUTTONS) printf("*** WARNING PostureEngine encountered </buttons> when not in sensor section - line %d\n",linenum); curSection=SECTION_OUTPUTS; idx=0; //reset idx any time we switch sections, avoid illegal idx values continue; } else if(strncmp(buf,"<sensors>",strlen("<sensors>"))==0) { written=0; sscanf(buf,"<sensors>%n",&written); if(!checkInc(written,buf,len,"*** ERROR PostureEngine load corrupted - line %d\n",linenum)) return 0; if(curSection!=SECTION_OUTPUTS) printf("*** WARNING PostureEngine encountered nested <sensors> - line %d\n",linenum); curSection=SECTION_SENSORS; idx=0; //reset idx any time we switch sections, avoid illegal idx values continue; } else if(strncmp(buf,"</sensors>",strlen("</sensors>"))==0) { written=0; sscanf(buf,"</sensors>%n",&written); if(!checkInc(written,buf,len,"*** ERROR PostureEngine load corrupted - line %d\n",linenum)) return 0; if(curSection!=SECTION_SENSORS) printf("*** WARNING PostureEngine encountered </sensors> when not in sensor section - line %d\n",linenum); curSection=SECTION_OUTPUTS; idx=0; //reset idx any time we switch sections, avoid illegal idx values continue; } else if(strncmp(buf,"<pidduties>",strlen("<pidduties>"))==0) { written=0; sscanf(buf,"<pidduties>%n",&written); if(!checkInc(written,buf,len,"*** ERROR PostureEngine load corrupted - line %d\n",linenum)) return 0; if(curSection!=SECTION_OUTPUTS) printf("*** WARNING PostureEngine encountered nested <pidduties> - line %d\n",linenum); curSection=SECTION_PIDDUTIES; idx=0; //reset idx any time we switch sections, avoid illegal idx values continue; } else if(strncmp(buf,"</pidduties>",strlen("</pidduties>"))==0) { written=0; sscanf(buf,"</pidduties>%n",&written); if(!checkInc(written,buf,len,"*** ERROR PostureEngine load corrupted - line %d\n",linenum)) return 0; if(curSection!=SECTION_PIDDUTIES) printf("*** WARNING PostureEngine encountered </pidduties> when not in sensor section - line %d\n",linenum); curSection=SECTION_OUTPUTS; idx=0; //reset idx any time we switch sections, avoid illegal idx values continue; } else { if(curSection==SECTION_METAINFO) { const unsigned int nameLen=64; char name[nameLen]; unsigned int ival; written=0; sscanf(buf,"%64s %u%n",name,&ival,&written); if(!checkInc(written,buf,len,"*** ERROR PostureEngine load corrupted - line %d\n",linenum)) return 0; if(strncmp(name,"timestamp",nameLen)==0) { loadSaveSensors->lastSensorUpdateTime=ival; } else if(strncmp(name,"framenumber",nameLen)==0) { loadSaveSensors->frameNumber=ival; } else { std::cout << "*** WARNING '" << name << "' is not a valid meta-info on this model. (ignoring line " << linenum << ")" << std::endl; } } else if(curSection==SECTION_BUTTONS) { const unsigned int nameLen=64; char name[nameLen]; written=0; sscanf(buf,"%64s %g%n",name,&fval,&written); if(!checkInc(written,buf,len,"*** ERROR PostureEngine load corrupted - line %d\n",linenum)) return 0; unsigned int startidx=idx; for(;idx<NumButtons;idx++) if(strncmp(name,buttonNames[idx],nameLen)==0) { loadSaveSensors->buttons[idx]=fval; break; } if(idx==NumButtons) { //not found following startidx, look from beginning for(idx=0;idx<startidx;idx++) if(strncmp(name,buttonNames[idx],nameLen)==0) { loadSaveSensors->buttons[idx]=fval; break; } if(idx==startidx && strcmp(name,buttonNames[idx])!=0) //not found at all std::cout << "*** WARNING '" << name << "' is not a valid button on this model. (ignoring line " << linenum << ")" << std::endl; } } else if(curSection==SECTION_SENSORS) { const unsigned int nameLen=64; char name[nameLen]; written=0; sscanf(buf,"%64s %g%n",name,&fval,&written); if(!checkInc(written,buf,len,"*** ERROR PostureEngine load corrupted - line %d\n",linenum)) return 0; unsigned int startidx=idx; for(;idx<NumSensors;idx++) if(strncmp(name,sensorNames[idx],nameLen)==0) { loadSaveSensors->sensors[idx]=fval; break; } if(idx==NumSensors) { //not found following startidx, look from beginning for(idx=0;idx<startidx;idx++) if(strncmp(name,sensorNames[idx],nameLen)==0) { loadSaveSensors->sensors[idx]=fval; break; } if(idx==startidx && strcmp(name,sensorNames[idx])!=0) //not found at all std::cout << "*** WARNING '" << name << "' is not a valid sensor on this model. (ignoring line " << linenum << ")" << std::endl; } } else if(curSection==SECTION_PIDDUTIES) { const unsigned int nameLen=64; char name[nameLen]; written=0; sscanf(buf,"%64s %g%n",name,&fval,&written); if(!checkInc(written,buf,len,"*** ERROR PostureEngine load corrupted - line %d\n",linenum)) return 0; unsigned int startidx=idx; for(;idx<NumPIDJoints;idx++) if(strncmp(name,outputNames[idx],nameLen)==0) { loadSaveSensors->pidduties[PIDJointOffset+idx]=fval; break; } if(idx==NumPIDJoints) { //not found following startidx, look from beginning for(idx=0;idx<startidx;idx++) if(strncmp(name,outputNames[idx],nameLen)==0) { loadSaveSensors->pidduties[PIDJointOffset+idx]=fval; break; } if(idx==startidx && strcmp(name,outputNames[idx])!=0) //not found at all std::cout << "*** WARNING '" << name << "' is not a valid PID joint on this model. (ignoring line " << linenum << ")" << std::endl; } } else { //general joint specification jname[0]='\0'; written=0; sscanf(buf,formatstring,jname,&fval,&fwht,&written); if(!checkInc(written,buf,len,"*** ERROR PostureEngine load corrupted - line %d\n",linenum)) return 0; //std::cout << '"' << jname << "\"\t" << (float)fval << '\t' << (float)fwht << std::endl; // we continue the search in order from where we left off - these are often // going to go in order, so might as well save a little time unsigned int startidx=idx; for(;idx<NumOutputs;idx++) if(strncmp(jname,outputNames[idx],outputNameLen+1)==0) { cmds[idx].set(fval,fwht); break; } if(idx==NumOutputs) { //not found following startidx, look from beginning for(idx=0;idx<startidx;idx++) if(strncmp(jname,outputNames[idx],outputNameLen+1)==0) { cmds[idx].set(fval,fwht); break; } if(idx==startidx && strcmp(jname,outputNames[idx])!=0) {//not found at all, check if this is a symmetric case unsigned int lidx=NumOutputs, ridx=NumOutputs; char tname[outputNameLen+1]; strncpy(tname+1,jname,outputNameLen); tname[0]='L'; for(idx=0;idx<NumOutputs;idx++) { if(strncmp(tname,outputNames[idx],outputNameLen+1)==0) { lidx=idx; break; } } tname[0]='R'; for(idx=0;idx<NumOutputs;idx++) { if(strncmp(tname,outputNames[idx],outputNameLen+1)==0) { ridx=idx; break; } } if(lidx!=NumOutputs && ridx!=NumOutputs) { cmds[lidx].set(fval,fwht); cmds[ridx].set(fval,fwht); } else { idx=startidx; std::cout << "*** WARNING '" << jname << "' is not a valid joint on this model. (ignoring line " << linenum << ")" << std::endl; } } } } } } } while(*buf!='\n' && *buf!='\r') { buf++; len--; } if(buf[0]=='\r' && buf[1]=='\n') { buf+=2; len-=2; } else { buf++; len--; } linenum++; } std::cout << "*** WARNING PostureEngine load missing #END" << std::endl; return origlen-len; } //Why do i need a cast to const char**??? It doesn't work without, should be automatic... grrrr unsigned int PostureEngine::saveBuffer(char buf[], unsigned int len) const { unsigned int origlen=len; int written=snprintf(buf,len,"#POS\n"); if(!checkInc(written,buf,len,"*** ERROR PostureEngine save failed on header\n")) return 0; if(saveFormatCondensed) { written=snprintf(buf,len,"condensed %s\n",RobotName); if(!checkInc(written,buf,len,"*** ERROR PostureEngine save condensed header failed\n")) return 0; if(loadSaveSensors!=NULL) { written=snprintf(buf,len,"meta-info = %u %u\n",loadSaveSensors->lastSensorUpdateTime,loadSaveSensors->frameNumber); if(!checkInc(written,buf,len,"*** ERROR PostureEngine save pidduty failed\n")) return 0; } bool weightsAllEqual=true; float weightsVal=cmds[0].weight; for(unsigned int i=1; i<NumOutputs && weightsAllEqual; i++) weightsAllEqual=(cmds[i].weight==weightsVal); if(!weightsAllEqual || weightsVal!=0) { //if they're all 0, skip outputs and weights written=snprintf(buf,len,"outputs ="); if(!checkInc(written,buf,len,"*** ERROR PostureEngine save outputs header failed\n")) return 0; for(unsigned int i=0; i<NumOutputs; i++) { written=snprintf(buf,len," %g",cmds[i].value); if(!checkInc(written,buf,len,"*** ERROR PostureEngine save output failed\n")) return 0; } if(!weightsAllEqual || weightsVal!=1) { //if they're all 1, skip weights written=snprintf(buf,len,"\nweights ="); if(!checkInc(written,buf,len,"*** ERROR PostureEngine save weights header failed\n")) return 0; for(unsigned int i=0; i<NumOutputs; i++) { written=snprintf(buf,len," %g",cmds[i].weight); if(!checkInc(written,buf,len,"*** ERROR PostureEngine save weight failed\n")) return 0; } } written=snprintf(buf,len,"\n"); if(!checkInc(written,buf,len,"*** ERROR PostureEngine save final newline failed\n")) return 0; } if(loadSaveSensors!=NULL) { written=snprintf(buf,len,"buttons ="); if(!checkInc(written,buf,len,"*** ERROR PostureEngine save buttons header failed\n")) return 0; for(unsigned int i=0; i<NumButtons; i++) { written=snprintf(buf,len," %g",loadSaveSensors->buttons[i]); if(!checkInc(written,buf,len,"*** ERROR PostureEngine save button failed\n")) return 0; } written=snprintf(buf,len,"\nsensors ="); if(!checkInc(written,buf,len,"*** ERROR PostureEngine save sensors header failed\n")) return 0; for(unsigned int i=0; i<NumSensors; i++) { written=snprintf(buf,len," %g",loadSaveSensors->sensors[i]); if(!checkInc(written,buf,len,"*** ERROR PostureEngine save sensor failed\n")) return 0; } written=snprintf(buf,len,"\npidduties ="); if(!checkInc(written,buf,len,"*** ERROR PostureEngine save pidduties header failed\n")) return 0; for(unsigned int i=0; i<NumPIDJoints; i++) { written=snprintf(buf,len," %g",loadSaveSensors->pidduties[i]); if(!checkInc(written,buf,len,"*** ERROR PostureEngine save pidduty failed\n")) return 0; } written=snprintf(buf,len,"\n"); if(!checkInc(written,buf,len,"*** ERROR PostureEngine save final newline failed\n")) return 0; } } else { if(loadSaveSensors!=NULL) { written=snprintf(buf,len,"<meta-info>\n timestamp\t%u\n framenumber\t%u\n</meta-info>\n",loadSaveSensors->lastSensorUpdateTime,loadSaveSensors->frameNumber); if(!checkInc(written,buf,len,"*** ERROR PostureEngine sensor begin save failed\n")) return 0; } for(unsigned int i=0; i<NumOutputs; i++) if(cmds[i].weight>0) { written=snprintf(buf,len,"%s\t% .4f\t% .4f\n",outputNames[i],cmds[i].value,cmds[i].weight); if(!checkInc(written,buf,len,"*** ERROR PostureEngine save failed\n")) return 0; } if(loadSaveSensors!=NULL) { written=snprintf(buf,len,"<buttons>\n"); if(!checkInc(written,buf,len,"*** ERROR PostureEngine sensor begin save failed\n")) return 0; for(unsigned int i=0; i<NumButtons; i++) { written=snprintf(buf,len," %s\t% .4f\t\n",buttonNames[i],loadSaveSensors->buttons[i]); if(!checkInc(written,buf,len,"*** ERROR PostureEngine save button failed\n")) return 0; } written=snprintf(buf,len,"</buttons><sensors>\n"); if(!checkInc(written,buf,len,"*** ERROR PostureEngine sensor end save failed\n")) return 0; for(unsigned int i=0; i<NumSensors; i++) { written=snprintf(buf,len," %s\t% .4f\t\n",sensorNames[i],loadSaveSensors->sensors[i]); if(!checkInc(written,buf,len,"*** ERROR PostureEngine save sensor failed\n")) return 0; } written=snprintf(buf,len,"</sensors><pidduties>\n"); if(!checkInc(written,buf,len,"*** ERROR PostureEngine sensor end save failed\n")) return 0; for(unsigned int i=0; i<NumPIDJoints; i++) { written=snprintf(buf,len," %s\t% .4f\t\n",outputNames[i],loadSaveSensors->pidduties[i]); if(!checkInc(written,buf,len,"*** ERROR PostureEngine save pidduties failed\n")) return 0; } written=snprintf(buf,len,"</pidduties>\n"); if(!checkInc(written,buf,len,"*** ERROR PostureEngine sensor end save failed\n")) return 0; } } written=snprintf(buf,len,"#END\n"); if(!checkInc(written,buf,len,"*** ERROR PostureEngine save failed on #END\n")) return 0; return origlen-len; } unsigned int PostureEngine::loadFile(const char filename[]) { return LoadSave::loadFile(config->motion.makePath(filename).c_str()); } unsigned int PostureEngine::saveFile(const char filename[]) const { return LoadSave::saveFile(config->motion.makePath(filename).c_str()); } /* NEWMAT::ReturnMatrix Kinematics::getOrientation(unsigned int j) { unsigned int c=-1U,l=-1U; if(!lookup(j,c,l)) { NEWMAT::Matrix A(4,4); A<<ROBOOP::fourbyfourident; A.Release(); return A; } update(c,l); NEWMAT::Matrix R; NEWMAT::ColumnVector p; chains[c]->kine(R,p,j); R.Release(); return R; } NEWMAT::ReturnMatrix Kinematics::getPosition(unsigned int j) { unsigned int c=-1U,l=-1U; if(!lookup(j,c,l)) { NEWMAT::Matrix A(4,4); A<<ROBOOP::fourbyfourident; A.Release(); return A; } update(c,l); NEWMAT::Matrix R; NEWMAT::ColumnVector p; chains[c]->kine(R,p,j); p.Release(); return p; } */ bool PostureEngine::solveLinkPosition(const NEWMAT::ColumnVector& Pobj, unsigned int j, const NEWMAT::ColumnVector& Plink) { unsigned int c=-1U,l=-1U; if(!lookup(j,c,l)) return false; update(c,l); bool conv=false; NEWMAT::ColumnVector q=chains[c]->inv_kin_pos(Pobj,0,l,Plink,conv); for(unsigned int i=1; i<=l && i<=chainMaps[c].size(); i++) if(chainMaps[c][i]<NumOutputs) setOutputCmd(chainMaps[c][i],chains[c]->get_q(i)); return conv; } bool PostureEngine::solveLinkVector(const NEWMAT::ColumnVector& Pobj, unsigned int j, const NEWMAT::ColumnVector& Plink) { solveLinkPosition(Pobj,j,Plink); //This method is an approximation, not entirely precise or fast as it could be //Something to work on some more down the road! :) //(this method is shared with HeadPointerMC::lookAtPoint(x,y,z)) NEWMAT::ColumnVector poE=baseToLink(j)*Pobj; if(poE.nrows()>3 && poE(4)!=0) { poE/=poE(4); } poE=poE.SubMatrix(1,3,1,1); NEWMAT::ColumnVector plE=Plink.SubMatrix(1,3,1,1); if(Plink.nrows()>3 && Plink(4)!=0) plE/=Plink(4); float plE2=plE.SumSquare(); float plE_len=sqrt(plE2); float obj_comp_link=NEWMAT::DotProduct(plE,poE)/plE_len; if(obj_comp_link<plE_len) obj_comp_link=obj_comp_link*.975; //.975 is a bit of fudge - accounts for joints moving Plink when adjusting else obj_comp_link=obj_comp_link/.975; //.975 is a bit of fudge - accounts for joints moving Plink when adjusting NEWMAT::ColumnVector obj_proj_link(4); obj_proj_link.SubMatrix(1,3,1,1)=obj_comp_link*plE/plE_len; obj_proj_link(4)=1; return solveLinkPosition(Pobj,j,obj_proj_link); } void PostureEngine::update(unsigned int c, unsigned int l) { for(unsigned int j=1; j<=l; j++) { unsigned int tkout=chainMaps[c][j]; if(tkout<NumOutputs) chains[c]->set_q(getOutputCmd(tkout).value,j); } } /*! @file * @brief Implements PostureEngine, a base class for managing the values and weights of all the outputs * @author ejt (Creator) * * $Author: ejt $ * $Name: tekkotsu-3_0 $ * $Revision: 1.32 $ * $State: Exp $ * $Date: 2006/09/09 04:32:55 $ */

Tekkotsu v3.0

Generated Wed Oct 4 00:03:45 2006 by Doxygen 1.4.7