MotionSequenceEngine.ccGo to the documentation of this file.00001 #include "MotionSequenceMC.h"
00002 #include "Shared/get_time.h"
00003 #include "Shared/WorldState.h"
00004 #include "Shared/Config.h"
00005 #include <iostream>
00006
00007 using std::cout;
00008 using std::endl;
00009
00010 MotionSequenceEngine::Move_idx_t MotionSequenceEngine::invalid_move=-1U;
00011
00012 int MotionSequenceEngine::updateOutputs() {
00013 if(isPlaying()) {
00014 if(lasttime==0)
00015 play();
00016 double diff=(get_time()-lasttime)*playspeed;
00017 if(diff+playtime<0)
00018 setTime(0);
00019 else
00020 setTime(static_cast<unsigned int>(diff+playtime));
00021 lasttime=get_time();
00022 return 1;
00023 } else {
00024 lasttime=get_time();
00025 return 0;
00026 }
00027 }
00028
00029 const OutputCmd& MotionSequenceEngine::getOutputCmd(unsigned int i) {
00030 if(curstamps[i]!=playtime) {
00031 if(nexts[i]!=invalid_move)
00032 calcOutput(curs[i],playtime,getKeyFrame(prevs[i]),getKeyFrame(nexts[i]));
00033 else
00034 curs[i].unset();
00035 curstamps[i]=playtime;
00036 }
00037 return curs[i];
00038 }
00039
00040 unsigned int MotionSequenceEngine::getBinSize() const {
00041 char buf[128];
00042 unsigned int len=128;
00043 unsigned int used=strlen("#MSq\n");
00044 used+=snprintf(buf,len,isSaveRadians()?"radians\n":"degrees\n");
00045 unsigned int t=0;
00046 Move_idx_t tprevs[NumOutputs];
00047 Move_idx_t tnexts[NumOutputs];
00048 for(unsigned int i=0;i<NumOutputs;i++)
00049 tnexts[i]=getKeyFrame(tprevs[i]=starts[i]).next;
00050 while(t!=-1U) {
00051 for(unsigned int i=0; i<NumOutputs; i++) {
00052 if((t!=0 || getKeyFrame(tprevs[i]).cmd.weight!=0) && getKeyFrame(tprevs[i]).starttime==t) {
00053 if(getKeyFrame(tprevs[i]).cmd.weight==1) {
00054 if(t!=0)
00055 used+=snprintf(buf,len,"%s\t%g\n",outputNames[i],getKeyFrame(tprevs[i]).cmd.value/loadSaveMode);
00056 } else
00057 used+=snprintf(buf,len,"%s\t%g\t%g\n",outputNames[i],getKeyFrame(tprevs[i]).cmd.value/loadSaveMode,getKeyFrame(tprevs[i]).cmd.weight);
00058 }
00059 }
00060 unsigned int last=t;
00061 t=setNextFrameTime(tprevs,tnexts);
00062 if(t!=-1U)
00063 used+=snprintf(buf,len,"advanceTime\t%d\n",t-last);
00064 }
00065 used+=strlen("#END\n");
00066 return used+1;
00067 }
00068
00069 unsigned int MotionSequenceEngine::LoadBuffer(const char buf[], unsigned int len) {
00070 unsigned int origlen=len;
00071 if(strncmp("#POS",buf,4)==0) {
00072
00073 PostureEngine pose;
00074 unsigned int used=pose.LoadBuffer(buf,len);
00075 if(used!=0)
00076 setPose(pose);
00077 return used;
00078 }
00079 if(strncmp("#MSq",buf,4)!=0) {
00080
00081
00082
00083
00084 return 0;
00085 }
00086 unsigned int linenum=1;
00087 unsigned int lastOutputIdx=0;
00088 while(len<=origlen && len>0) {
00089 int written;
00090
00091 if(buf[0]=='\r') {
00092 buf++; len--;
00093 if(buf[0]=='\n') {
00094 buf++; len--;
00095 }
00096 linenum++;
00097 continue;
00098 }
00099 if(buf[0]=='\n') {
00100 buf++; len--;
00101 linenum++;
00102 continue;
00103 }
00104 if(buf[0]=='#') {
00105 if(strncmp("#END\n",buf,5)==0 || strncmp("#END\r",buf,5)==0) {
00106 return origlen-len+5;
00107 } else if(strncmp("#END\r\n",buf,6)==0) {
00108 return origlen-len+6;
00109 } else {
00110 while(len>0 && *buf!='\n' && *buf!='\r') {len--;buf++;}
00111 if(*buf=='\n') {
00112 buf++;
00113 len--;
00114 }
00115 linenum++;
00116 continue;
00117 }
00118 }
00119 written=-1;
00120 const unsigned int cmdlen=16, arglen=32;
00121 char command[cmdlen];
00122 char arg1[arglen];
00123 char arg2[arglen];
00124 written=readWord(buf,&buf[len],command,cmdlen);
00125 if(!ChkAdvance(written,&buf,&len,"*** ERROR MotionSequenceEngine load corrupted - line %d\n",linenum)) return 0;
00126 written=readWord(buf,&buf[len],arg1,arglen);
00127 if(written>0)
00128 if(!ChkAdvance(written,&buf,&len,"*** ERROR MotionSequenceEngine load corrupted - line %d\n",linenum)) return 0;
00129 written=readWord(buf,&buf[len],arg2,arglen);
00130 if(written!=0)
00131 if(!ChkAdvance(written,&buf,&len,"*** ERROR MotionSequenceEngine load corrupted - line %d\n",linenum)) return 0;
00132 for(;len>0 && *buf!='\n' && *buf!='\r';buf++,len--) {}
00133 if(*buf=='\n') {
00134 buf++;
00135 len--;
00136 }
00137
00138 if(strcasecmp(command,"delay")==0 || strcasecmp(command,"advanceTime")==0) {
00139 char* used;
00140 int delay = strtol(arg1,&used,0);
00141 if(*used!='\0') {
00142 cout << "*** WARNING illegal delay argument: " << arg1 << " - line " << linenum << endl;
00143 } else {
00144 setTime(playtime+delay);
00145 }
00146 } else if(strcasecmp(command,"settime")==0) {
00147 char* used;
00148 int newtime = strtol(arg1,&used,0);
00149 if(*used!='\0') {
00150 cout << "*** WARNING illegal settime argument: " << arg1 << " - line " << linenum << endl;
00151 } else {
00152 setTime(newtime);
00153 }
00154 } else if(strcasecmp(command,"load")==0) {
00155 PostureEngine pose;
00156 std::string f;
00157 if(arg1[0]!='/')
00158 f="/ms/data/motion/";
00159 f+=arg1;
00160 if(pose.LoadFile(f.c_str())!=0) {
00161 setPose(pose);
00162 } else
00163 cout << "*** WARNING could not read file " << arg1 << " for load - line " << linenum << endl;
00164 } else if(strcasecmp(command,"overlay")==0) {
00165 PostureEngine pose;
00166 std::string f;
00167 if(arg1[0]!='/')
00168 f="/ms/data/motion/";
00169 f+=arg1;
00170 if(pose.LoadFile(f.c_str())!=0)
00171 overlayPose(pose);
00172 else if(LoadFile(f.c_str())==0)
00173 cout << "*** WARNING could not read file " << arg1 << " for overlay - line " << linenum << endl;
00174 } else if(strcasecmp(command,"degrees")==0) {
00175 setSaveDegrees();
00176 } else if(strcasecmp(command,"radians")==0) {
00177 setSaveRadians();
00178 } else {
00179 lastOutputIdx=getOutputIndex(command,lastOutputIdx+1);
00180 if(lastOutputIdx==NumOutputs)
00181 cout << "*** WARNING " << command << " is not a valid joint on this model." << endl;
00182 else {
00183 char* used;
00184 double value=strtod(arg1,&used), weight=1;
00185 if(*used!='\0')
00186 cout << "*** WARNING illegal value argument: " << arg1 << " - line " << linenum << endl;
00187 else {
00188 if(arg2[0]!='\0') {
00189 weight=strtod(arg2,&used);
00190 if(*used!='\0') {
00191 cout << "*** WARNING illegal weight argument: " << arg2 << " - line " << linenum << endl;
00192 weight=1;
00193 }
00194 }
00195 setOutputCmd(lastOutputIdx,OutputCmd(value*loadSaveMode,weight));
00196 }
00197 }
00198 }
00199
00200 linenum++;
00201
00202 }
00203 cout << "*** WARNING MotionSequenceEngine load missing #END" << endl;
00204 return origlen-len;
00205 }
00206
00207 unsigned int MotionSequenceEngine::SaveBuffer(char buf[], unsigned int len) const {
00208
00209 unsigned int origlen=len;
00210 int written=snprintf(buf,len,"#MSq\n");
00211 if(!ChkAdvance(written,(const char**)&buf,&len,"*** ERROR MotionSequenceEngine save failed on header\n")) return 0; if(len==0 || len>origlen) {
00212 cout << "*** ERROR MotionSequenceEngine save overflow on header" << endl;
00213 return 0;
00214 }
00215 written=snprintf(buf,len,isSaveRadians()?"radians\n":"degrees\n");
00216 if(!ChkAdvance(written,(const char**)&buf,&len,"*** ERROR MotionSequenceEngine save failed on mode\n")) return 0; if(len==0 || len>origlen) {
00217 cout << "*** ERROR MotionSequenceEngine save overflow" << endl;
00218 return 0;
00219 }
00220 unsigned int t=0;
00221 Move_idx_t tprevs[NumOutputs];
00222 Move_idx_t tnexts[NumOutputs];
00223 for(unsigned int i=0;i<NumOutputs;i++)
00224 tnexts[i]=getKeyFrame(tprevs[i]=starts[i]).next;
00225 while(t!=-1U) {
00226
00227 for(unsigned int i=0; i<NumOutputs; i++) {
00228
00229
00230 if((t!=0 || getKeyFrame(tprevs[i]).cmd.weight!=0) && getKeyFrame(tprevs[i]).starttime==t) {
00231 if(getKeyFrame(tprevs[i]).cmd.weight==1) {
00232 if(t!=0) {
00233 written=snprintf(buf,len,"%s\t%g\n",outputNames[i],getKeyFrame(tprevs[i]).cmd.value/loadSaveMode);
00234 if(!ChkAdvance(written,(const char**)&buf,&len,"*** ERROR MotionSequenceEngine save failed\n")) return 0;
00235 }
00236 } else {
00237 written=snprintf(buf,len,"%s\t%g\t%g\n",outputNames[i],getKeyFrame(tprevs[i]).cmd.value/loadSaveMode,getKeyFrame(tprevs[i]).cmd.weight);
00238 if(!ChkAdvance(written,(const char**)&buf,&len,"*** ERROR MotionSequenceEngine save failed\n")) return 0;
00239 }
00240 if(len==0 || len>origlen) {
00241 cout << "*** ERROR MotionSequenceEngine save overflow" << endl;
00242 return 0;
00243 }
00244 }
00245 }
00246 unsigned int last=t;
00247 t=setNextFrameTime(tprevs,tnexts);
00248 if(t!=-1U) {
00249 written=snprintf(buf,len,"delay\t%d\n",t-last);
00250 if(!ChkAdvance(written,(const char**)&buf,&len,"*** ERROR MotionSequenceEngine save failed\n")) return 0;
00251 if(len==0 || len>origlen) {
00252 cout << "*** ERROR MotionSequenceEngine save overflow" << endl;
00253 return 0;
00254 }
00255 }
00256 }
00257 written=snprintf(buf,len,"#END\n");
00258 if(!ChkAdvance(written,(const char**)&buf,&len,"*** ERROR MotionSequenceEngine save failed on #END\n")) return 0;
00259 if(len==0 || len>origlen) {
00260 cout << "*** ERROR MotionSequenceEngine save overflow on #END" << endl;
00261 return 0;
00262 }
00263 return origlen-len;
00264 cout << "SAVE-done!" << endl;
00265 }
00266
00267 unsigned int MotionSequenceEngine::LoadFile(const char filename[]) {
00268 return LoadSave::LoadFile(config->motion.makePath(filename).c_str());
00269 }
00270 unsigned int MotionSequenceEngine::SaveFile(const char filename[]) const {
00271 return LoadSave::SaveFile(config->motion.makePath(filename).c_str());
00272 }
00273
00274 void MotionSequenceEngine::setTime(unsigned int x) {
00275 playtime=x;
00276 for(unsigned int i=0; i<NumOutputs; i++)
00277 setRange(x,prevs[i],nexts[i]);
00278 }
00279
00280 void MotionSequenceEngine::setOutputCmd(unsigned int i, const OutputCmd& cmd) {
00281 Move& p_m=getKeyFrame(prevs[i]);
00282 if(playtime==p_m.starttime) {
00283 p_m.cmd=cmd;
00284 } else {
00285 Move_idx_t x = newKeyFrame();
00286 if(x==invalid_move)
00287 return;
00288 Move& cur=getKeyFrame(x);
00289 Move& prev_m=getKeyFrame(prevs[i]);
00290 cur.cmd=cmd;
00291 cur.starttime=playtime;
00292 cur.prev=prevs[i];
00293 cur.next=nexts[i];
00294 prev_m.next=x;
00295 if(nexts[i]!=invalid_move)
00296 getKeyFrame(nexts[i]).prev=x;
00297 else {
00298 if(playtime>endtime)
00299 endtime=playtime;
00300 }
00301 prevs[i]=x;
00302
00303 }
00304 }
00305
00306 void MotionSequenceEngine::setPose(const PostureEngine& pose) {
00307 for(unsigned int i=0; i<NumOutputs; i++)
00308 setOutputCmd(i,pose.getOutputCmd(i));
00309 }
00310
00311 void MotionSequenceEngine::overlayPose(const PostureEngine& pose) {
00312 for(unsigned int i=0; i<NumOutputs; i++)
00313 if(pose.getOutputCmd(i).weight>0)
00314 setOutputCmd(i,pose.getOutputCmd(i));
00315 }
00316
00317 void MotionSequenceEngine::compress() {
00318 for(unsigned int i=0; i<NumOutputs; i++) {
00319 Move_idx_t prev=getKeyFrame(starts[i]).next;
00320 if(prev==(Move_idx_t)-1)
00321 break;
00322 Move_idx_t cur=getKeyFrame(prev).next;
00323 if(cur==(Move_idx_t)-1)
00324 break;
00325 Move_idx_t next=getKeyFrame(cur).next;
00326 while(next!=(Move_idx_t)-1) {
00327 OutputCmd tmp;
00328 Move& prev_m=getKeyFrame(prev);
00329 Move& cur_m=getKeyFrame(cur);
00330 Move& next_m=getKeyFrame(next);
00331 calcOutput(tmp,cur_m.starttime,prev_m,next_m);
00332 if(tmp==cur_m.cmd || tmp.weight==0 && cur_m.cmd.weight==0) {
00333 prev_m.next=next;
00334 next_m.prev=prev;
00335 eraseKeyFrame(cur);
00336 } else
00337 prev=cur;
00338 cur=next;
00339 next=next_m.next;
00340 }
00341 }
00342 }
00343
00344 void MotionSequenceEngine::makeSafe(const float vels[NumOutputs], float margin) {
00345 float comps[NumOutputs];
00346 for(unsigned int i=0;i<NumOutputs;i++)
00347 comps[i]=vels[i]*margin;
00348 unsigned int t=0;
00349 Move_idx_t tprevs[NumOutputs];
00350 Move_idx_t tnexts[NumOutputs];
00351 for(unsigned int i=0;i<NumOutputs;i++)
00352 tnexts[i]=getKeyFrame(tprevs[i]=starts[i]).next;
00353 while(t!=-1U) {
00354 for(unsigned int i=0; i<NumOutputs; i++) {
00355
00356 if(tnexts[i]!=(Move_idx_t)-1 && (getKeyFrame(tprevs[i]).cmd.weight!=0 || getKeyFrame(tnexts[i]).cmd.weight!=0) && getKeyFrame(tprevs[i]).starttime==t) {
00357 float dv=fabs(getKeyFrame(tprevs[i]).cmd.value-getKeyFrame(tnexts[i]).cmd.value);
00358 unsigned int dt=getKeyFrame(tnexts[i]).starttime-getKeyFrame(tprevs[i]).starttime;
00359 if(dv/dt>comps[i]) {
00360 unsigned int delay=(unsigned int)(dv/comps[i])-dt;
00361 for(unsigned int j=0; j<NumOutputs; j++)
00362 for(Move_idx_t c=tnexts[j]; c!=(Move_idx_t)-1; c=getKeyFrame(c).next)
00363 getKeyFrame(c).starttime+=delay;
00364 }
00365 }
00366 }
00367 t=setNextFrameTime(tprevs,tnexts);
00368 }
00369
00370 }
00371
00372 bool MotionSequenceEngine::isPlaying() {
00373 return playing && ((playspeed>0) ? (playtime<=endtime) : (playtime>0));
00374 }
00375
00376
00377 void MotionSequenceEngine::play() {
00378 if(playspeed>0)
00379 setTime(0);
00380 else
00381 setTime(endtime);
00382 resume();
00383 }
00384
00385 void MotionSequenceEngine::resume() {
00386 playing=true;
00387 lasttime=get_time();
00388 for(unsigned int i=0; i<NumOutputs; i++) {
00389 Move_idx_t cur=starts[i];
00390 while(cur!=(Move_idx_t)-1) {
00391 if(getKeyFrame(cur).cmd.weight!=0) {
00392 getKeyFrame(starts[i]).cmd.value=state->outputs[i];
00393 break;
00394 }
00395 cur=getKeyFrame(cur).next;
00396 }
00397 }
00398 }
00399
00400 unsigned int MotionSequenceEngine::setNextFrameTime(Move_idx_t p[NumOutputs], Move_idx_t n[NumOutputs]) const {
00401 unsigned int ans=-1U;
00402 for(unsigned int i=0; i<NumOutputs; i++)
00403 if(n[i]!=invalid_move && getKeyFrame(n[i]).starttime<ans)
00404 ans=getKeyFrame(n[i]).starttime;
00405 if(ans!=-1U)
00406 for(unsigned int i=0; i<NumOutputs; i++)
00407 setRange(ans,p[i],n[i]);
00408 return ans;
00409 }
00410
00411 unsigned int MotionSequenceEngine::readWord(const char buf[], const char * const bufend, char wrd[], const unsigned int wordlen) {
00412 const char* origbuf=buf;
00413 wrd[0]='\0';
00414 unsigned int i;
00415
00416 for(;buf<bufend && isspace(*buf) && *buf!='\n' && *buf!='\r';buf++) {}
00417
00418 for(i=0; buf<bufend && !isspace(*buf); buf++)
00419 if(i<wordlen-1)
00420 wrd[i++]=*buf;
00421 wrd[i]='\0';
00422 if(buf>=bufend)
00423 return -1U;
00424 return buf-origbuf;
00425 }
00426
00427 unsigned int MotionSequenceEngine::getOutputIndex(const char name[], unsigned int idx) {
00428 if(idx<NumOutputs) {
00429 unsigned int startidx=idx;
00430 for(;idx<NumOutputs;idx++)
00431 if(strcmp(name,outputNames[idx])==0)
00432 return idx;
00433 for(idx=0;idx<startidx;idx++)
00434 if(strcmp(name,outputNames[idx])==0)
00435 return idx;
00436 return NumOutputs;
00437 } else {
00438 for(idx=0;idx<NumOutputs;idx++)
00439 if(strcmp(name,outputNames[idx])==0)
00440 return idx;
00441 return idx;
00442 }
00443 }
00444
00445
00446 void MotionSequenceEngine::setPlayTime(unsigned int x) {
00447 setTime(x);
00448 }
00449
00450
00451 unsigned int MotionSequenceEngine::getPlayTime() const {
00452 return getTime();
00453 }
00454
00455
00456 void MotionSequenceEngine::setPlaySpeed(float x) {
00457 setSpeed(x);
00458 }
00459
00460
00461 float MotionSequenceEngine::getPlaySpeed() const {
00462 return getSpeed();
00463 }
00464
00465
00466
00467
00468
00469
00470
00471
00472
00473
00474
00475
|