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