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
00055
00056
00057
00058
00059
00060
00061
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
00079
00080
00081
00082
00083
00084
00085
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;
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) {
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) {
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
00222
00223
00224
00225 return 0;
00226 }
00227 saveFormatCondensed=false;
00228 Capabilities* caps = &capabilities;
00229 unsigned int linenum=0;
00230 bool filtered=false;
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
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)
00252 break;
00253 if(isreturn && buf[0]=='\n') {
00254 ++buf;
00255 --len;
00256 }
00257 break;
00258 }
00259 }
00260
00261
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
00270
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
00285
00286
00287
00288
00289
00290 if(!loadLine(linenum,sectionMap,words,curSection,caps,filtered))
00291 return 0;
00292
00293 }
00294
00295
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
00355
00356 } else {
00357
00358 map<string,section_t>::const_iterator it = sectionMap.find(words[0]);
00359 if(it==sectionMap.end()) {
00360
00361 if(words[0]=="weights") {
00362 if(caps==&capabilities) {
00363
00364 for(unsigned int i=1; i<words.size(); ++i)
00365 cmds[i-1].weight = atof(words[i].c_str());
00366 } else {
00367
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
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
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 }
00445 }
00446
00447 } else {
00448
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
00466 for(unsigned int i=1; i<size; ++i)
00467 cmds[i-1].set(atof(words[i].c_str()));
00468 } else {
00469
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
00491 for(unsigned int i=1; i<size; ++i)
00492 loadSaveSensors->buttons[i-1] = atof(words[i].c_str());
00493 } else {
00494
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
00516 for(unsigned int i=1; i<size; ++i)
00517 loadSaveSensors->sensors[i-1] = atof(words[i].c_str());
00518 } else {
00519
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
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 }
00559 }
00560 }
00561 return true;
00562 }
00563
00564
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) {
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) {
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
00675
00676
00677
00678
00679
00680
00681
00682
00683
00684
00685
00686
00687
00688
00689
00690
00691
00692
00693
00694
00695
00696
00697
00698
00699
00700
00701
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
00723
00724
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;
00738 else
00739 obj_comp_link=obj_comp_link/.975;
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
00756
00757
00758
00759
00760
00761
00762
00763
00764
00765