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