Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

MotionManager.cc

Go to the documentation of this file.
00001 #include "MotionManager.h"
00002 #include "Shared/debuget.h"
00003 #include "Shared/WorldState.h"
00004 #include "Events/EventRouter.h"
00005 
00006 #include "Shared/ERS210Info.h"
00007 #include "Shared/ERS220Info.h"
00008 #include "Shared/ERS7Info.h"
00009 
00010 #include <list>
00011 
00012 MotionManager * motman=NULL;
00013 int MotionManager::_MMaccID=-1U;
00014 EventTranslator* MotionManager::etrans=NULL;
00015 
00016 const float MotionManager::kIgnoredPriority    =-1;
00017 const float MotionManager::kBackgroundPriority = 0;
00018 const float MotionManager::kLowPriority        = 5;
00019 const float MotionManager::kStdPriority        = 10;
00020 const float MotionManager::kHighPriority       = 50;
00021 const float MotionManager::kEmergencyPriority  = 100;
00022 
00023 using namespace std;
00024 
00025 //! just for convenience
00026 typedef unsigned int uint;
00027 
00028 MotionManager::MotionManager()
00029 : pidchanges(),cmdlist(),cur_cmd(invalid_MC_ID),MMlock(),numAcc(0)
00030 {
00031   for(uint x=0; x<NumOutputs; x++)
00032     cmdSums[x]=0;
00033 }
00034 
00035 #ifdef PLATFORM_APERIOS
00036 
00037 void
00038 MotionManager::InitAccess(OSubject* subj) {
00039   if(numAcc==MAX_ACCESS) {
00040     printf("*** ERROR *** attempted to register more accessors with MotionManager than allowed by MAX_ACCESS\n");
00041     return;
00042   }
00043   _MMaccID=numAcc++;
00044   //  cout << "ID is now " << _MMaccID << " of " << numAcc << endl;
00045   //  cout << "_MMaccID is " << &_MMaccID << endl;
00046   //  cout << "numAcc is " << &numAcc << " from " << this << endl;
00047   MMlock.lock(_MMaccID);
00048   //  accRegs[accID].init();
00049   subjs[_MMaccID]=subj;
00050   if(cmdlist.size()>0) //Shouldn't happen - busy wait in addMotion
00051     cout << "*** WARNING *** MOTIONS ADDED BEFORE ALL INITACCESSED" << endl;
00052   MMlock.unlock();
00053 }
00054 
00055 #else //now PLATFORM_LOCAL
00056 
00057 void
00058 MotionManager::InitAccess(MessageQueueBase& mcbufq) {
00059   if(numAcc==MAX_ACCESS) {
00060     printf("*** ERROR *** attempted to register more accessors with MotionManager than allowed by MAX_ACCESS\n");
00061     return;
00062   }
00063   _MMaccID=numAcc++;
00064   //  cout << "ID is now " << _MMaccID << " of " << numAcc << endl;
00065   //  cout << "_MMaccID is " << &_MMaccID << endl;
00066   //  cout << "numAcc is " << &numAcc << " from " << this << endl;
00067   MMlock.lock(_MMaccID);
00068   subjs[_MMaccID]=&mcbufq;
00069   if(cmdlist.size()>0) //Shouldn't happen - busy wait in doAddMotion
00070     cout << "*** WARNING *** MOTIONS ADDED BEFORE ALL INITACCESSED" << endl;
00071   MMlock.unlock();
00072 }
00073 
00074 void
00075 MotionManager::RemoveAccess() {
00076   func_begin();
00077   for(MC_ID mc_id=cmdlist.begin(); mc_id!=cmdlist.end(); mc_id=cmdlist.next(mc_id)) {
00078     if(cmdlist[mc_id].rcr[_MMaccID]!=NULL) {
00079       checkoutMotion(mc_id,true);
00080       cmdlist[mc_id].rcr[_MMaccID]->RemoveReference();
00081       cmdlist[mc_id].rcr[_MMaccID]=NULL;
00082       bool found=false;
00083       for(unsigned int i=0; i<numAcc; i++)
00084         if(cmdlist[mc_id].rcr[i]!=NULL) {
00085           found=true;
00086           break;
00087         }
00088       if(!found) {
00089         cout << "Warning: dropping motion command " << mc_id << ", was active at shutdown (leaked?)" << endl;
00090         push_free(mc_id);
00091         //should we check in?  The lock isn't technically valid anymore...
00092         //checkinMotion(mc_id);
00093       } else
00094         checkinMotion(mc_id);
00095     }
00096   }
00097   func_end();
00098 }
00099 
00100 #endif //PLATFORM-specific initialization
00101 
00102 MotionManager::~MotionManager() {
00103   if(!cmdlist.empty()) {
00104     func_begin();
00105     cout << "WARNING: MotionManager destruction with MotionCommands still attached." << endl;
00106     while(!cmdlist.empty()) {
00107       MC_ID mc_id=cmdlist.begin();
00108       for(unsigned int i=0; i<numAcc; i++)
00109         if(cmdlist[mc_id].rcr[i]!=NULL)
00110           cout << "MC " << mc_id << " was still referenced by InitAccess caller #" << _MMaccID << endl;
00111       push_free(mc_id);
00112     }
00113     func_end();
00114   }
00115 }
00116 
00117 void
00118 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd& cmd) {
00119   if(output >= NumOutputs)
00120     return;
00121   if(cmd.weight<=0)
00122     return;
00123 
00124   if(caller==NULL || caller->getID()!=cur_cmd)
00125     func_begin();
00126   if(cur_cmd==invalid_MC_ID) {
00127     cmdSums[output]=cmd.value;
00128   } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00129     cmdstatelist_t& curstatelist=cmdstates[output];
00130     cmdstatelist_t::index_t ent=curstatelist.begin();
00131     if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00132       curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,cmd));
00133     else
00134       for(unsigned int i=0; i<NumFrames; i++)
00135         curstatelist[ent].frames[i]=cmd;
00136   }
00137   if(caller==NULL || caller->getID()!=cur_cmd)
00138     func_end();
00139 }
00140 
00141 void
00142 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd& cmd, unsigned int frame) {
00143   if(output >= NumOutputs)
00144     return;
00145   if(cmd.weight<=0)
00146     return;
00147 
00148   if(caller==NULL || caller->getID()!=cur_cmd)
00149     func_begin();
00150   if(cur_cmd==invalid_MC_ID) {
00151     cmdSums[output]=cmd.value;
00152   } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00153     cmdstatelist_t& curstatelist=cmdstates[output];
00154     cmdstatelist_t::index_t ent=curstatelist.begin();
00155     if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00156       curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,cmd,frame));
00157     else
00158       curstatelist[ent].frames[frame]=cmd;
00159   }
00160   if(caller==NULL || caller->getID()!=cur_cmd)
00161     func_end();
00162 }
00163 
00164 void 
00165 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd ocmds[NumFrames]) {
00166   if(output >= NumOutputs)
00167     return;
00168   unsigned int hasWeight=NumFrames;
00169   for(unsigned int i=NumFrames-1; i!=0; i--)
00170     if(ocmds[i].weight>0) {
00171       hasWeight=i;
00172       break;
00173     }
00174   if(hasWeight==NumFrames)
00175     return;
00176   
00177 
00178   if(caller==NULL || caller->getID()!=cur_cmd)
00179     func_begin();
00180   if(cur_cmd==invalid_MC_ID) {
00181     cmdSums[output]=ocmds[hasWeight].value;
00182   } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00183     cmdstatelist_t& curstatelist=cmdstates[output];
00184     cmdstatelist_t::index_t ent=curstatelist.begin();
00185     if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00186       curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,ocmds));
00187     else
00188       for(unsigned int i=0; i<NumFrames; i++)
00189         curstatelist[ent].frames[i]=ocmds[i];
00190   }
00191   if(caller==NULL || caller->getID()!=cur_cmd)
00192     func_end();
00193 }
00194 
00195 void
00196 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputPID& pid) {
00197   if(output >= NumOutputs)
00198     return;
00199 
00200   if(caller==NULL || caller->getID()!=cur_cmd)
00201     func_begin();
00202   if(cur_cmd==invalid_MC_ID) {
00203     setPID(output,pid.pid);
00204   } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00205     cmdstatelist_t& curstatelist=cmdstates[output];
00206     cmdstatelist_t::index_t ent=curstatelist.begin();
00207     if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00208       curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,pid));
00209     else
00210       curstatelist[ent].pid=pid;
00211   }
00212   if(caller==NULL || caller->getID()!=cur_cmd)
00213     func_end();
00214 }
00215 
00216 void
00217 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd& cmd, const OutputPID& pid) {
00218   if(output >= NumOutputs)
00219     return;
00220 
00221   if(caller==NULL || caller->getID()!=cur_cmd)
00222     func_begin();
00223   if(cur_cmd==invalid_MC_ID) {
00224     if(cmd.weight>0)
00225       cmdSums[output]=cmd.value;
00226     setPID(output,pid.pid);
00227   } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00228     cmdstatelist_t& curstatelist=cmdstates[output];
00229     cmdstatelist_t::index_t ent=curstatelist.begin();
00230     if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00231       curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,cmd,pid));
00232     else {
00233       for(unsigned int i=0; i<NumFrames; i++)
00234         curstatelist[ent].frames[i]=cmd;
00235       curstatelist[ent].pid=pid;
00236     }
00237   }
00238   if(caller==NULL || caller->getID()!=cur_cmd)
00239     func_end();
00240 }
00241 
00242 void
00243 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd ocmds[NumFrames], const OutputPID& pid) {
00244   if(output >= NumOutputs)
00245     return;
00246 
00247   if(caller==NULL || caller->getID()!=cur_cmd)
00248     func_begin();
00249   if(cur_cmd==invalid_MC_ID) {
00250     if(ocmds[NumFrames-1].weight>0)
00251       cmdSums[output]=ocmds[NumFrames-1].value;
00252     setPID(output,pid.pid);
00253   } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00254     cmdstatelist_t& curstatelist=cmdstates[output];
00255     cmdstatelist_t::index_t ent=curstatelist.begin();
00256     if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00257       curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,ocmds,pid));
00258     else {
00259       for(unsigned int i=0; i<NumFrames; i++)
00260         curstatelist[ent].frames[i]=ocmds[i];
00261       curstatelist[ent].pid=pid;
00262     }
00263   }
00264   if(caller==NULL || caller->getID()!=cur_cmd)
00265     func_end();
00266 }
00267 
00268 /*! What's worse? A plethora of functions which are only called, and only useful at one place,
00269  *  or a big massive function which doesn't pollute the namespace?  This is the latter, for
00270  *  better or worse. */
00271 void
00272 MotionManager::getOutputs(float outputs[NumFrames][NumOutputs]) {
00273   //  if(begin(id)!=end())
00274   //if(state && state->buttons[LFrPawOffset]) cout << "getAngles..." << flush;
00275   if(state==NULL) {
00276     // we haven't gotten the WorldState memory region from Main yet, just set LEDs to a wierd pattern and leave
00277     for(uint f=0;f<NumFrames;f++)
00278       for(uint i=0; i<NumOutputs; i++)
00279         outputs[f][i]=0;
00280     for(uint f=0;f<NumFrames;f++)
00281       for(uint l=0; l<NumLEDs; l++)
00282         outputs[f][l]=l/(NumLEDs-1.0);
00283     //  if(begin(id)!=end())
00284     //if(state && state->buttons[LFrPawOffset]) cout << "getangles-nostate-done..." << flush;
00285     return;
00286   }
00287   func_begin();
00288   //  if(begin(id)!=end())
00289   //  cout << id << "..." << flush;
00290   //  cout << "CHECKOUT..." << flush;
00291   for(uint output=0; output<NumOutputs; output++)
00292     cmdstates[output].clear();
00293 
00294   // for each PID joint which is set to 0 power, set the background
00295   // position value to current sensed value this prevents jerking back
00296   // to the previous position when joint(s) are moved during 0 power,
00297   // and then power is turned back on. (power here is in the 0 pid
00298   // sense, the joints are still receiving power from the system -
00299   // that's a separate system call)
00300   // Note that we wouldn't want to do this all the time, because
00301   // miscalibration between the sensed position and target position
00302   // will cause joints to drift to the extremities of motion, in some
00303   // cases, very quickly, and in worse cases, colliding with other
00304   // joints
00305   for(uint output=0; output<NumPIDJoints; output++)
00306     if(state->pids[output][0]==0 && state->pids[output][1]==0 && state->pids[output][2]==0)
00307       cmdSums[output]=state->outputs[output];
00308 
00309   //  std::cout << "UPDATE..." << std::flush;
00310   std::list<MC_ID> unlocked;
00311   for(MC_ID it=begin(); it!=end(); it=next(it)) // check out all the MotionCommands (only one at a time tho)
00312     unlocked.push_back(it);
00313   unsigned int lastProcessed=get_time();
00314   while(unlocked.size()>0) { // keep cycling through all the locks we didn't get
00315     for(std::list<MC_ID>::iterator it=unlocked.begin(); it!=unlocked.end(); ) {
00316       MotionCommand* mc=checkoutMotion(*it,false);
00317       if(mc==NULL)
00318         it++; //we didn't get a lock, skip it (we'll keep looping until we get it)
00319       else {
00320         // we got a lock
00321         cur_cmd=*it;
00322         if(mc->shouldPrune()) {
00323           cout << "Removing expired " << *it << " (autoprune)" << endl;
00324           removeMotion(*it);
00325         } else
00326           mc->updateOutputs(); // the MotionCommand should make calls to setOutput from within here
00327         cur_cmd=invalid_MC_ID;
00328         checkinMotion(*it); // release lock, done with motion
00329         // remove id from list of unprocessed motioncommands
00330         std::list<MC_ID>::iterator rem=it++;
00331         unlocked.erase(rem);
00332         lastProcessed=get_time();
00333       }
00334     }
00335     if(get_time()-lastProcessed>FrameTime*NumFrames/2)
00336       break;
00337   }
00338   if(unlocked.size()>0) {
00339     cerr << "Warning: MotionManager was unable to obtain a lock on MCs: ";
00340     for(std::list<MC_ID>::iterator it=unlocked.begin(); it!=unlocked.end(); it++)
00341       cerr << *it << ' ';
00342     cerr << endl;
00343     cerr << (unlocked.size()>1?"They":"It") << " may have been left locked by a Behavior while it was busy." << endl;
00344     cerr << "Try reducing the scope of your MMAccessor or call checkinMotion sooner." << endl;
00345   }
00346 
00347   // sort the list of requested outputs based on priority
00348   // (insertion sort, data structure is linked list)
00349   for(uint output=0; output<NumOutputs; output++) {
00350     cmdstatelist_t& curstatelist=cmdstates[output];
00351     for(cmdstatelist_t::index_t bit=curstatelist.begin(); bit!=curstatelist.end(); bit=curstatelist.next(bit)) {
00352       MC_ID high_ent=bit;
00353       float high_p=cmdlist[curstatelist[high_ent].mcid].priority;
00354       for(cmdstatelist_t::index_t cit=curstatelist.next(bit); cit!=curstatelist.end(); cit=curstatelist.next(cit)) {
00355         float curp=cmdlist[curstatelist[cit].mcid].priority;
00356         if(curp>high_p) {
00357           high_p=curp;
00358           high_ent=cit;
00359         }
00360       }
00361       curstatelist.swap(bit,high_ent);
00362       /*if(curstatelist.countf()!=curstatelist.countb() || curstatelist.countf()!=curstatelist.size()) {
00363         cout << "LOST ONE! " << bit << ' ' << high_ent << endl;
00364         cout << curstatelist.countf() << ' ' << curstatelist.countb() << ' ' << curstatelist.size() << endl;
00365         }*/
00366       bit=high_ent;
00367     }
00368   }
00369 
00370   // now we've got, for each output, a list of requested values sorted by priority
00371   // summarize each output
00372   for(uint frame=0; frame<NumFrames; frame++)
00373     for(uint output=0; output<NumOutputs; output++) {
00374       cmdstatelist_t& curstatelist=cmdstates[output];
00375       float alpha=1;
00376       OutputCmd sumcmd;
00377       cmdstatelist_t::index_t ent=curstatelist.begin();
00378       while(ent!=curstatelist.end() && alpha>0) {
00379         OutputCmd curcmd;
00380         float curp=curstatelist[ent].priority;
00381         float curalpha=1; // curalpha is multiplicative sum of leftovers (weights between 0 and 1)
00382         for(;curstatelist[ent].priority==curp; ent=curstatelist.next(ent)) {
00383           //weighted average within priority level
00384           float curweight=curstatelist[ent].frames[frame].weight;
00385           ASSERT(curweight>=0,"negative output weights are illegal");
00386           if(curweight<0) { //negative weights are illegal
00387             cout << "weight=" << curweight << endl;
00388             curweight=0;
00389           }
00390           curcmd.value+=curstatelist[ent].frames[frame].value*curweight;
00391           curcmd.weight+=curweight;
00392           if(curweight<1)
00393             curalpha*=(1-curweight);
00394           else
00395             curalpha=0;
00396         }
00397         if(curcmd.weight>0) {
00398           //weighted average of priority levels
00399           sumcmd.value+=curcmd.value/curcmd.weight*(1-curalpha);
00400           sumcmd.weight+=(1-curalpha);
00401           alpha*=curalpha;
00402         }
00403       }
00404       if(sumcmd.weight>0) 
00405         outputs[frame][output]=sumcmd.value/sumcmd.weight;
00406       else //if zero weight, hold last value
00407         sumcmd.value=outputs[frame][output]=cmdSums[output];
00408       if(frame==NumFrames-1)
00409         cmds[output]=sumcmd;
00410     }
00411   
00412   for(uint output=0; output<NumOutputs; output++)
00413     cmdSums[output]=outputs[NumFrames-1][output];
00414         
00415   // now summarize each output's PID values (for those which use PID control)
00416   for(uint output=PIDJointOffset; output<PIDJointOffset+NumPIDJoints; output++) {
00417     cmdstatelist_t& curstatelist=cmdstates[output];
00418     float alpha=1;
00419     float sumpid[3];
00420     for(uint i=0; i<3; i++)
00421       sumpid[i]=0;
00422     float sumweight=0;
00423     cmdstatelist_t::index_t ent=curstatelist.begin();
00424     while(ent!=curstatelist.end() && alpha>0) {
00425       float tmppid[3];
00426       for(uint i=0; i<3; i++)
00427         tmppid[i]=0;
00428       float tmpweight=0;
00429       float curp=curstatelist[ent].priority;
00430       float curalpha=1; // curalpha is multiplicative sum of leftovers (weights between 0 and 1)
00431       for(;curstatelist[ent].priority==curp; ent=curstatelist.next(ent)) {
00432         //weighted average within priority level
00433         float curweight=curstatelist[ent].pid.weight;
00434         ASSERT(curweight>=0,"negative PID weights are illegal")
00435         if(curweight<0) //negative weights are illegal
00436           curweight=0;
00437         for(uint i=0; i<3; i++)
00438           tmppid[i]+=curstatelist[ent].pid.pid[i]*curweight;
00439         tmpweight+=curweight;
00440         if(curweight<1)
00441           curalpha*=(1-curweight);
00442         else
00443           curalpha=0;
00444       }
00445       if(tmpweight>0) {
00446         //weighted average of priority levels
00447         for(uint i=0; i<3; i++)
00448           sumpid[i]+=tmppid[i]/tmpweight*(1-curalpha);
00449         sumweight+=(1-curalpha);
00450         alpha*=curalpha;
00451       }
00452     }
00453     if(sumweight>0) {
00454       for(uint i=0; i<3; i++)
00455         sumpid[i]/=sumweight;
00456       setPID(output,sumpid);
00457     }
00458   }
00459 
00460   func_end();
00461   //  if(begin(id)!=end())
00462   //if(state && state->buttons[LFrPawOffset]) cout << "getAngles-done." << flush;
00463 }
00464 
00465 void
00466 MotionManager::updateWorldState() {
00467   //cout << "updateWorldState" << endl;
00468   for(uint output=LEDOffset; output<LEDOffset+NumLEDs; output++)
00469     state->outputs[output]=cmdSums[output];
00470   for(uint output=BinJointOffset; output<BinJointOffset+NumBinJoints; output++)
00471     state->outputs[output]=cmdSums[output];
00472 
00473   // these parts check to see if there are "fake" joints, and sets their "sensed" values
00474   // to be the current target value, just in case a behavior is waiting for a non-existant
00475   // non-existant joint to move to a certain position.
00476   if(state->robotDesign & WorldState::ERS210Mask) {
00477     for(uint output=0; output<NumPIDJoints; output++)
00478       if(!ERS210Info::IsRealERS210[output])
00479         state->outputs[output]=cmdSums[output];
00480   } else if(state->robotDesign & WorldState::ERS220Mask) {
00481     for(uint output=0; output<NumPIDJoints; output++)
00482       if(!ERS220Info::IsRealERS220[output])
00483         state->outputs[output]=cmdSums[output];
00484   } else if(state->robotDesign & WorldState::ERS7Mask) {
00485     for(uint output=0; output<NumPIDJoints; output++)
00486       if(!ERS7Info::IsRealERS7[output])
00487         state->outputs[output]=cmdSums[output];
00488   } else
00489     cout << "MotionManager::updateWorldState() - could not detect model" << endl;
00490 }
00491 
00492 #ifdef PLATFORM_APERIOS
00493 
00494 /*! This function handles the conversion from the Tekkotsu format (one
00495  *  regular IEEE float per parameter, to the OPEN-R format (which
00496  *  takes a specialized, reduced precision floating point number) This
00497  *  is all documented in PIDMC as well.
00498  *
00499  *  In order to send Tekkotsu's PIDs to the system, they are converted
00500  *  to the gain/shift format.  On the ERS-2xx, we could dynamically
00501  *  choose shift values to allow more precision in setting values.
00502  *
00503  *  With the ERS-7, all shifts are shared, so they must be set to a
00504  *  common set of values, defined by WorldState::DefaultPIDShifts.
00505  *  This limits the range of gains which can then be set.
00506  *
00507  *  Due to the mysterious warning which would occur with the 2xx,
00508  *  (AGRMSDriver::SetGain() : 0x0A IS USED FOR GAIN SHIFT VALUE.)  and
00509  *  since this seems to be the way things are going, all models now,
00510  *  by default, use global shift values (which can vary from model to
00511  *  model, just global for each model)
00512  *
00513  *  You can revert to the dynamic shift selection by commenting-in
00514  *  the noted code section below.
00515  *
00516  *  A final note: the OPENR::SetJointGain function seems to be
00517  *  a rather costly function call.  You should probably try to avoid
00518  *  setting PIDs at too high a frequency.
00519  */
00520 bool
00521 MotionManager::updatePIDs(OPrimitiveID primIDs[NumOutputs]) {
00522   //cout << "updatePIDs " << endl;
00523   bool dirty=!pidchanges.empty();
00524   while(!pidchanges.empty()) {
00525     float gain[3];
00526     word shift[3];
00527 
00528     //if you want to enforce the default shifts:
00529     for(uint i=0; i<3; i++) {
00530       shift[i]=DefaultPIDShifts[i];
00531       gain[i]=pidchanges.front().pids[i]*(1<<(0x10-shift[i]));
00532     }
00533 
00534     //if you want to allow shifts to move for better precision:
00535     // this is OK on 2xx, although it occasionally produces warnings like:
00536     // AGRMSDriver::SetGain() : 0x0A IS USED FOR GAIN SHIFT VALUE.
00537     // It still seems to work fine though.
00538     // HOWEVER, the ERS-7 is a different story.  Apparently ( https://openr.aibo.com/cgi-bin/openr/e_regi/im_trbbs.cgi?uid=general&df=bbs.dat&prm=TAN&pg=1&no=0893#0893 )
00539     // all joints share the same shift, and so there must be one
00540     // global setting enforced.  If this is the way things are heading,
00541     // might as well just have everyone use the first method instead.
00542     // but here's the more dynamic way just for posterity:
00543     /*
00544     for(uint i=0; i<3; i++) {
00545       gain[i]=pidchanges.front().pids[i]*2;
00546       shift[i]=0xF;
00547       while(shift[i]!=2 && (gain[i]!=(word)gain[i] || gain[i]<=1) && gain[i]<0x20) {
00548         gain[i]*=2;
00549         shift[i]--;
00550       }
00551     }
00552     */
00553 
00554     //some debugging output (pick your favorite joint - i was having trouble with the ERS-7 nod joint in particular)
00555     //if(pidchanges.front().joint==HeadOffset+NodOffset)
00556     //cout << (word)gain[0] << ' ' << shift[0] << "   " << (word)gain[1] << ' ' << shift[1] << "   " << (word)gain[2] << ' ' << shift[2] << endl;
00557     //cout << gain[0] << ' ' << shift[0] << "   " << gain[1] << ' ' << shift[1] << "   " << gain[2] << ' ' << shift[2] << endl;
00558 
00559     OPENR::SetJointGain(primIDs[pidchanges.front().joint],(word)gain[0],(word)gain[1],(word)gain[2],shift[0],shift[1],shift[2]);
00560     for(uint i=0; i<3; i++)
00561       state->pids[pidchanges.front().joint][i]=pidchanges.front().pids[i];
00562     pidchanges.pop_front();
00563   }
00564   return dirty;
00565 }
00566 
00567 void
00568 MotionManager::receivedMsg(const ONotifyEvent& event) {
00569   //  cout << "receivedMsg..." << flush;
00570   func_begin();
00571   //  cout << id << "..." << flush;
00572   //cout << "Received at " << get_time() << endl;
00573   for(int x=0; x<event.NumOfData(); x++)
00574     processMsg(event.RCData(x));
00575   //  cout << "receivedMsg-done" << endl;
00576   func_end();
00577 }
00578 
00579 #endif //PLATFORM_APERIOS or PLATFORM_LOCAL
00580 
00581 void
00582 MotionManager::processMsg(RCRegion * rcr) {
00583   //cout << "processMsg..." << flush;
00584   if(rcr==NULL) {
00585     cout << "WARNING: MotionManager::processMsg was given a NULL region" << endl;
00586     return;
00587   }
00588   func_begin();
00589   //  cout << id << "..." << flush;
00590   MotionManagerMsg * mminfo = reinterpret_cast<MotionManagerMsg*>(rcr->Base());
00591   MC_ID mc_id=mminfo->mc_id;
00592   switch(mminfo->type) {
00593     case MotionManagerMsg::addMotion: {
00594       //cout << "receiveMotion(): rcr->NumberOfReference()==" << rcr->NumberOfReference() << endl;
00595       rcr->AddReference();
00596       //cout << "receiveMotion()NOW: rcr->NumberOfReference()==" << rcr->NumberOfReference() << endl;
00597       cmdlist[mc_id].rcr[_MMaccID]=rcr;
00598       //should be able to do a nice dynamic cast instead of a static one
00599       // but it gives NULL for some reason - i blame having to do the fork trick
00600       //cout << "Adding mc_id=="<< mc_id << " (and dynamic_cast is still " << dynamic_cast<MotionCommand*>(mminfo) << ")" << endl;
00601       cmdlist[mc_id].baseaddrs[_MMaccID]=static_cast<MotionCommand*>(mminfo);
00602       cmdlist[mc_id].baseaddrs[_MMaccID]->DoStart();
00603       erouter->postEvent(new EventBase(EventBase::motmanEGID,mc_id,EventBase::activateETID,0));
00604     } break;
00605     case MotionManagerMsg::deleteMotion: {
00606       //cout << "deleteMotion(): cmdlist[mc_id].rcr[_MMaccID]->NumberOfReference()==" << cmdlist[mc_id].rcr[_MMaccID]->NumberOfReference() << endl;
00607       //cout << "deleting mc_id=="<<mc_id << endl;
00608       if(cmdlist[mc_id].rcr[_MMaccID]==NULL)
00609         cout << "WARNING: MotionManager attempted to delete a NULL motion! mc_id="<<mc_id<<" _MMaccID=" << _MMaccID << endl;
00610       else {
00611         checkoutMotion(mc_id,true);
00612         cmdlist[mc_id].baseaddrs[_MMaccID]->DoStop();
00613         erouter->postEvent(new EventBase(EventBase::motmanEGID,mc_id,EventBase::deactivateETID,0));
00614         cmdlist[mc_id].rcr[_MMaccID]->RemoveReference();
00615         cmdlist[mc_id].rcr[_MMaccID]=NULL;
00616         bool found=false;
00617         for(unsigned int i=0; i<numAcc; i++)
00618           if(cmdlist[mc_id].rcr[i]!=NULL) {
00619             found=true;
00620             break;
00621           }
00622         if(!found) {
00623           push_free(mc_id);
00624           //should we check in?  The lock isn't technically valid anymore...
00625           //checkinMotion(mc_id);
00626         } else {
00627           cout << "Warning: motion command " << mc_id << " still has attachments after delete message received" << endl;
00628           checkinMotion(mc_id);
00629         }
00630       }
00631       //cout << "deleteMotion()NOW: cmdlist[mc_id].rcr[_MMaccID]->NumberOfReference()==" << cmdlist[mc_id].rcr[_MMaccID]->NumberOfReference() << endl;
00632     } break;
00633     default:
00634       printf("*** WARNING *** unknown MotionManager msg type received\n");
00635   }
00636   //  cout << "processMsg-done" << endl;
00637   func_end();
00638 }
00639 
00640 
00641 /*! We have made this function protected because it's more readable if you
00642 *  call addPrunableMotion() or addPersistentMotion() instead... we decided
00643 *  requiring people to pass a true/false arguement wouldn't make it clear
00644 *  what that true/false was controlling. */
00645 MotionManager::MC_ID 
00646 MotionManager::doAddMotion(const SharedObjectBase& sm, bool autoprune, float priority) {
00647   MotionCommand * mc = dynamic_cast<MotionCommand*>(reinterpret_cast<MotionManagerMsg*>(sm.data()));
00648   if(mc==NULL) {
00649     cout << "MotionManager::addMotion() - SharedObject does not seem to hold a MotionCommand" << endl;
00650     return invalid_MC_ID;
00651   }
00652   mc->setAutoPrune(autoprune);
00653   //cout << "addMotion...";
00654   while(numAcc<MAX_ACCESS-1) { std::cout << "WAIT" << std::flush; } //Wait for everyone to register
00655   func_begin();
00656   //cout << cmdlist.size() << " exist..." << endl;
00657   //  cout << id << "..." << flush;
00658   for(MC_ID it=cmdlist.begin(); it!=cmdlist.end(); it=cmdlist.next(it)) {
00659     if(cmdlist[it].baseaddrs[_MMaccID]==mc) {
00660       cerr << "Warning: re-added motion command " << it << endl;
00661       return it;
00662     }
00663   }
00664   MC_ID mc_id = pop_free();
00665   if(mc_id==cmdlist.end()) {
00666     cout << "MotionManager::addMotion() - Out of room, could not add" << endl;
00667     return func_end(cmdlist.end());
00668   }
00669   cmdlist[mc_id].baseaddrs[_MMaccID]=mc;
00670   cmdlist[mc_id].rcr[_MMaccID]=sm.getRegion();
00671   //cout << "addMotion(): sm.getRegion()->NumberOfReference()==" << sm.getRegion()->NumberOfReference() << endl;
00672   cmdlist[mc_id].rcr[_MMaccID]->AddReference();
00673   //cout << "addMotion()NOW: sm.getRegion()->NumberOfReference()==" << sm.getRegion()->NumberOfReference() << endl;
00674   cmdlist[mc_id].lastAccessor=_MMaccID;
00675   cmdlist[mc_id].priority=priority;
00676   //cout << "setAdd(" << mc_id << ")" << endl;
00677   mc->setAdd(mc_id);
00678 #ifdef PLATFORM_APERIOS
00679   OStatus err;
00680   /*{
00681     unsigned int i=0;
00682     for(ObserverConstIterator it=subjs[_MMaccID]->begin();it!=subjs[_MMaccID]->end();it++) {
00683       cout << "RemainBuffer("<<i++<<")==" << subjs[_MMaccID]->RemainBuffer(*it) << endl;
00684     }
00685     ASSERT((int)i==subjs[_MMaccID]->NumberOfObservers(),"did I miss an observer?");
00686   }*/
00687   //cout << "Sent at " << get_time() << flush;
00688   err=subjs[_MMaccID]->SetData(sm.getRegion());
00689   ASSERT(err==oSUCCESS,"*** ERROR MotionManager: SetData returned " << err);
00690   //cout << "addMotion()afterSetData: sm.getRegion()->NumberOfReference()==" << sm.getRegion()->NumberOfReference() << endl;
00691   err=subjs[_MMaccID]->NotifyObservers();
00692   ASSERT(err==oSUCCESS,"*** ERROR MotionManager: NotifyObservers returned " << err);
00693 #else //PLATFORM_LOCAL
00694   subjs[_MMaccID]->sendMessage(sm.getRegion());
00695 #endif //PLATFORM check for IPC stuff
00696   //  cout << "addMotion-done" << endl;
00697   //cout << " - " << get_time() << endl;
00698   return func_end(mc_id);
00699 }
00700 
00701 MotionCommand *
00702 MotionManager::checkoutMotion(MC_ID mcid,bool block) {
00703   //cout << "checkout..." << flush;
00704   if(mcid>=MAX_MOTIONS) {
00705     cout << "*** WARNING *** " << _MMaccID << " tried to access invalid mcid " << mcid << endl;
00706     return NULL;
00707   }
00708   if(block)
00709     cmdlist[mcid].lock.lock(_MMaccID);
00710   else
00711     if(!cmdlist[mcid].lock.try_lock(_MMaccID))
00712       return NULL;
00713   if(cmdlist[mcid].lastAccessor==(accID_t)-1) {
00714     cout << "*** WARNING *** " << _MMaccID << " tried to access dead mcid " << mcid << endl;
00715     cmdlist[mcid].lock.unlock();
00716     return NULL;
00717   }
00718   //cout << "locked..." << endl;
00719   MotionCommand * base = cmdlist[mcid].baseaddrs[_MMaccID];
00720   //  cout << "base=" << base << "..." << flush;
00721   if(cmdlist[mcid].lastAccessor!=_MMaccID) {
00722     //cout << "converting from " << MCRegistrar::getRaw(base) << "..." << flush;
00723     //cout << "prev=" << accRegs[cmdlist[mcid].lastAccessor].getReg(base) << "..." << flush;
00724     //    accRegs[id].convert(base);
00725     //cout << "to=" << MCRegistrar::getRaw(base) << ", " << accRegs[cmdlist[mcid].lastAccessor].getReg(base) << endl;
00726     cmdlist[mcid].lastAccessor=_MMaccID;
00727   }
00728   base->setTranslator(etrans);
00729   //cout << "checkout-done..." << flush;
00730   return base;
00731 }
00732 
00733 void
00734 MotionManager::checkinMotion(MC_ID mcid) {
00735   cmdlist[mcid].baseaddrs[_MMaccID]->setTranslator(NULL);
00736   if(mcid!=invalid_MC_ID)
00737     cmdlist[mcid].lock.unlock();
00738 }
00739 
00740 void
00741 MotionManager::removeMotion(MC_ID mcid) {
00742   if(mcid==invalid_MC_ID)
00743     return;
00744   func_begin();
00745 #ifdef PLATFORM_APERIOS
00746   MotionManagerMsg dmsg;
00747   dmsg.setDelete(mcid);
00748   //cout << "Remove at " << get_time() << flush;
00749   subjs[_MMaccID]->SetData(&dmsg,sizeof(dmsg));
00750   subjs[_MMaccID]->NotifyObservers();
00751 #else //PLATFORM_LOCAL
00752   SharedObject<MotionManagerMsg> dmsg;
00753   dmsg->setDelete(mcid);
00754   //cout << "Remove at " << get_time() << flush;
00755   subjs[_MMaccID]->sendMessage(dmsg.getRegion());
00756 #endif //PLATFORM check for IPC stuff
00757    //cout << " - " << get_time() << endl;
00758    //cout << "removeMotion(): cmdlist[mcid].rcr[_MMaccID]->NumberOfReference()==" << cmdlist[mcid].rcr[_MMaccID]->NumberOfReference() << endl;
00759   if(cmdlist[mcid].rcr[_MMaccID]!=NULL) {
00760     cmdlist[mcid].rcr[_MMaccID]->RemoveReference();
00761     cmdlist[mcid].rcr[_MMaccID]=NULL;
00762   }
00763   //cout << "removeMotion()NOW: cmdlist[mcid].rcr[_MMaccID]->NumberOfReference()==" << cmdlist[mcid].rcr[_MMaccID]->NumberOfReference() << endl;
00764   func_end();
00765 }
00766 
00767 
00768 /*! Note that we don't actually set the PIDs in the system here, we just queue them up.
00769  *  PID changes seem to be an expensive operation, so may only want to clear the queue
00770  *  at some reduced rate (although that's not actually currently being done, it just
00771  *  could be) */
00772 void
00773 MotionManager::setPID(unsigned int joint, const float pids[3]) {
00774   func_begin();
00775 
00776   //see if there's already an update for this joint
00777   for(uint u = pidchanges.begin(); u!=pidchanges.end(); u=pidchanges.next(u)) {
00778     if(pidchanges[u].joint==joint) { //found it
00779       for(uint i=0; i<3; i++) {
00780         pidchanges[u].pids[i]=pids[i];
00781         if(pids[i]!=state->pids[joint][i]) { //see if we're setting back to current PID
00782           for(i++; i<3; i++) //we aren't, copy over the rest
00783             pidchanges[u].pids[i]=pids[i];
00784           func_end();
00785           return;
00786         }
00787       }
00788       //if it didn't return within the loop, no difference was found from current state
00789       //so just delete the update
00790       pidchanges.erase(u);
00791       func_end();
00792       return;
00793     }
00794   }
00795 
00796   //if we completed the for loop, we didn't find an update for the joint
00797   for(uint i=0; i<3; i++) //check to see if it's different from the current
00798     if(pids[i]!=state->pids[joint][i]) {
00799       PIDUpdate update(joint,pids); //it is different, insert a new update
00800       pidchanges.push_back(update);
00801       // or for debugging:
00802       //ListMemBuf<PIDUpdate,NumPIDJoints>::index_t it=pidchanges.push_back(update);
00803       //ASSERT(it!=pidchanges.end(),"MotionManager ran out of pidchanges entries!");
00804       break;
00805     }
00806   func_end();
00807 }
00808 
00809 
00810 MotionManager::MC_ID
00811 MotionManager::skip_ahead(MC_ID mcid) const {
00812   // this is in case a new motion has been added, but the current
00813   // process hasn't received its own copy yet, so should skip over them
00814   while(mcid!=cmdlist.end() && cmdlist[mcid].rcr[_MMaccID]==NULL)
00815     mcid=cmdlist.next(mcid);
00816   return mcid;
00817 }
00818 
00819 MotionManager::OutputState::OutputState()
00820   : priority(0),mcid(MotionManager::invalid_MC_ID), pid()
00821 {}
00822 MotionManager::OutputState::OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd cmds[NumFrames])
00823   : priority(pri),mcid(mc), pid(DefaultPIDs[out])
00824 {
00825   for(unsigned int i=0; i<NumFrames; i++)
00826     frames[i]=cmds[i];
00827 }
00828 MotionManager::OutputState::OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd)
00829   : priority(pri),mcid(mc), pid(DefaultPIDs[out])
00830 {
00831   for(unsigned int i=0; i<NumFrames; i++)
00832     frames[i]=cmd;
00833 }
00834 MotionManager::OutputState::OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd, unsigned int frame)
00835   : priority(pri),mcid(mc), pid(DefaultPIDs[out])
00836 {
00837   frames[frame]=cmd;
00838 }
00839 MotionManager::OutputState::OutputState(unsigned int /*out*/, float pri, MC_ID mc, const OutputPID& p)
00840   : priority(pri),mcid(mc), pid(p)
00841 {}
00842 MotionManager::OutputState::OutputState(unsigned int /*out*/, float pri, MC_ID mc, const OutputCmd cmds[NumFrames], const OutputPID& p)
00843   : priority(pri),mcid(mc), pid(p)
00844 {
00845   for(unsigned int i=0; i<NumFrames; i++)
00846     frames[i]=cmds[i];
00847 }
00848 MotionManager::OutputState::OutputState(unsigned int /*out*/, float pri, MC_ID mc, const OutputCmd& cmd, const OutputPID& p)
00849   : priority(pri),mcid(mc), pid(p)
00850 {
00851   for(unsigned int i=0; i<NumFrames; i++)
00852     frames[i]=cmd;
00853 }
00854 
00855 
00856 /*! @file
00857  * @brief Implements MotionManager, simplifies sharing of MotionCommand's and provides mutual exclusion to their access
00858  * @author ejt (Creator)
00859  *
00860  * $Author: ejt $
00861  * $Name: tekkotsu-2_4_1 $
00862  * $Revision: 1.43 $
00863  * $State: Exp $
00864  * $Date: 2005/06/29 22:04:51 $
00865  */
00866 
00867 
00868 /*
00869     for(uint f=0;f<NumFrames;f++)
00870       for(uint i=0; i<NumOutputs; i++)
00871         outputs[f][i]=0;
00872     const uint cyctime=128;
00873     uint ot=get_time()+3*cyctime;
00874     for(uint f=0;f<NumFrames;f++) {
00875       uint t=ot+f*FrameTime;
00876       outputs[f][TopBrLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
00877       t-=cyctime;
00878       outputs[f][TopLLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
00879       outputs[f][TopRLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
00880       t-=cyctime;
00881       outputs[f][MidLLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
00882       outputs[f][MidRLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
00883       t-=cyctime;
00884       outputs[f][BotLLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
00885       outputs[f][BotRLEDOffset]=(t/(double)cyctime-t/cyctime)*.75+.25;
00886     }
00887 */
00888 
00889   /*  for(uint output=TlRedLEDOffset-1; output<LEDOffset+NumLEDs-1; output++) {
00890     cmdstatelist_t& curstatelist=cmdstates[output];
00891     cout << "Out " << output << ": ";
00892     for(cmdstatelist_t::index_t bit=curstatelist.begin(); bit!=curstatelist.end(); bit=curstatelist.next(bit))
00893       cout << '('<<curstatelist[bit].mcid<<','<<cmdlist[curstatelist[bit].mcid].priority<<','<<curstatelist[bit].frames[0].value<<','<<curstatelist[bit].frames[0].weight<<") ";
00894     cout << endl;
00895     }*/
00896 
00897 
00898   /*  cout << get_time() << ' ' << size() << endl;
00899   for(uint output=TlRedLEDOffset; output<LEDOffset+NumLEDs-1; output++) {
00900     cmdstatelist_t& curstatelist=cmdstates[output];
00901     cout << "Out " << output << ": ";
00902     for(cmdstatelist_t::index_t bit=curstatelist.begin(); bit!=curstatelist.end(); bit=curstatelist.next(bit))
00903       cout << '('<<curstatelist[bit].mcid<<','<<cmdlist[curstatelist[bit].mcid].priority<<','<<curstatelist[bit].frames[0].value<<','<<curstatelist[bit].frames[0].weight<<") ";
00904     cout << endl;
00905   }
00906   */

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:47 2005 by Doxygen 1.4.4