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