Tekkotsu Homepage

Demos

Overview

Downloads

Dev. Resources

Reference

Credits

MotionSequenceEngine.cc Go to the documentation of this file. #include "MotionSequenceEngine.h" #include "DynamicMotionSequence.h" #include "Shared/get_time.h" #include "Shared/WorldState.h" #include "Shared/Config.h" #include <iostream> using std::cout; using std::endl; MotionSequenceEngine::Move_idx_t MotionSequenceEngine::invalid_move=-1U; MotionSequenceEngine::MotionSequenceEngine() : LoadSave(), playtime(1), lasttime(0), endtime(0), playspeed(1.0), playing(true), hold(true), loadSaveMode(M_PI/180) { for(unsigned int i=0; i<NumOutputs; i++) curstamps[i]=-1U; } int MotionSequenceEngine::updateOutputs() { if(isPlaying()) { if(lasttime==0) play(); unsigned int curtime=get_time(); float diff=(curtime-lasttime)*playspeed; if(playtime<-diff) setTime(0); else setTime(static_cast<unsigned int>(diff+playtime)); lasttime=curtime; return 1; } else { lasttime=get_time(); return 0; } } const OutputCmd& MotionSequenceEngine::getOutputCmd(unsigned int i) { if(curstamps[i]!=playtime) { if(nexts[i]!=invalid_move) calcOutput(curs[i],playtime,getKeyFrame(prevs[i]),getKeyFrame(nexts[i])); else if(hold) curs[i]=getKeyFrame(prevs[i]).cmd; else curs[i].unset(); curstamps[i]=playtime; } return curs[i]; } unsigned int MotionSequenceEngine::getBinSize() const { char buf[128]; unsigned int len=128; unsigned int used=strlen("#MSq\n"); used+=snprintf(buf,len,isSaveRadians()?"radians\n":"degrees\n"); unsigned int t=0; Move_idx_t tprevs[NumOutputs]; Move_idx_t tnexts[NumOutputs]; bool hasInitialFrame=false; for(unsigned int i=0;i<NumOutputs;i++) { tnexts[i]=getKeyFrame(tprevs[i]=starts[i]).next; if(getKeyFrame(starts[i]).cmd.weight!=0) hasInitialFrame=true; } if(hasInitialFrame) used+=snprintf(buf,len,"setTime\t0\n"); while(t!=-1U) { for(unsigned int i=0; i<NumOutputs; i++) { if((t!=0 || getKeyFrame(tprevs[i]).cmd.weight!=0) && getKeyFrame(tprevs[i]).starttime==t) { if(getKeyFrame(tprevs[i]).cmd.weight==1) used+=snprintf(buf,len,"%s\t%g\n",outputNames[i],getKeyFrame(tprevs[i]).cmd.value/loadSaveMode); else used+=snprintf(buf,len,"%s\t%g\t%g\n",outputNames[i],getKeyFrame(tprevs[i]).cmd.value/loadSaveMode,getKeyFrame(tprevs[i]).cmd.weight); } } unsigned int last=t; t=setNextFrameTime(tprevs,tnexts); if(t!=-1U) used+=snprintf(buf,len,"advanceTime\t%d\n",t-last); } used+=strlen("#END\n"); return used+1; } unsigned int MotionSequenceEngine::loadBuffer(const char buf[], unsigned int len) { unsigned int origlen=len; if(strncmp("#POS",buf,4)==0) { // allow inlined loading of posture files PostureEngine pose; unsigned int used=pose.loadBuffer(buf,len); if(used!=0) setPose(pose); return used; } if(strncmp("#MSq",buf,4)!=0) { // we don't want to display an error here because we may be only testing file type, // so it's up to the caller to decide if it's necessarily an error if the file isn't // a motion sequence //cout << "ERROR MotionSequenceEngine load corrupted - expected #MSq header" << endl; return 0; } unsigned int linenum=1; unsigned int lastOutputIdx=0; while(len<=origlen && len>0) { int written; //printf("%d %.9s\n",linenum+1,buf); if(buf[0]=='\r') { buf++; len--; if(buf[0]=='\n') { buf++; len--; } linenum++; continue; } if(buf[0]=='\n') { buf++; len--; linenum++; continue; } if(buf[0]=='#') { if(strncmp("#END\n",buf,5)==0 || strncmp("#END\r",buf,5)==0) { return origlen-len+5; } else if(strncmp("#END\r\n",buf,6)==0) { return origlen-len+6; } else { while(len>0 && *buf!='\n' && *buf!='\r') {len--;buf++;} if(*buf=='\n') { //in case of \r\n buf++; len--; } linenum++; continue; } } written=-1; const unsigned int cmdlen=16, arglen=32; char command[cmdlen]; char arg1[arglen]; char arg2[arglen]; written=readWord(buf,&buf[len],command,cmdlen); if(!checkInc(written,buf,len,"*** ERROR MotionSequenceEngine load corrupted - line %d\n",linenum)) return 0; written=readWord(buf,&buf[len],arg1,arglen); if(written>0) if(!checkInc(written,buf,len,"*** ERROR MotionSequenceEngine load corrupted - line %d\n",linenum)) return 0; written=readWord(buf,&buf[len],arg2,arglen); if(written!=0) if(!checkInc(written,buf,len,"*** ERROR MotionSequenceEngine load corrupted - line %d\n",linenum)) return 0; for(;len>0 && *buf!='\n' && *buf!='\r';buf++,len--) {} if(*buf=='\n') { //in case of \r\n buf++; len--; } if(strcasecmp(command,"delay")==0 || strcasecmp(command,"advanceTime")==0) { char* used; int delay = strtol(arg1,&used,0); if(*used!='\0') { cout << "*** WARNING illegal delay argument: " << arg1 << " - line " << linenum << endl; } else { setTime(playtime+delay); } } else if(strcasecmp(command,"settime")==0) { char* used; int newtime = strtol(arg1,&used,0); if(*used!='\0') { cout << "*** WARNING illegal settime argument: " << arg1 << " - line " << linenum << endl; } else { setTime(newtime); } } else if(strcasecmp(command,"load")==0 || strcasecmp(command,"overlay")==0) { PostureEngine pose; if(pose.loadFile(arg1)!=0) { // it's a posture file setPose(pose); } else if(overlayMotion(arg1)!=0) { // it's a motionsequence file (conditional test did the overlay) } else { // unknown file, give warning cout << "*** WARNING could not read file " << arg1 << " for overlay - line " << linenum << endl; } } else if(strcasecmp(command,"loadExplicit")==0) { PostureEngine pose; if(pose.loadFile(arg1)!=0) { setExplicitPose(pose); } else cout << "*** WARNING could not read file " << arg1 << " for load (loadExplicit only accepts posture files) - line " << linenum << endl; } else if(strcasecmp(command,"degrees")==0) { setSaveDegrees(); } else if(strcasecmp(command,"radians")==0) { setSaveRadians(); } else { lastOutputIdx=getOutputIndex(command,lastOutputIdx+1); bool found = (lastOutputIdx!=NumOutputs); unsigned int lidx=lastOutputIdx,ridx=NumOutputs; if(!found) { // base name not found // try symmetric left/right versions char tname[cmdlen+1]; strncpy(tname+1,command,cmdlen); tname[0]='L'; lidx=getOutputIndex(tname,lastOutputIdx+1); if(lidx!=NumOutputs) { tname[0]='R'; ridx=getOutputIndex(tname,lidx+1); if(ridx!=NumOutputs) found=true; } } if (!found) { cout << "*** WARNING " << command << " is not a valid joint on this model." << endl; } else { char* used; double value=strtod(arg1,&used), weight=1; if(*used!='\0') cout << "*** WARNING illegal value argument: " << arg1 << " - line " << linenum << endl; else { if(arg2[0]!='\0') { weight=strtod(arg2,&used); if(*used!='\0') { cout << "*** WARNING illegal weight argument: " << arg2 << " - line " << linenum << endl; weight=1; } } if(lastOutputIdx!=NumOutputs) { // found exact match earlier setOutputCmd(lastOutputIdx,OutputCmd(value*loadSaveMode,weight)); } else { // found symmetric matches earlier setOutputCmd(lidx,OutputCmd(value*loadSaveMode,weight)); setOutputCmd(ridx,OutputCmd(value*loadSaveMode,weight)); lastOutputIdx=ridx; } } } } linenum++; } cout << "*** WARNING MotionSequenceEngine load missing #END" << endl; return origlen-len; } unsigned int MotionSequenceEngine::saveBuffer(char buf[], unsigned int len) const { //std::cout << "SAVEBUFFER..." << std::flush; unsigned int origlen=len; int written=snprintf(buf,len,"#MSq\n"); if(!checkInc(written,buf,len,"*** ERROR MotionSequenceEngine save failed on header\n")) return 0; if(len==0 || len>origlen) { cout << "*** ERROR MotionSequenceEngine save overflow on header" << endl; return 0; } written=snprintf(buf,len,isSaveRadians()?"radians\n":"degrees\n"); if(!checkInc(written,buf,len,"*** ERROR MotionSequenceEngine save failed on mode\n")) return 0; if(len==0 || len>origlen) { cout << "*** ERROR MotionSequenceEngine save overflow" << endl; return 0; } unsigned int t=0; Move_idx_t tprevs[NumOutputs]; Move_idx_t tnexts[NumOutputs]; bool hasInitialFrame=false; for(unsigned int i=0;i<NumOutputs;i++) { tnexts[i]=getKeyFrame(tprevs[i]=starts[i]).next; if(getKeyFrame(starts[i]).cmd.weight!=0) hasInitialFrame=true; } if(hasInitialFrame) { written=snprintf(buf,len,"setTime\t0\n"); if(!checkInc(written,buf,len,"*** ERROR MotionSequenceEngine save failed on initial frame spec\n")) return 0; } while(t!=-1U) { //std::cout << "t="<<t<<"..."<<std::endl; for(unsigned int i=0; i<NumOutputs; i++) { //cout << i << ' ' << outputNames[i] << " (" << getKeyFrame(tprevs[i]).cmd.value << ',' << getKeyFrame(tprevs[i]).cmd.weight << ") " << getKeyFrame(tprevs[i]).starttime << " state: " << starts[i] <<' '<< prevs[i] <<' '<< nexts[i] << " cur: " << getKeyFrame(tprevs[i]).prev << ' ' << tprevs[i] << ' ' << getKeyFrame(tprevs[i]).next << endl; //first conditional is to skip 0 weighted values in first frame if((t!=0 || getKeyFrame(tprevs[i]).cmd.weight!=0) && getKeyFrame(tprevs[i]).starttime==t) { if(getKeyFrame(tprevs[i]).cmd.weight==1) { written=snprintf(buf,len,"%s\t%g\n",outputNames[i],getKeyFrame(tprevs[i]).cmd.value/loadSaveMode); if(!checkInc(written,buf,len,"*** ERROR MotionSequenceEngine save failed\n")) return 0; } else { written=snprintf(buf,len,"%s\t%g\t%g\n",outputNames[i],getKeyFrame(tprevs[i]).cmd.value/loadSaveMode,getKeyFrame(tprevs[i]).cmd.weight); if(!checkInc(written,buf,len,"*** ERROR MotionSequenceEngine save failed\n")) return 0; } if(len==0 || len>origlen) { cout << "*** ERROR MotionSequenceEngine save overflow" << endl; return 0; } } } unsigned int last=t; t=setNextFrameTime(tprevs,tnexts); if(t!=-1U) { written=snprintf(buf,len,"delay\t%d\n",t-last); if(!checkInc(written,buf,len,"*** ERROR MotionSequenceEngine save failed\n")) return 0; if(len==0 || len>origlen) { cout << "*** ERROR MotionSequenceEngine save overflow" << endl; return 0; } } } written=snprintf(buf,len,"#END\n"); if(!checkInc(written,buf,len,"*** ERROR MotionSequenceEngine save failed on #END\n")) return 0; if(len==0 || len>origlen) { cout << "*** ERROR MotionSequenceEngine save overflow on #END" << endl; return 0; } return origlen-len; cout << "SAVE-done!" << endl; } unsigned int MotionSequenceEngine::loadFile(const char filename[]) { return LoadSave::loadFile(config->motion.makePath(filename).c_str()); } unsigned int MotionSequenceEngine::saveFile(const char filename[]) const { return LoadSave::saveFile(config->motion.makePath(filename).c_str()); } void MotionSequenceEngine::setTime(unsigned int x) { playtime=x; WorldState * st=WorldState::getCurrent(); for(unsigned int i=0; i<NumOutputs; i++) { if(setRange(x,prevs[i],nexts[i])) { OutputCmd& cmd=getKeyFrame((playspeed<0)?nexts[i]:prevs[i]).cmd; if(cmd.weight<=0) cmd.value=st->outputs[i]; } } } void MotionSequenceEngine::setOutputCmd(unsigned int i, const OutputCmd& cmd) { Move& p_m=getKeyFrame(prevs[i]); if(playtime==p_m.starttime) { //edit current keyframe p_m.cmd=cmd; } else { //add new keyframe Move_idx_t x = newKeyFrame(); if(x==invalid_move) //ran out of memory - newKeyFrame should report error, we can silently die return; Move& cur=getKeyFrame(x); Move& prev_m=getKeyFrame(prevs[i]); //it's important to refresh this - newKeyFrame may have invalidated p_m reference cur.cmd=cmd; cur.starttime=playtime; cur.prev=prevs[i]; cur.next=nexts[i]; prev_m.next=x; if(nexts[i]!=invalid_move) //in middle getKeyFrame(nexts[i]).prev=x; else { //at end if(playtime>endtime) endtime=playtime; } prevs[i]=x; // cout << "set " << i << ' ' << outputNames[i] << ' ' << cur.cmd.value << ' ' << cur.cmd.weight << ' ' << cur.starttime << " state: " << starts[i] <<' '<< prevs[i] <<' '<< prev_m.next <<' '<< nexts[i] << " new: " << cur.prev << ' ' << x << ' ' << cur.next << endl; } curstamps[i]=-1U; } void MotionSequenceEngine::setPose(const PostureEngine& pose) { for(unsigned int i=0; i<NumOutputs; i++) if(pose.getOutputCmd(i).weight>0) setOutputCmd(i,pose.getOutputCmd(i)); } void MotionSequenceEngine::setExplicitPose(const PostureEngine& pose) { for(unsigned int i=0; i<NumOutputs; i++) setOutputCmd(i,pose.getOutputCmd(i)); } void MotionSequenceEngine::overlayPose(const PostureEngine& pose) { setPose(pose); } PostureEngine MotionSequenceEngine::getPose() { PostureEngine pose; getPose(pose); return pose; } void MotionSequenceEngine::getPose(PostureEngine& pose) { for(unsigned int i=0; i<NumOutputs; i++) pose.setOutputCmd(i,getOutputCmd(i)); } unsigned int MotionSequenceEngine::overlayMotion(const std::string& msFile) { DynamicMotionSequence ms; if(ms.loadFile(msFile.c_str())==0) return 0; overlayMotion(ms); return ms.getEndTime(); } /*! @todo should better handle conflicts with keyframes in original motion * * This is not a particularly well implemented function -- it will interleave * @a ms's keyframes with any pre-existing ones, but will clobber frames which * coincide with @a ms's own. Probably will not give the desired effect when * the motion is actually overlaying something, but it will work for basic * usage cases. */ void MotionSequenceEngine::overlayMotion(const MotionSequenceEngine& ms) { //merge keyframes for each output, advance playhead by ms.endtime at the end for(unsigned int i=0; i<NumOutputs; i++) { //cout << "Processing " << outputNames[i] << ":" << endl; Move_idx_t myPrevIdx=prevs[i]; unsigned int myPrevTime=getKeyFrame(prevs[i]).starttime; Move_idx_t myNextIdx=getKeyFrame(myPrevIdx).next; unsigned int myNextTime=-1U; if(myNextIdx!=invalid_move) myNextTime=getKeyFrame(myNextIdx).starttime; for(Move_idx_t curOtherIdx=ms.starts[i]; curOtherIdx!=invalid_move; curOtherIdx=ms.getKeyFrame(curOtherIdx).next) { const Move& curOther=ms.getKeyFrame(curOtherIdx); while(myNextTime <= curOther.starttime+getTime()) { //cout << "Advancing to " << myNextTime << endl; myPrevIdx=myNextIdx; myPrevTime=myNextTime; myNextIdx=getKeyFrame(myPrevIdx).next; myNextTime= (myNextIdx==invalid_move ? -1U : getKeyFrame(myNextIdx).starttime); } if(curOther.cmd.weight > 0) { if(curOther.starttime+getTime() == myPrevTime) { //replace current 'prev' //cout << "replacing frame " << myPrevIdx << " at " << myPrevTime << endl; getKeyFrame(myPrevIdx).cmd=curOther.cmd; } else { //insert new keyframe between 'prev' and 'next' Move_idx_t insIdx=newKeyFrame(); //cout << "adding new frame at " << curOther.starttime+getTime() << " with index " << insIdx << endl; if(insIdx==invalid_move) return; //newKeyFrame should have reported error, we can silently return Move& ins=getKeyFrame(insIdx); ins.prev=myPrevIdx; ins.next=myNextIdx; ins.cmd=curOther.cmd; ins.starttime=curOther.starttime+getTime(); getKeyFrame(myPrevIdx).next=insIdx; if(myNextIdx!=invalid_move) getKeyFrame(myNextIdx).prev=insIdx; if(myPrevIdx==prevs[i]) { nexts[i]=insIdx; curstamps[i]=-1U; } myPrevIdx=insIdx; //inserted frame is now 'prev' myPrevTime=ins.starttime; if(myPrevTime>endtime) endtime=myPrevTime; } } } } advanceTime(ms.getEndTime()); } void MotionSequenceEngine::compress() { for(unsigned int i=0; i<NumOutputs; i++) { Move_idx_t prev=getKeyFrame(starts[i]).next; if(prev==invalid_move) break; Move_idx_t cur=getKeyFrame(prev).next; if(cur==invalid_move) break; Move_idx_t next=getKeyFrame(cur).next; while(next!=invalid_move) { OutputCmd tmp; Move& prev_m=getKeyFrame(prev); Move& cur_m=getKeyFrame(cur); Move& next_m=getKeyFrame(next); calcOutput(tmp,cur_m.starttime,prev_m,next_m); if(tmp==cur_m.cmd || tmp.weight==0 && cur_m.cmd.weight==0) { prev_m.next=next; next_m.prev=prev; eraseKeyFrame(cur); } else prev=cur; cur=next; next=next_m.next; } } } void MotionSequenceEngine::makeSafe(const float vels[NumOutputs], float margin) { float comps[NumOutputs]; for(unsigned int i=0;i<NumOutputs;i++) comps[i]=vels[i]*margin; unsigned int t=0; Move_idx_t tprevs[NumOutputs]; Move_idx_t tnexts[NumOutputs]; for(unsigned int i=0;i<NumOutputs;i++) { tnexts[i]=getKeyFrame(tprevs[i]=starts[i]).next; curstamps[i]=-1U; } while(t!=-1U) { for(unsigned int i=0; i<NumOutputs; i++) { //second and third conditionals are to skip transitions between 0 weighted frames if(tnexts[i]!=invalid_move && (getKeyFrame(tprevs[i]).cmd.weight!=0 || getKeyFrame(tnexts[i]).cmd.weight!=0) && getKeyFrame(tprevs[i]).starttime==t) { float dv=fabs(getKeyFrame(tprevs[i]).cmd.value-getKeyFrame(tnexts[i]).cmd.value); unsigned int dt=getKeyFrame(tnexts[i]).starttime-getKeyFrame(tprevs[i]).starttime; if(dv/dt>comps[i]) { unsigned int delay=(unsigned int)(dv/comps[i])-dt; for(unsigned int j=0; j<NumOutputs; j++) for(Move_idx_t c=tnexts[j]; c!=invalid_move; c=getKeyFrame(c).next) getKeyFrame(c).starttime+=delay; } } } t=setNextFrameTime(tprevs,tnexts); } } bool MotionSequenceEngine::isPlaying() { return playing && ((playspeed>0) ? (playtime<=endtime) : (playtime>0)); } void MotionSequenceEngine::play() { if(playspeed>0) setTime(0); else setTime(endtime); resume(); } void MotionSequenceEngine::resume() { playing=true; lasttime=get_time(); WorldState * st=WorldState::getCurrent(); for(unsigned int i=0; i<NumOutputs; i++) { Move_idx_t cur=starts[i]; while(cur!=invalid_move) { if(getKeyFrame(cur).cmd.weight!=0) { getKeyFrame(starts[i]).cmd.value=st->outputs[i]; break; } cur=getKeyFrame(cur).next; } } } unsigned int MotionSequenceEngine::setNextFrameTime(Move_idx_t p[NumOutputs], Move_idx_t n[NumOutputs]) const { unsigned int ans=-1U; for(unsigned int i=0; i<NumOutputs; i++) if(n[i]!=invalid_move && getKeyFrame(n[i]).starttime<ans) ans=getKeyFrame(n[i]).starttime; if(ans!=-1U) { WorldState * st=WorldState::getCurrent(); for(unsigned int i=0; i<NumOutputs; i++) { if(setRange(ans,p[i],n[i])) { const OutputCmd& cmd=getKeyFrame((playspeed<0)?n[i]:p[i]).cmd; if(cmd.weight<=0) const_cast<OutputCmd&>(cmd).value=st->outputs[i]; //it's ok to assign the value when weight is '0', still "const"... kinda } } } return ans; } unsigned int MotionSequenceEngine::readWord(const char buf[], const char * const bufend, char wrd[], const unsigned int wordlen) { const char* origbuf=buf; wrd[0]='\0'; unsigned int i; //skip whitespace for(;buf<bufend && isspace(*buf) && *buf!='\n' && *buf!='\r';buf++) {} //store wrd for(i=0; buf<bufend && !isspace(*buf); buf++) if(i<wordlen-1) wrd[i++]=*buf; wrd[i]='\0'; if(buf>=bufend) return -1U; return buf-origbuf; } unsigned int MotionSequenceEngine::getOutputIndex(const char name[], unsigned int idx) { if(idx<NumOutputs) { unsigned int startidx=idx; for(;idx<NumOutputs;idx++) if(strcmp(name,outputNames[idx])==0) return idx; for(idx=0;idx<startidx;idx++) if(strcmp(name,outputNames[idx])==0) return idx; return NumOutputs; } else { for(idx=0;idx<NumOutputs;idx++) if(strcmp(name,outputNames[idx])==0) return idx; return idx; } } /*! @file * @brief Implements MotionSequenceEngine, abstract code for smoothly transitioning between a sequence of postures * @author ejt (Creator) * * $Author: ejt $ * $Name: tekkotsu-3_0 $ * $Revision: 1.15 $ * $State: Exp $ * $Date: 2006/09/23 19:51:11 $ */

Tekkotsu v3.0

Generated Wed Oct 4 00:03:44 2006 by Doxygen 1.4.7